Arm sim, MyloggedRobot (#21)

added arm sim, got around issue w/ advatagekit not detecting wpilib by changing a file that probably shouldn't be changed.
FRC3636 · Dec 5, 2024 · e6d9e1a · e6d9e1a
2 parents 1a53e98 + ec0a0d9
commit e6d9e1a
Show file tree

Hide file tree

Showing 9 changed files with 237 additions and 66 deletions.
diff --git a/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt b/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt
@@ -1,5 +1,6 @@
 package com.frcteam3636.frc2024
 
+import com.ctre.phoenix6.hardware.Pigeon2
 import com.ctre.phoenix6.hardware.TalonFX
 import com.revrobotics.CANSparkFlex
 import com.revrobotics.CANSparkLowLevel
@@ -29,13 +30,15 @@ fun CANSparkMax(id: REVMotorControllerId, type: CANSparkLowLevel.MotorType) =
 fun CANSparkFlex(id: REVMotorControllerId, type: CANSparkLowLevel.MotorType) =
     CANSparkFlex(id.num, type)
 
-enum class CTREMotorControllerId(val num: Int, val bus: String) {
+enum class CTREDeviceId(val num: Int, val bus: String) {
     FrontLeftDrivingMotor(1, "*"),
     BackLeftDrivingMotor(2, "*"),
     BackRightDrivingMotor(3, "*"),
     FrontRightDrivingMotor(4, "*"),
     RightArmMotor(10, "*"),
     LeftArmMotor(11, "*"),
+    PigeonGyro(20, "*"),
 }
 
-fun TalonFX(id: CTREMotorControllerId) = TalonFX(id.num, id.bus)
+fun TalonFX(id: CTREDeviceId) = TalonFX(id.num, id.bus)
+fun Pigeon2(id: CTREDeviceId) = Pigeon2(id.num, id.bus)
diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt
@@ -23,8 +23,8 @@ 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
 import org.littletonrobotics.junction.Logger
+import org.littletonrobotics.junction.PatchedLoggedRobot
 import org.littletonrobotics.junction.networktables.NT4Publisher
 import org.littletonrobotics.junction.wpilog.WPILOGReader
 import org.littletonrobotics.junction.wpilog.WPILOGWriter
@@ -41,7 +41,7 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter
  * update the `Main.kt` file in the project. (If you use the IDE's Rename or Move refactorings when
  * renaming the object or package, it will get changed everywhere.)
  */
-object Robot : LoggedRobot() {
+object Robot : PatchedLoggedRobot() {
     private val controller = CommandXboxController(2)
     private val joystickLeft = Joystick(0)
     private val joystickRight = Joystick(1)
@@ -124,6 +124,7 @@ object Robot : LoggedRobot() {
         Drivetrain.register()
         Indexer.register()
         Intake.register()
+        Arm.register()
     }
 
     /** Expose commands for autonomous routines to use and display an auto picker in Shuffleboard. */

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt
@@ -2,16 +2,25 @@ package com.frcteam3636.frc2024.subsystems.arm
 
 import com.ctre.phoenix6.SignalLogger
 import com.frcteam3636.frc2024.Robot
+import com.frcteam3636.frc2024.subsystems.intake.Intake
+import com.frcteam3636.frc2024.subsystems.intake.Intake.indexerAngleLigament
+import com.frcteam3636.frc2024.subsystems.intake.Intake.intakeAngleLigament
 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.wpilibj.Timer
+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.Subsystem
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism
 import org.littletonrobotics.junction.Logger
+import kotlin.math.cos
+import kotlin.math.sin
 
 
 private const val SECONDS_BETWEEN_ARM_UPDATES = 0.5
@@ -23,7 +32,17 @@ object Arm : Subsystem {
         Robot.Model.PROTOTYPE -> ArmIOPrototype()
     }
 
-    var inputs = ArmIO.ArmInputs()
+    var inputs = LoggedArmInputs()
+    var mechanism = Mechanism2d(200.0, 200.0)
+    var armAngleLigament = MechanismLigament2d("Arm Ligament", 50.0, 90.0, 5.0, Color8Bit(Color.kGreen))
+    var armWristAngleLigament = MechanismLigament2d("Arm Wrist Ligament", 20.0, 90.0, 5.0, Color8Bit(Color.kGreen))
+
+    init {
+        mechanism.getRoot("Arm", 100.0, 100.0).apply{
+            append(armAngleLigament)
+        }
+        armAngleLigament.append(armWristAngleLigament)
+    }
 
     private var sysID = SysIdRoutine(
         SysIdRoutine.Config(
@@ -41,9 +60,13 @@ object Arm : Subsystem {
     override fun periodic() {
         io.updateInputs(inputs)
         Logger.processInputs("/Arm", inputs)
+        armAngleLigament.angle = inputs.position.`in`(Degrees)
+        armWristAngleLigament.angle = 90.0 - inputs.position.`in`(Degrees)
+
         if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) && inputs.absoluteEncoderConnected){
                 io.updatePosition(inputs.absoluteEncoderPosition)
         }
+        Logger.recordOutput("/Arm/Mechanism", mechanism)
 
     }
 

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt
@@ -5,7 +5,7 @@ import com.ctre.phoenix6.controls.VoltageOut
 import com.ctre.phoenix6.signals.GravityTypeValue
 import com.ctre.phoenix6.signals.InvertedValue
 import com.ctre.phoenix6.signals.NeutralModeValue
-import com.frcteam3636.frc2024.CTREMotorControllerId
+import com.frcteam3636.frc2024.CTREDeviceId
 import com.frcteam3636.frc2024.Robot
 import com.frcteam3636.frc2024.TalonFX
 import com.frcteam3636.frc2024.utils.math.*
@@ -20,52 +20,26 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.Logger
 import org.littletonrobotics.junction.inputs.LoggableInputs
+import org.team9432.annotation.Logged
 
+@Logged
+open class ArmInputs {
+    var rightPosition = Radians.zero()!!
+    var leftPosition = Radians.zero()!!
+    var position = Radians.zero()!!
 
-interface ArmIO{
-        class ArmInputs : LoggableInputs {
-
-            var rightPosition = Radians.zero()!!
-            var leftPosition = Radians.zero()!!
-            var position = Radians.zero()!!
-
-            var absoluteEncoderPosition = Radians.zero()!!
-
-            var rightCurrent = Volts.zero()!!
-            var leftCurrent = Volts.zero()!!
-
-            var rightVelocity = RadiansPerSecond.zero()!!
-            var leftVelocity = RadiansPerSecond.zero()!!
-
-            var absoluteEncoderConnected = false
-
-
-            override fun toLog(table: LogTable) {
-                table.put("Right Arm Motor Position", rightPosition)
-                table.put("Left Arm Motor Position", leftPosition)
+    var absoluteEncoderPosition = Radians.zero()!!
 
-                table.put("Right Arm Motor Current", rightCurrent)
-                table.put("Left Arm Motor Current", leftCurrent)
+    var rightCurrent = Volts.zero()!!
+    var leftCurrent = Volts.zero()!!
 
-                table.put("Right Arm Motor Velocity", rightVelocity)
-                table.put("Left Arm Motor Velocity", leftVelocity)
+    var rightVelocity = RadiansPerSecond.zero()!!
+    var leftVelocity = RadiansPerSecond.zero()!!
 
-                table.put("Absolute Encoder Connected",absoluteEncoderConnected )
-            }
-
-            override fun fromLog(table: LogTable) {
-                rightPosition = table.get("Right Arm Motor Position", rightPosition)
-                leftPosition = table.get("Left Arm Motor Position", leftPosition)
-
-                rightCurrent = table.get("Right Arm Motor Current", rightCurrent)
-                leftCurrent = table.get("Left Arm Motor Current", leftCurrent)
-
-                rightVelocity = table.get("Right Arm Motor Velocity", rightVelocity)
-                leftVelocity = table.get("Left Arm Motor Velocity", leftVelocity)
+    var absoluteEncoderConnected = false
+}
 
-                absoluteEncoderConnected = table.get("Absolute Encoder Connected", absoluteEncoderConnected)
-            }
-        }
+interface ArmIO{
 
         fun updateInputs(inputs: ArmInputs)
 
@@ -79,13 +53,13 @@ interface ArmIO{
 
     class ArmIOReal: ArmIO{
 
-        private val leftMotor = TalonFX(CTREMotorControllerId.LeftArmMotor)
+        private val leftMotor = TalonFX(CTREDeviceId.LeftArmMotor)
 
-        private val rightMotor = TalonFX(CTREMotorControllerId.RightArmMotor)
+        private val rightMotor = TalonFX(CTREDeviceId.RightArmMotor)
 
         private val absoluteEncoder = DutyCycleEncoder(DigitalInput(8))
 
-        override fun updateInputs(inputs: ArmIO.ArmInputs) {
+        override fun updateInputs(inputs: ArmInputs) {
             inputs.position = Rotations.of(absoluteEncoder.absolutePosition)
             inputs.absoluteEncoderConnected = absoluteEncoder.isConnected
 
@@ -179,23 +153,25 @@ interface ArmIO{
         val armSim = SingleJointedArmSim(
             DCMotor.getKrakenX60(1),
             3.0,
-            0.0301907551,
+            0.244,
             0.331414,
             -0.142921,
             2.32814365359,
             true,
             -0.142921
         )
 
-        val pidController = PIDController(ArmIOReal.PID_GAINS)
+        // TODO: Once we know the PID gains, this should be added back.
+//        val pidController = PIDController(ArmIOReal.PID_GAINS)
         var setPoint = 0.0
 
-        override fun updateInputs(inputs: ArmIO.ArmInputs) {
+        override fun updateInputs(inputs: ArmInputs) {
             armSim.update(Robot.period)
             inputs.rightVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec)
             inputs.leftVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec)
-            val pidVoltage = pidController.calculate(inputs.position.`in`(Radians),setPoint)
-            armSim.setInputVoltage(pidVoltage)
+            inputs.position = Radians.of(armSim.angleRads)
+//            val pidVoltage = pidController.calculate(inputs.position.`in`(Radians),setPoint)
+//            armSim.setInputVoltage(pidVoltage)
         }
 
         override fun pivotToPosition(position: Measure<Angle>) {
@@ -204,6 +180,7 @@ interface ArmIO{
 
         override fun setVoltage(volts: Measure<Voltage>) {
             armSim.setInputVoltage(volts.`in`(Volts))
+            Logger.recordOutput("/Arm/OutVolt", volts)
         }
 
         override fun updatePosition(position: Measure<Angle>) {
@@ -213,7 +190,7 @@ interface ArmIO{
 
     class ArmIOPrototype: ArmIO {
         //placeholder
-        override fun updateInputs(inputs: ArmIO.ArmInputs) {
+        override fun updateInputs(inputs: ArmInputs) {
         }
 
         override fun pivotToPosition(position: Measure<Angle>) {
@@ -224,4 +201,4 @@ interface ArmIO{
 
         override fun updatePosition(position: Measure<Angle>) {
         }
-    }
+    }
diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt
@@ -1,6 +1,6 @@
 package com.frcteam3636.frc2024.subsystems.drivetrain
 
-import com.frcteam3636.frc2024.CTREMotorControllerId
+import com.frcteam3636.frc2024.CTREDeviceId
 import com.frcteam3636.frc2024.REVMotorControllerId
 import com.frcteam3636.frc2024.Robot
 import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain.Constants.BRAKE_POSITION
@@ -336,22 +336,22 @@ object Drivetrain : Subsystem, Sendable {
             PerCorner(
                 frontLeft =
                 Pair(
-                    CTREMotorControllerId.FrontLeftDrivingMotor,
+                    CTREDeviceId.FrontLeftDrivingMotor,
                     REVMotorControllerId.FrontLeftTurningMotor
                 ),
                 frontRight =
                 Pair(
-                    CTREMotorControllerId.FrontRightDrivingMotor,
+                    CTREDeviceId.FrontRightDrivingMotor,
                     REVMotorControllerId.FrontRightTurningMotor
                 ),
                 backRight =
                 Pair(
-                    CTREMotorControllerId.BackRightDrivingMotor,
+                    CTREDeviceId.BackRightDrivingMotor,
                     REVMotorControllerId.BackRightTurningMotor
                 ),
                 backLeft =
                 Pair(
-                    CTREMotorControllerId.BackLeftDrivingMotor,
+                    CTREDeviceId.BackLeftDrivingMotor,
                     REVMotorControllerId.BackLeftTurningMotor
                 ),
             )

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/DrivetrainIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/DrivetrainIO.kt
@@ -1,7 +1,8 @@
 package com.frcteam3636.frc2024.subsystems.drivetrain
 
+import com.frcteam3636.frc2024.CTREDeviceId
+import com.frcteam3636.frc2024.Pigeon2
 import com.frcteam3636.frc2024.utils.swerve.PerCorner
-import com.kauailabs.navx.frc.AHRS
 import edu.wpi.first.math.geometry.Rotation3d
 import edu.wpi.first.math.kinematics.SwerveModulePosition
 import edu.wpi.first.math.kinematics.SwerveModuleState
@@ -44,7 +45,7 @@ abstract class DrivetrainIO {
 
 /** Drivetrain I/O layer that uses real swerve modules along with a NavX gyro. */
 class DrivetrainIOReal(override val modules: PerCorner<out SwerveModule>) : DrivetrainIO() {
-    override val gyro = GyroNavX(AHRS())
+    override val gyro = GyroPigeon(Pigeon2(CTREDeviceId.PigeonGyro))
 
     companion object {
         fun fromKrakenSwerve() =

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Gyro.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Gyro.kt
@@ -1,5 +1,8 @@
 package com.frcteam3636.frc2024.subsystems.drivetrain
 
+import com.ctre.phoenix6.BaseStatusSignal
+import com.ctre.phoenix6.configs.Pigeon2Configuration
+import com.ctre.phoenix6.hardware.Pigeon2
 import com.frcteam3636.frc2024.Robot
 import com.frcteam3636.frc2024.utils.swerve.PerCorner
 import com.frcteam3636.frc2024.utils.swerve.translation2dPerSecond
@@ -27,20 +30,40 @@ class GyroNavX(private val ahrs: AHRS) : Gyro {
     private var offset: Rotation3d = Rotation3d()
 
     init {
-        Logger.recordOutput("Gyro/Offset", offset)
+        Logger.recordOutput("NavXGyro/Offset", offset)
     }
 
     override var rotation: Rotation3d
         get() = offset + ahrs.rotation3d
         set(goal) {
             offset = goal - ahrs.rotation3d
-            Logger.recordOutput("Gyro/Offset", offset)
+            Logger.recordOutput("NavXGyro/Offset", offset)
         }
 
     override val connected
         get() = ahrs.isConnected
 }
 
+class  GyroPigeon(private val pigeon: Pigeon2) : Gyro {
+
+    private var offset: Rotation3d = Rotation3d()
+
+    init {
+        Logger.recordOutput("PigeonGyro/Offset", offset)
+        BaseStatusSignal.setUpdateFrequencyForAll(100.0, pigeon.yaw, pigeon.pitch, pigeon.roll);
+    }
+
+    override var rotation: Rotation3d
+        get() = offset + pigeon.rotation3d
+        set(goal) {
+            offset = goal - pigeon.rotation3d
+            Logger.recordOutput("PigeonGyro/Offset", offset)
+        }
+
+    override val connected
+        get() = pigeon.yaw.status.isOK
+}
+
 class GyroSim(private val modules: PerCorner<SwerveModule>) : Gyro {
     override var rotation = Rotation3d()
     override val connected = true

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt
@@ -109,7 +109,7 @@ interface DrivingMotor {
     var velocity: Double
 }
 
-class DrivingTalon(id: CTREMotorControllerId) : DrivingMotor {
+class DrivingTalon(id: CTREDeviceId) : DrivingMotor {
 
     private val inner = TalonFX(id).apply {
         configurator.apply(Slot0Configs().apply {