Merge pull request #7 from FRC3636/IndexerSim+IntakeSim

src/main/java/com/frcteam3636/frc2024/Robot.kt

@@ -148,12 +148,30 @@ object Robot : LoggedRobot() {
         }).ignoringDisable(true))
 
 //        //Intake
-//        controller.rightBumper()
+//        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

@@ -9,7 +9,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

@@ -81,7 +81,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

@@ -1,31 +1,40 @@
 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
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
 
 
 interface IntakeIO {
     class Inputs : LoggableInputs {
-        var rollerVelocity = Rotation2d()
-        var current: Double = 0.0
+        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)!![0]
+            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: Inputs)
+
     class IntakeIOReal: IntakeIO {
         private var motor =
             CANSparkFlex(
@@ -37,11 +46,33 @@ interface IntakeIO {
             assert(percent in -1.0..1.0)
             motor.set(percent)
         }
+
+        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))
+        }
     }
 
     class IntakeIOSim: IntakeIO {
+
+        var motor = FlywheelSim(
+            DCMotor.getNeoVortex(1),
+            1.0,
+            1.0
+        )
+
+        override fun updateInputs(inputs: Inputs) {
+            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) {
-            TODO("Not yet implemented")
+            assert(percent in -1.0..1.0)
+            motor.setInputVoltage(percent * 12)
         }
     }
 }

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