From 1bf597254a27ae76e7cc7d3cb45c62019dc59be3 Mon Sep 17 00:00:00 2001 From: taimorioka Date: Wed, 20 Nov 2024 19:46:55 -0800 Subject: [PATCH] Added intake and index simulation --- .../java/com/frcteam3636/frc2024/Robot.kt | 35 +++++++++++++++---- .../frcteam3636/frc2024/subsystems/arm/Arm.kt | 1 - .../frc2024/subsystems/arm/ArmIO.kt | 2 +- .../frc2024/subsystems/indexer/Indexer.kt | 21 +++++++++++ .../frc2024/subsystems/indexer/IndexerIO.kt | 33 +++++++++-------- .../frc2024/subsystems/intake/Intake.kt | 22 ++++++++++++ .../frc2024/subsystems/intake/IntakeIO.kt | 11 ++++-- .../frc2024/subsystems/wrist/Wrist.kt | 1 - 8 files changed, 100 insertions(+), 26 deletions(-) diff --git a/src/main/java/com/frcteam3636/frc2024/Robot.kt b/src/main/java/com/frcteam3636/frc2024/Robot.kt index f00fe13..f4675b3 100644 --- a/src/main/java/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/java/com/frcteam3636/frc2024/Robot.kt @@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.button.CommandXboxController -import edu.wpi.first.wpilibj2.command.button.JoystickButton import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import org.littletonrobotics.junction.LogFileUtil import org.littletonrobotics.junction.LoggedRobot @@ -138,14 +137,14 @@ object Robot : LoggedRobot() { /** Configure which commands each joystick button triggers. */ private fun configureBindings() { - Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft, joystickRight) +// Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft, joystickRight) Indexer.defaultCommand = Indexer.autoRun() // (The button with the yellow tape on it) - JoystickButton(joystickLeft, 8).onTrue(Commands.runOnce({ - println("Zeroing gyro.") - Drivetrain.zeroGyro() - }).ignoringDisable(true)) +// JoystickButton(joystickLeft, 8).onTrue(Commands.runOnce({ +// println("Zeroing gyro.") +// Drivetrain.zeroGyro() +// }).ignoringDisable(true)) // //Intake // controller.rightBumper() @@ -154,6 +153,30 @@ object Robot : LoggedRobot() { // Intake.intake() // ) // + controller.a() + .debounce(0.150) + .whileTrue( + Intake.outtake() + ) + + controller.x() + .debounce(0.150) + .whileTrue( + Intake.intake() + ) + + controller.b() + .debounce(0.150) + .whileTrue( + Indexer.outtakeBalloon() + ) + + controller.y() + .debounce(0.150) + .whileTrue( + Indexer.indexBalloon() + ) + // //Outtake // controller.leftBumper() // .whileTrue( diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt index 7ef3197..f18d69c 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt @@ -5,7 +5,6 @@ import edu.wpi.first.units.Angle import edu.wpi.first.units.Measure import edu.wpi.first.units.Units.Degrees import edu.wpi.first.units.Units.Volts -import edu.wpi.first.units.Voltage import edu.wpi.first.wpilibj2.command.Subsystem import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index 5fe01fc..59e0bfa 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -80,7 +80,7 @@ interface ArmIO{ private val rightMotor = TalonFX(CTREMotorControllerId.RightArmMotor) - private val absoluteEncoder = DutyCycleEncoder(DigitalInput(7)) + private val absoluteEncoder = DutyCycleEncoder(DigitalInput(8)) override fun updateInputs(inputs: ArmIO.ArmInputs) { inputs.position = Rotations.of(absoluteEncoder.absolutePosition) diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt index 19f9a4d..8675494 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt @@ -1,7 +1,11 @@ package com.frcteam3636.frc2024.subsystems.indexer import com.frcteam3636.frc2024.Robot +import com.frcteam3636.frc2024.subsystems.intake.Intake +import edu.wpi.first.units.Units.Degrees import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.util.Color +import edu.wpi.first.wpilibj.util.Color8Bit import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem import org.littletonrobotics.junction.Logger @@ -18,6 +22,13 @@ object Indexer: Subsystem { override fun periodic() { io.updateInputs(inputs) Logger.processInputs("Indexer", inputs) + + Intake.indexerAngleLigament.angle = inputs.position.`in`(Degrees) + Intake.indexerAngleLigament.color = when (inputs.balloonState) { + BalloonState.Blue -> Color8Bit(Color.kBlue) + BalloonState.Red -> Color8Bit(Color.kRed) + else -> Color8Bit(Color.kGreen) + } } /** @@ -45,4 +56,14 @@ object Indexer: Subsystem { io.setSpinSpeed(0.0) } ) + + fun indexBalloon(): Command = runEnd( + {io.setSpinSpeed(0.5)}, + {io.setSpinSpeed(0.0)} + ) + + fun outtakeBalloon(): Command = runEnd( + {io.setSpinSpeed(-0.5)}, + {io.setSpinSpeed(0.0)} + ) } \ No newline at end of file diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt index e3a1c30..4ddac6f 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt @@ -1,19 +1,18 @@ package com.frcteam3636.frc2024.subsystems.indexer -import edu.wpi.first.math.geometry.Rotation2d import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs import com.frcteam3636.frc2024.CANSparkFlex import com.frcteam3636.frc2024.REVMotorControllerId import com.frcteam3636.frc2024.Robot import com.frcteam3636.frc2024.utils.LimelightHelpers +import com.frcteam3636.frc2024.utils.math.TAU import com.revrobotics.CANSparkLowLevel import edu.wpi.first.math.system.plant.DCMotor -import edu.wpi.first.units.Units.RadiansPerSecond -import edu.wpi.first.units.Units.RotationsPerSecond +import edu.wpi.first.units.Units.* import edu.wpi.first.wpilibj.simulation.FlywheelSim -public enum class BalloonState { +enum class BalloonState { Blue, Red, None @@ -22,19 +21,22 @@ public enum class BalloonState { interface IndexerIO { class Inputs : LoggableInputs { var indexerVelocity = RotationsPerSecond.zero() - var indexerCurrent: Double = 0.0 + var indexerCurrent = Amps.zero() var balloonState: BalloonState = BalloonState.None + var position = Radians.zero() override fun toLog(table: LogTable?) { table?.put("Indexer Wheel Velocity", indexerVelocity) table?.put("Indexer Wheel Current", indexerCurrent) table?.put("Balloon Color", balloonState) + table?.put("Indexer Wheel Angle", position) } override fun fromLog(table: LogTable) { - indexerVelocity = table.get("Indexer Velocity", indexerVelocity)!! + indexerVelocity = table.get("Indexer Velocity", indexerVelocity) indexerCurrent = table.get("Indexer Wheel Current", indexerCurrent) balloonState = table.get("Balloon Color", balloonState) + position = table.get("Indexer Wheel Angle", position) } } fun updateInputs(inputs: Inputs) @@ -45,9 +47,9 @@ interface IndexerIO { class IndexerIOReal : IndexerIO{ companion object Constants { - const val RED_CLASS = "red"; - const val BLUE_CLASS = "blue"; - const val NONE_CLASS = "none"; + const val RED_CLASS = "red" + const val BLUE_CLASS = "blue" + const val NONE_CLASS = "none" } private var indexerMotor = @@ -57,13 +59,14 @@ class IndexerIOReal : IndexerIO{ ) override fun updateInputs(inputs: IndexerIO.Inputs) { - inputs.indexerVelocity = RadiansPerSecond.of(indexerMotor.encoder.velocity) - inputs.indexerCurrent = indexerMotor.outputCurrent + inputs.indexerVelocity = Rotations.per(Minute).of(indexerMotor.encoder.velocity) + inputs.indexerCurrent = Amps.of(indexerMotor.outputCurrent) + inputs.position = Rotations.of(indexerMotor.encoder.position) when (val colorClass = LimelightHelpers.getClassifierClass("limelight-sensor")) { - RED_CLASS -> inputs.balloonState = BalloonState.Red; - BLUE_CLASS -> inputs.balloonState = BalloonState.Blue; - NONE_CLASS -> inputs.balloonState = BalloonState.None; + RED_CLASS -> inputs.balloonState = BalloonState.Red + BLUE_CLASS -> inputs.balloonState = BalloonState.Blue + NONE_CLASS -> inputs.balloonState = BalloonState.None else -> throw AssertionError("Unknown balloon class: $colorClass") } } @@ -84,6 +87,8 @@ class IndexerIOSim: IndexerIO { override fun updateInputs(inputs: IndexerIO.Inputs) { flywheelSim.update(Robot.period) inputs.indexerVelocity = RadiansPerSecond.of(flywheelSim.angularVelocityRadPerSec) + inputs.position += Radians.of(flywheelSim.angularVelocityRadPerSec * Robot.period) + inputs.position = Radians.of(inputs.position.`in`(Radians).mod(TAU)) } override fun setSpinSpeed(speed: Double) { diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/intake/Intake.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/intake/Intake.kt index 67ef5ca..2cad191 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/intake/Intake.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/intake/Intake.kt @@ -1,6 +1,11 @@ package com.frcteam3636.frc2024.subsystems.intake import com.frcteam3636.frc2024.Robot +import edu.wpi.first.units.Units.Degrees +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d +import edu.wpi.first.wpilibj.util.Color +import edu.wpi.first.wpilibj.util.Color8Bit import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem import org.littletonrobotics.junction.Logger @@ -14,8 +19,25 @@ object Intake: Subsystem { var inputs = IntakeIO.Inputs() + var mechanism = Mechanism2d(100.0, 200.0) + var intakeAngleLigament = MechanismLigament2d("Intake Ligament", 50.0, 90.0, 5.0, Color8Bit(Color.kGreen)) + var indexerAngleLigament = MechanismLigament2d("Indexer Ligament", 50.0, 90.0, 5.0, Color8Bit(Color.kGreen)) + + init { + mechanism.getRoot("Indexer", 50.0, 150.0).apply{ + append(indexerAngleLigament) + } + mechanism.getRoot("Intake", 50.0,50.0).apply{ + append(intakeAngleLigament) + } + } + override fun periodic() { + io.updateInputs(inputs) + Logger.processInputs("Intake", inputs) + intakeAngleLigament.angle = inputs.position.`in`(Degrees) + Logger.recordOutput("Intake Angle", mechanism) } fun outtake(): Command = diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/intake/IntakeIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/intake/IntakeIO.kt index 6b9b122..71aec8d 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/intake/IntakeIO.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/intake/IntakeIO.kt @@ -2,8 +2,8 @@ package com.frcteam3636.frc2024.subsystems.intake import com.frcteam3636.frc2024.CANSparkFlex import com.frcteam3636.frc2024.REVMotorControllerId import com.frcteam3636.frc2024.Robot +import com.frcteam3636.frc2024.utils.math.TAU import com.revrobotics.CANSparkLowLevel -import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.system.plant.DCMotor import edu.wpi.first.units.Units.* import edu.wpi.first.wpilibj.simulation.FlywheelSim @@ -15,22 +15,25 @@ interface IntakeIO { class Inputs : LoggableInputs { var rollerVelocity = RotationsPerSecond.zero() var current = Amps.zero() + var position = Radians.zero() override fun toLog(table: LogTable) { table.put("Intake Velocity", rollerVelocity) table.put("Intake Current", current) + table.put("Intake Roller Position", position) } override fun fromLog(table: LogTable) { rollerVelocity = table.get("Intake Velocity", rollerVelocity) current = table.get("Intake Current", current) + position = table.get("Intake Roller Position", position) } } fun setSpeed(percent: Double) - fun updateInputs(inputs: IntakeIO.Inputs) + fun updateInputs(inputs: Inputs) class IntakeIOReal: IntakeIO { private var motor = @@ -47,6 +50,7 @@ interface IntakeIO { override fun updateInputs(inputs: Inputs) { inputs.rollerVelocity = RotationsPerSecond.of(motor.encoder.velocity) inputs.current = Amps.of(motor.outputCurrent) + inputs.position = Rotations.of(motor.encoder.position.mod(1.0)) } } @@ -62,7 +66,8 @@ interface IntakeIO { motor.update(Robot.period) inputs.rollerVelocity = RadiansPerSecond.of(motor.angularVelocityRadPerSec) inputs.current = Amps.of(motor.currentDrawAmps) - + inputs.position += Radians.of(motor.angularVelocityRadPerSec * Robot.period) + inputs.position = Radians.of(inputs.position.`in`(Radians).mod(TAU)) } override fun setSpeed(percent: Double) { diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt index 80d8c38..f5c6584 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt @@ -1,7 +1,6 @@ package com.frcteam3636.frc2024.subsystems.wrist import com.frcteam3636.frc2024.subsystems.arm.Arm -import com.frcteam3636.frc2024.subsystems.intake.IntakeIO import edu.wpi.first.wpilibj2.command.Subsystem import org.littletonrobotics.junction.Logger