fix: correct can ids, resolve command loop overruns

build.gradle.kts

@@ -75,6 +75,12 @@ deploy {
         roborio.artifacts {
             register<FRCJavaArtifact>("frcJava") {
                 jvmArgs.add("-ea") // Remove this flag during comp to disable asserts
+//                jvmArgs.add("-Dcom.sun.management.jmxremote=true")
+//                jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
+//                jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
+//                jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
+//                jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
+//                jvmArgs.add("-Djava.rmi.server.hostname=10.36.36.2") // Replace TE.AM with team number
                 setJarTask(tasks.jar)
                 dependsOn(tasks.assemble)
             }

src/main/kotlin/com/frcteam3636/frc2024/CAN.kt

@@ -19,9 +19,8 @@ enum class REVMotorControllerId(val num: Int) {
     BackRightDrivingMotor(3),
     FrontRightDrivingMotor(4),
 
+    IntakeMotor(12),
     IndexerMotor(13),
-
-    IntakeMotor(21),
 }
 
 fun CANSparkMax(id: REVMotorControllerId, type: CANSparkLowLevel.MotorType) =

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

@@ -67,7 +67,7 @@ object Robot : PatchedLoggedRobot() {
         )
 
         // Joysticks are likely to be missing in simulation, which usually isn't a problem.
-        DriverStation.silenceJoystickConnectionWarning(!isReal())
+        DriverStation.silenceJoystickConnectionWarning(true) // FIXME: re-add before comp
 
         configureAdvantageKit()
         configureSubsystems()

src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt

@@ -23,7 +23,7 @@ import kotlin.math.cos
 import kotlin.math.sin
 
 
-private const val SECONDS_BETWEEN_ARM_UPDATES = 0.5
+private const val SECONDS_BETWEEN_ARM_UPDATES = 2.0
 
 object Arm : Subsystem {
     private var io: ArmIO = when (Robot.model) {

src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt

@@ -39,7 +39,7 @@ open class ArmInputs {
     var absoluteEncoderConnected = false
 }
 
-interface ArmIO{
+interface ArmIO {
 
         fun updateInputs(inputs: ArmInputs)
 
@@ -48,157 +48,154 @@ interface ArmIO{
         fun setVoltage(volts: Measure<Voltage>)
 
         fun updatePosition(position: Measure<Angle>)
-    }
+}
 
 
-    class ArmIOReal: ArmIO{
+class ArmIOReal: ArmIO {
 
-        private val leftMotor = TalonFX(CTREDeviceId.LeftArmMotor)
+    private val leftMotor = TalonFX(CTREDeviceId.LeftArmMotor)
 
-        private val rightMotor = TalonFX(CTREDeviceId.RightArmMotor)
+    private val rightMotor = TalonFX(CTREDeviceId.RightArmMotor)
 
-        private val absoluteEncoder = DutyCycleEncoder(DigitalInput(8))
+    private val absoluteEncoder = DutyCycleEncoder(DigitalInput(0))
 
-        override fun updateInputs(inputs: ArmInputs) {
-            inputs.position = Rotations.of(absoluteEncoder.absolutePosition)
-            inputs.absoluteEncoderConnected = absoluteEncoder.isConnected
+    override fun updateInputs(inputs: ArmInputs) {
+        inputs.position = Rotations.of(absoluteEncoder.absolutePosition)
+        inputs.absoluteEncoderConnected = absoluteEncoder.isConnected
 
-            inputs.absoluteEncoderPosition = Rotations.of(absoluteEncoder.absolutePosition)
+        inputs.absoluteEncoderPosition = Rotations.of(absoluteEncoder.absolutePosition)
 
-            inputs.leftPosition = Rotations.of(leftMotor.position.value)
-            inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value)
+        inputs.leftPosition = Rotations.of(leftMotor.position.value)
+        inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value)
 
 
-            inputs.rightPosition = Rotations.of(rightMotor.position.value)
-            inputs.rightVelocity = RotationsPerSecond.of(rightMotor.position.value)
+        inputs.rightPosition = Rotations.of(rightMotor.position.value)
+        inputs.rightVelocity = RotationsPerSecond.of(rightMotor.position.value)
 
-            inputs.rightCurrent = Volts.of(leftMotor.motorVoltage.value)
+        inputs.rightCurrent = Volts.of(leftMotor.motorVoltage.value)
+    }
 
-            leftMotor.setPosition(inputs.absoluteEncoderPosition.`in`(Rotations))
-            rightMotor.setPosition(inputs.absoluteEncoderPosition.`in`(Rotations))
-        }
+    override fun updatePosition(position: Measure<Angle>) {
+        leftMotor.setPosition(position.`in`(Rotations))
+        rightMotor.setPosition(position.`in`(Rotations))
+    }
 
-        override fun updatePosition(position: Measure<Angle>) {
-            leftMotor.setPosition(position.`in`(Rotations))
-            rightMotor.setPosition(position.`in`(Rotations))
-        }
+    override fun pivotToPosition(position: Measure<Angle>) {
+        Logger.recordOutput("Shooter/Pivot/Position Setpoint", position)
 
-        override fun pivotToPosition(position: Measure<Angle>) {
-            Logger.recordOutput("Shooter/Pivot/Position Setpoint", position)
+        val control = MotionMagicTorqueCurrentFOC(0.0).apply {
+            Slot = 0
+            Position = position.`in`(Rotations)
+        }
+        leftMotor.setControl(control)
+        rightMotor.setControl(control)
 
-            val control = MotionMagicTorqueCurrentFOC(0.0).apply {
-                Slot = 0
-                Position = position.`in`(Rotations)
-            }
-            leftMotor.setControl(control)
-            rightMotor.setControl(control)
+    }
 
-        }
+    override fun setVoltage(volts: Measure<Voltage>) {
+        assert(volts in Volts.of(-12.0)..Volts.of(12.0))
+        val control = VoltageOut(volts.`in`(Volts))
+        leftMotor.setControl(control)
+        rightMotor.setControl(control)
+    }
 
-        override fun setVoltage(volts: Measure<Voltage>) {
-            assert(volts in Volts.of(-12.0)..Volts.of(12.0))
-            val control = VoltageOut(volts.`in`(Volts))
-            leftMotor.setControl(control)
-            rightMotor.setControl(control)
-        }
+    init {
+        val config = TalonFXConfiguration().apply {
+            MotorOutput.apply {
+                NeutralMode = NeutralModeValue.Brake
+            }
 
-        init {
-            val config = TalonFXConfiguration().apply {
-                MotorOutput.apply {
-                    NeutralMode = NeutralModeValue.Brake
-                }
-
-                Feedback.apply {
-                    SensorToMechanismRatio = GEAR_RATIO
-                    FeedbackRotorOffset = 0.0
-                }
-
-                Slot0.apply {
-                    pidGains = PID_GAINS
-                    motorFFGains = FF_GAINS
-                    GravityType = GravityTypeValue.Arm_Cosine
-                    kG = GRAVITY_GAIN
-                }
-
-                MotionMagic.apply {
-                    MotionMagicCruiseVelocity = PROFILE_VELOCITY
-                    MotionMagicAcceleration = PROFILE_ACCELERATION
-                    MotionMagicJerk = PROFILE_JERK
-                }
+            Feedback.apply {
+                SensorToMechanismRatio = GEAR_RATIO
+                FeedbackRotorOffset = 0.0
             }
 
-            config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive
-            leftMotor.configurator.apply(
-                config
-            )
+            Slot0.apply {
+                pidGains = PID_GAINS
+                motorFFGains = FF_GAINS
+                GravityType = GravityTypeValue.Arm_Cosine
+                kG = GRAVITY_GAIN
+            }
 
-            config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive
-            rightMotor.configurator.apply(config)
+            MotionMagic.apply {
+                MotionMagicCruiseVelocity = PROFILE_VELOCITY
+                MotionMagicAcceleration = PROFILE_ACCELERATION
+                MotionMagicJerk = PROFILE_JERK
+            }
         }
 
+        config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive
+        leftMotor.configurator.apply(
+            config
+        )
+
+        config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive
+        rightMotor.configurator.apply(config)
+    }
 
-        internal companion object Constants {
-            private const val GEAR_RATIO = 0.0
-            val PID_GAINS = PIDGains(120.0, 0.0, 100.0) //placeholders
-            val FF_GAINS = MotorFFGains(7.8, 0.0, 0.0) //placeholders
-            private const val GRAVITY_GAIN = 0.0
-            private const val PROFILE_ACCELERATION = 0.0
-            private const val PROFILE_JERK = 0.0
-            private const val PROFILE_VELOCITY = 0.0
-        }
 
+    internal companion object Constants {
+        private const val GEAR_RATIO = 0.0
+        val PID_GAINS = PIDGains(120.0, 0.0, 100.0) //placeholders
+        val FF_GAINS = MotorFFGains(7.8, 0.0, 0.0) //placeholders
+        private const val GRAVITY_GAIN = 0.0
+        private const val PROFILE_ACCELERATION = 0.0
+        private const val PROFILE_JERK = 0.0
+        private const val PROFILE_VELOCITY = 0.0
     }
 
-    class ArmIOSim : ArmIO {
-        val armSim = SingleJointedArmSim(
-            DCMotor.getKrakenX60(1),
-            3.0,
-            0.244,
-            0.331414,
-            -0.142921,
-            2.32814365359,
-            true,
-            -0.142921
-        )
+}
 
-        // TODO: Once we know the PID gains, this should be added back.
+class ArmIOSim : ArmIO {
+    val armSim = SingleJointedArmSim(
+        DCMotor.getKrakenX60(1),
+        3.0,
+        0.244,
+        0.331414,
+        -0.142921,
+        2.32814365359,
+        true,
+        -0.142921
+    )
+
+    // TODO: Once we know the PID gains, this should be added back.
 //        val pidController = PIDController(ArmIOReal.PID_GAINS)
-        var setPoint = 0.0
+    var setPoint = 0.0
 
-        override fun updateInputs(inputs: ArmInputs) {
-            armSim.update(Robot.period)
-            inputs.rightVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec)
-            inputs.leftVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec)
-            inputs.position = Radians.of(armSim.angleRads)
+    override fun updateInputs(inputs: ArmInputs) {
+        armSim.update(Robot.period)
+        inputs.rightVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec)
+        inputs.leftVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec)
+        inputs.position = Radians.of(armSim.angleRads)
 //            val pidVoltage = pidController.calculate(inputs.position.`in`(Radians),setPoint)
 //            armSim.setInputVoltage(pidVoltage)
-        }
+    }
 
-        override fun pivotToPosition(position: Measure<Angle>) {
-            setPoint = position.`in`(Radians)
-        }
+    override fun pivotToPosition(position: Measure<Angle>) {
+        setPoint = position.`in`(Radians)
+    }
 
-        override fun setVoltage(volts: Measure<Voltage>) {
-            armSim.setInputVoltage(volts.`in`(Volts))
-            Logger.recordOutput("/Arm/OutVolt", volts)
-        }
+    override fun setVoltage(volts: Measure<Voltage>) {
+        armSim.setInputVoltage(volts.`in`(Volts))
+        Logger.recordOutput("/Arm/OutVolt", volts)
+    }
 
-        override fun updatePosition(position: Measure<Angle>) {
-            // no drifting in sim so no need to update
-        }
+    override fun updatePosition(position: Measure<Angle>) {
+        // no drifting in sim so no need to update
     }
+}
 
-    class ArmIOPrototype: ArmIO {
-        //placeholder
-        override fun updateInputs(inputs: ArmInputs) {
-        }
+class ArmIOPrototype: ArmIO {
+    //placeholder
+    override fun updateInputs(inputs: ArmInputs) {
+    }
 
-        override fun pivotToPosition(position: Measure<Angle>) {
-        }
+    override fun pivotToPosition(position: Measure<Angle>) {
+    }
 
-        override fun setVoltage(volts: Measure<Voltage>) {
-        }
+    override fun setVoltage(volts: Measure<Voltage>) {
+    }
 
-        override fun updatePosition(position: Measure<Angle>) {
-        }
+    override fun updatePosition(position: Measure<Angle>) {
     }
+}

src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt

@@ -279,7 +279,7 @@ object Drivetrain : Subsystem, Sendable {
     internal object Constants {
         // Translation/rotation coefficient for teleoperated driver controls
         /** Unit: Percent of max robot speed */
-        const val TRANSLATION_SENSITIVITY = 0.1
+        const val TRANSLATION_SENSITIVITY = 1.0
 
         /** Unit: Rotations per second */
         const val ROTATION_SENSITIVITY = 0.4