Added intake and index simulation

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(

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

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)

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)}
+    )
 }

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) {

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 =

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) {

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