tuned wrist

src/main/kotlin/com/team4099/robot2023/BuildConstants.kt

@@ -6,10 +6,10 @@ package com.team4099.robot2023
 const val MAVEN_GROUP = ""
 const val MAVEN_NAME = "Crescendo-2024"
 const val VERSION = "unspecified"
-const val GIT_REVISION = 91
-const val GIT_SHA = "c0c4d2a27924aee8076363b091858c762200b7ca"
-const val GIT_DATE = "2024-01-25T20:05:57Z"
+const val GIT_REVISION = 96
+const val GIT_SHA = "2ac4df3605d67ef995dea36f9ccec757047a1ade"
+const val GIT_DATE = "2024-01-27T19:25:42Z"
 const val GIT_BRANCH = "shooter"
-const val BUILD_DATE = "2024-01-26T19:58:12Z"
-const val BUILD_UNIX_TIME = 1706317092819L
+const val BUILD_DATE = "2024-01-28T12:27:16Z"
+const val BUILD_UNIX_TIME = 1706462836611L
 const val DIRTY = 1

src/main/kotlin/com/team4099/robot2023/RobotContainer.kt

@@ -25,8 +25,6 @@ import org.team4099.lib.smoothDeadband
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
-import com.team4099.robot2023.commands.wrist.WristPositioningCommand
-import com.team4099.robot2023.commands.wrist.WristResetCommand
 
 object RobotContainer {
   private val drivetrain: Drivetrain
@@ -148,10 +146,9 @@ object RobotContainer {
     ControlBoard.openLoopFlywheel.whileTrue(flywheel.flywheelOpenLoopCommand())
 
     //ControlBoard.resetWrist.whileTrue(wrist.wristResetCommand())
-    ControlBoard.spinUpWrist.whileTrue(wrist.wristPositionCommand()) //matthew's pos cmd
     //ControlBoard.openLoopWrist.whileTrue(wrist.wristOpenLoopCommand())
-    ControlBoard.wristReset.whileTrue(WristResetCommand(wrist)) //matthew's reset cmd
-    ControlBoard.wristPID.whileTrue(WristPositioningCommand(wrist)) //cmd made thursday by nathan
+    ControlBoard.wristReset.whileTrue(wrist.wristResetCommand()) //matthew's reset cmd
+    ControlBoard.wristPID.whileTrue(wrist.wristPositionCommand()) //cmd made thursday by nathan
   }
 
   fun mapTestControls() {}

src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt

@@ -33,23 +33,23 @@ object WristConstants {
   val WRIST_VOLTAGE_COMPENSATION = 12.0.volts
   val WRIST_STATOR_CURRENT_LIMIT = 40.0.amps
 
-  val WRIST_MAX_ROTATION = 120.0.degrees
-  val WRIST_MIN_ROTATION = (-60.0).degrees
+  val WRIST_MAX_ROTATION = 45.degrees
+  val WRIST_MIN_ROTATION = (-47.0).degrees
 
-  val MAX_WRIST_VELOCITY = 1.0.radians.perSecond
-  val MAX_WRIST_ACCELERATION = 1.0.radians.perSecond.perSecond
+  val MAX_WRIST_VELOCITY = 1.radians.perSecond
+  val MAX_WRIST_ACCELERATION = 2.radians.perSecond.perSecond
   object PID {
     val REAL_KP: ProportionalGain<Radian, Volt> = 0.001.volts / 1.0.degrees
     val REAL_KI: IntegralGain<Radian, Volt> = 0.0.volts / (1.0.degrees * 1.0.seconds)
     val REAL_KD: DerivativeGain<Radian, Volt> = 0.0.volts / (1.0.rotations / 1.0.seconds)
 
-    val SIM_KP: ProportionalGain<Radian, Volt> = 0.091250.volts / 1.0.degrees
+    val SIM_KP: ProportionalGain<Radian, Volt> = 1.volts / 1.0.degrees
     val SIM_KI: IntegralGain<Radian, Volt> = 0.0.volts / (1.0.degrees * 1.0.seconds)
-    val SIM_KD: DerivativeGain<Radian, Volt> = 0.05.volts / (1.0.degrees / 1.0.seconds)
+    val SIM_KD: DerivativeGain<Radian, Volt> = 0.00.volts / (1.0.degrees / 1.0.seconds)
 
-    val WRIST_KG = 0.65.volts
-    val WRIST_KV = 1.61.volts / 1.0.degrees.perSecond
-    val WRIST_KA = 0.03.volts / 1.0.degrees.perSecond.perSecond
+    val WRIST_KG = 1.25.volts
+    val WRIST_KV = 1.61.volts / 1.0.radians.perSecond
+    val WRIST_KA = 0.03.volts / 1.0.radians.perSecond.perSecond
     val WRIST_KS = 0.0.volts
   }
 

src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt

@@ -9,6 +9,7 @@ import com.ctre.phoenix6.hardware.TalonFX
 import com.ctre.phoenix6.signals.NeutralModeValue
 import com.team4099.lib.phoenix6.VelocityVoltage
 import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.Constants.Shooter.FLYWHEEL_LEFT_MOTOR_ID
 import com.team4099.robot2023.config.constants.Constants.Shooter.FLYWHEEL_RIGHT_MOTOR_ID
 import com.team4099.robot2023.config.constants.FlywheelConstants
 import com.team4099.robot2023.subsystems.falconspin.Falcon500
@@ -35,19 +36,35 @@ import org.team4099.lib.units.perSecond
 
 object FlywheelIOTalon : FlywheelIO {
 
+  private val flywheelLeftTalon: TalonFX = TalonFX(FLYWHEEL_LEFT_MOTOR_ID)
+
   private val flywheelRightTalon: TalonFX = TalonFX(FLYWHEEL_RIGHT_MOTOR_ID)
 
+  private val flywheelLeftConfiguration: TalonFXConfiguration = TalonFXConfiguration()
+
   private val flywheelRightConfiguration: TalonFXConfiguration = TalonFXConfiguration()
 
  var velocityRequest = VelocityVoltage(-1337.degrees.perSecond, slot = 0, feedforward = -1337.volts)
 
+  private val flywheelLeftSensor =
+    ctreAngularMechanismSensor(
+      flywheelLeftTalon,
+      FlywheelConstants.LEFT_GEAR_RATIO,
+      FlywheelConstants.VOLTAGE_COMPENSATION,
+    )
+
   private val flywheelRightSensor =
     ctreAngularMechanismSensor(
       flywheelRightTalon,
       FlywheelConstants.RIGHT_GEAR_RATIO,
       FlywheelConstants.VOLTAGE_COMPENSATION,
     )
 
+  var leftFlywheelStatorCurrentSignal: StatusSignal<Double>
+  var leftFlywheelSupplyCurrentSignal: StatusSignal<Double>
+  var leftFlywheelTempSignal: StatusSignal<Double>
+  var leftFlywheelDutyCycle: StatusSignal<Double>
+
 
   var rightFlywheelStatorCurrentSignal: StatusSignal<Double>
   var rightFlywheelSupplyCurrentSignal: StatusSignal<Double>
@@ -58,6 +75,19 @@ object FlywheelIOTalon : FlywheelIO {
 
   init {
 
+    flywheelLeftTalon.configurator.apply(TalonFXConfiguration())
+    flywheelRightTalon.configurator.apply(TalonFXConfiguration())
+
+    flywheelLeftTalon.clearStickyFaults()
+    flywheelRightTalon.clearStickyFaults()
+
+    flywheelLeftConfiguration.Slot0.kP =
+      flywheelLeftSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KP)
+    flywheelLeftConfiguration.Slot0.kI =
+      flywheelLeftSensor.integralVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KI)
+    flywheelLeftConfiguration.Slot0.kD =
+      flywheelLeftSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KD)
+
     flywheelRightConfiguration.Slot0.kP =
       flywheelRightSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KP)
     flywheelRightConfiguration.Slot0.kI =
@@ -67,6 +97,17 @@ object FlywheelIOTalon : FlywheelIO {
     //
     // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF)
 
+    flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimit =
+      FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes
+    flywheelLeftConfiguration.CurrentLimits.SupplyCurrentThreshold =
+      FlywheelConstants.LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes
+    flywheelLeftConfiguration.CurrentLimits.SupplyTimeThreshold =
+      FlywheelConstants.LEFT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds
+    flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true
+    flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimit =
+      FlywheelConstants.LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes
+    flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimitEnable = false
+
     flywheelRightConfiguration.CurrentLimits.SupplyCurrentLimit =
       FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes
     flywheelRightConfiguration.CurrentLimits.SupplyCurrentThreshold =
@@ -78,11 +119,13 @@ object FlywheelIOTalon : FlywheelIO {
       FlywheelConstants.RIGHT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes
     flywheelRightConfiguration.CurrentLimits.StatorCurrentLimitEnable = false
 
+
+    flywheelLeftConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake
     flywheelRightConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake
 
     flywheelRightTalon.configurator.apply(flywheelRightConfiguration)
 
-    flywheelRightTalon.setControl(Follower(Constants.Shooter.FLYWHEEL_RIGHT_MOTOR_ID, true))
+    flywheelLeftTalon.setControl(Follower(Constants.Shooter.FLYWHEEL_RIGHT_MOTOR_ID, true))
 
     rightFlywheelStatorCurrentSignal = flywheelRightTalon.statorCurrent
     rightFlywheelSupplyCurrentSignal = flywheelRightTalon.supplyCurrent
@@ -91,6 +134,11 @@ object FlywheelIOTalon : FlywheelIO {
     motorVoltage = flywheelRightTalon.motorVoltage
     motorTorque =flywheelRightTalon.torqueCurrent
 
+    leftFlywheelStatorCurrentSignal = flywheelLeftTalon.statorCurrent
+    leftFlywheelSupplyCurrentSignal = flywheelLeftTalon.supplyCurrent
+    leftFlywheelTempSignal = flywheelLeftTalon.deviceTemp
+    leftFlywheelDutyCycle = flywheelLeftTalon.dutyCycle
+
 
 
     MotorChecker.add(
@@ -108,10 +156,10 @@ object FlywheelIOTalon : FlywheelIO {
       "Shooter",
       "Flywheel",
       MotorCollection(
-        mutableListOf(Falcon500(flywheelRightTalon, "Flywheel Right Motor")),
-        FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT,
+        mutableListOf(Falcon500(flywheelLeftTalon, "Flywheel Left Motor")),
+        FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT,
         90.celsius,
-        FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps,
+        FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps,
         110.celsius
       )
     )
@@ -138,7 +186,7 @@ object FlywheelIOTalon : FlywheelIO {
     PIDConfig.kI = flywheelRightSensor.integralVelocityGainToRawUnits(kI)
     PIDConfig.kD = flywheelRightSensor.derivativeVelocityGainToRawUnits(kD)
 
-
+    flywheelLeftTalon.configurator.apply(PIDConfig)
     flywheelRightTalon.configurator.apply(PIDConfig)
   }
 
@@ -161,6 +209,11 @@ object FlywheelIOTalon : FlywheelIO {
     inputs.rightFlywheelTorque = motorTorque.value
     inputs.rightFlywheelDutyCycle = rightFlywheelDutyCycle.value.volts
 
+    inputs.leftFlywheelVelocity = flywheelLeftSensor.velocity
+    inputs.leftFlywheelAppliedVoltage = leftFlywheelDutyCycle.value.volts
+    inputs.leftFlywheelStatorCurrent = leftFlywheelStatorCurrentSignal.value.amps
+    inputs.leftFlywheelSupplyCurrent = leftFlywheelSupplyCurrentSignal.value.amps
+    inputs.leftFlywheelTemperature = leftFlywheelTempSignal.value.celsius
 
   }
 

src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt

@@ -20,18 +20,26 @@ import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.Radian
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegrees
+import org.team4099.lib.units.derived.inRadians
 import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.inVoltsPerDegree
 import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond
 import org.team4099.lib.units.derived.inVoltsPerDegreePerSecondPerSecond
 import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds
+import org.team4099.lib.units.derived.inVoltsPerRadianPerSecond
+import org.team4099.lib.units.derived.inVoltsPerRadianPerSecondPerSecond
 import org.team4099.lib.units.derived.perDegree
 import org.team4099.lib.units.derived.perDegreePerSecond
 import org.team4099.lib.units.derived.perDegreePerSecondPerSecond
 import org.team4099.lib.units.derived.perDegreeSeconds
+import org.team4099.lib.units.derived.perRadian
+import org.team4099.lib.units.derived.perRadianPerSecond
+import org.team4099.lib.units.derived.perRadianPerSecondPerSecond
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.inDegreesPerSecond
+import org.team4099.lib.units.inRadiansPerSecond
+import org.team4099.lib.units.inRadiansPerSecondPerSecond
 import org.team4099.lib.units.perSecond
 
 class Wrist(val io: WristIO) : SubsystemBase() {
@@ -44,14 +52,12 @@ class Wrist(val io: WristIO) : SubsystemBase() {
   private val wristkV =
     LoggedTunableValue(
       "Wrist/kV",
-      WristConstants.PID.WRIST_KV,
-      Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond })
+      Pair({ it.inVoltsPerRadianPerSecond}, { it.volts.perRadianPerSecond})
     )
   private val wristkA =
     LoggedTunableValue(
       "Wrist/kA",
-      WristConstants.PID.WRIST_KA,
-      Pair({ it.inVoltsPerDegreePerSecondPerSecond }, { it.volts.perDegreePerSecondPerSecond })
+      Pair({ it.inVoltsPerRadianPerSecondPerSecond}, { it.volts.perRadianPerSecondPerSecond })
     )
   private val wristkG =
     LoggedTunableValue(
@@ -78,7 +84,7 @@ class Wrist(val io: WristIO) : SubsystemBase() {
           wristTargetVoltage = value.wristVoltage
         }
         is Request.WristRequest.TargetingPosition -> {
-          wristPositionTarget
+          wristPositionTarget = value.wristPosition
         }
         else -> {}
       }
@@ -129,6 +135,11 @@ class Wrist(val io: WristIO) : SubsystemBase() {
       wristkD.initDefault(WristConstants.PID.SIM_KD)
     }
 
+    wristkS.initDefault(WristConstants.PID.WRIST_KS)
+    wristkG.initDefault(WristConstants.PID.WRIST_KG)
+    wristkV.initDefault(WristConstants.PID.WRIST_KV)
+    wristkA.initDefault(WristConstants.PID.WRIST_KA)
+
     wristFeedForward =
       ArmFeedforward(
         WristConstants.PID.WRIST_KS,
@@ -148,8 +159,10 @@ class Wrist(val io: WristIO) : SubsystemBase() {
       wristkG.hasChanged() ||
       wristkS.hasChanged()
     ) {
+      print(wristkV.get().inVoltsPerRadianPerSecond)
       wristFeedForward = ArmFeedforward(wristkS.get(), wristkG.get(), wristkV.get(), wristkA.get())
     }
+
     Logger.processInputs("Wrist", inputs)
 
     Logger.recordOutput("Wrist/currentState", currentState.name)
@@ -248,16 +261,16 @@ class Wrist(val io: WristIO) : SubsystemBase() {
 
   fun wristPositionCommand(): Command {
     return Commands.runOnce({
-      currentRequest = Request.WristRequest.TargetingPosition(45.degrees)
+      currentRequest = Request.WristRequest.TargetingPosition(-15.degrees)
     })
   }
 
   fun wristOpenLoopCommand(): Command {
-    return Commands.runOnce({ currentRequest = Request.WristRequest.OpenLoop(0.volts) })
+    return Commands.runOnce({ currentRequest = Request.WristRequest.OpenLoop(10.volts) })
   }
 
   fun wristResetCommand(): Command {
-    return Commands.runOnce({ currentRequest = Request.WristRequest.OpenLoop(0.volts) })
+    return Commands.runOnce({ currentRequest = Request.WristRequest.OpenLoop(-10.volts) })
   }
 
   companion object {

src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt

@@ -92,7 +92,7 @@ object WristIOSim : WristIO {
     inputs.wristPostion = wristSim.angleRads.radians
     inputs.wristVelocity = wristSim.velocityRadPerSec.radians.perSecond
     inputs.wristSupplyCurrent = 0.amps
-    inputs.wristAppliedVoltage = 0.volts
+    inputs.wristAppliedVoltage = appliedVoltage
     inputs.wristStatorCurrent = wristSim.currentDrawAmps.amps
     inputs.wristTemperature = 0.0.celsius
 
@@ -105,16 +105,13 @@ object WristIOSim : WristIO {
    * @param voltage the voltage to set the roller motor to
    */
   override fun setWristVoltage(voltage: ElectricalPotential) {
-    val clampedVoltage = clamp(voltage, 5.volts, (WristConstants.VOLTAGE_COMPENSATION))
+    val clampedVoltage = clamp(voltage, -WristConstants.VOLTAGE_COMPENSATION, WristConstants.VOLTAGE_COMPENSATION)
     wristSim.setInputVoltage(clampedVoltage.inVolts)
-    println("stupid kotlin")
-    println(appliedVoltage)
-    println(voltage)
     appliedVoltage = clampedVoltage
   }
 
   override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) {
-    val feedback = wristController.calculate(wristSim.angleRads.radians * 180 / PI, position)
+    val feedback = wristController.calculate(wristSim.angleRads.radians, position)
     setWristVoltage(feedback + feedforward)
   }