Remove leftflywheel motor and fix setPositon and Voltage functions to…

… not create tons of objects also added torque and duty cycle to all talon files.

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

@@ -29,6 +29,8 @@ interface FlywheelIO {
     var rightFlywheelStatorCurrent = 0.amps
     var rightFlywheelSupplyCurrent = 0.amps
     var rightFlywheelTemperature = 0.celsius
+    var rightFlywheelDutyCycle = 0.0.volts
+    var rightFlywheelTorque = 0.0
 
     var leftFlywheelVelocity = 0.0.rotations.perMinute
     var leftFlywheelAppliedVoltage = 0.volts

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

@@ -9,12 +9,12 @@ 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
 import com.team4099.robot2023.subsystems.falconspin.MotorChecker
 import com.team4099.robot2023.subsystems.falconspin.MotorCollection
+import com.team4099.robot2023.subsystems.wrist.WristIOTalon
 import org.team4099.lib.units.AngularVelocity
 import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.amps
@@ -28,24 +28,18 @@ import org.team4099.lib.units.derived.IntegralGain
 import org.team4099.lib.units.derived.ProportionalGain
 import org.team4099.lib.units.derived.Radian
 import org.team4099.lib.units.derived.Volt
+import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.volts
+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()
 
-  private val flywheelLeftSensor =
-    ctreAngularMechanismSensor(
-      flywheelLeftTalon,
-      FlywheelConstants.LEFT_GEAR_RATIO,
-      FlywheelConstants.VOLTAGE_COMPENSATION,
-    )
+ var velocityRequest = VelocityVoltage(-1337.degrees.perSecond, slot = 0, feedforward = -1337.volts)
 
   private val flywheelRightSensor =
     ctreAngularMechanismSensor(
@@ -54,36 +48,22 @@ object FlywheelIOTalon : FlywheelIO {
       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>
   var rightFlywheelTempSignal: StatusSignal<Double>
   var rightFlywheelDutyCycle: StatusSignal<Double>
+  var motorVoltage : StatusSignal<Double>
+  var motorTorque :StatusSignal<Double>
 
   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 =
       flywheelRightSensor.integralVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KI)
     flywheelRightConfiguration.Slot0.kD =
-      flywheelLeftSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KD)
+      flywheelRightSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KD)
     //
     // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF)
 
@@ -98,34 +78,20 @@ object FlywheelIOTalon : FlywheelIO {
       FlywheelConstants.RIGHT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes
     flywheelRightConfiguration.CurrentLimits.StatorCurrentLimitEnable = false
 
-    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.MotorOutput.NeutralMode = NeutralModeValue.Brake
-    flywheelLeftConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake
 
-    flywheelLeftTalon.configurator.apply(flywheelRightConfiguration)
-    flywheelRightTalon.configurator.apply(flywheelLeftConfiguration)
+    flywheelRightTalon.configurator.apply(flywheelRightConfiguration)
 
-    flywheelLeftTalon.setControl(Follower(Constants.Shooter.FLYWHEEL_RIGHT_MOTOR_ID, true))
+    flywheelRightTalon.setControl(Follower(Constants.Shooter.FLYWHEEL_RIGHT_MOTOR_ID, true))
 
     rightFlywheelStatorCurrentSignal = flywheelRightTalon.statorCurrent
     rightFlywheelSupplyCurrentSignal = flywheelRightTalon.supplyCurrent
     rightFlywheelTempSignal = flywheelRightTalon.deviceTemp
     rightFlywheelDutyCycle = flywheelRightTalon.dutyCycle
+    motorVoltage = flywheelRightTalon.motorVoltage
+    motorTorque =flywheelRightTalon.torqueCurrent
+
 
-    leftFlywheelStatorCurrentSignal = flywheelLeftTalon.statorCurrent
-    leftFlywheelSupplyCurrentSignal = flywheelLeftTalon.supplyCurrent
-    leftFlywheelTempSignal = flywheelLeftTalon.deviceTemp
-    leftFlywheelDutyCycle = flywheelLeftTalon.dutyCycle
 
     MotorChecker.add(
       "Shooter",
@@ -172,30 +138,30 @@ object FlywheelIOTalon : FlywheelIO {
     PIDConfig.kI = flywheelRightSensor.integralVelocityGainToRawUnits(kI)
     PIDConfig.kD = flywheelRightSensor.derivativeVelocityGainToRawUnits(kD)
 
-    flywheelLeftTalon.configurator.apply(PIDConfig)
+
     flywheelRightTalon.configurator.apply(PIDConfig)
   }
 
   override fun setFlywheelVoltage(voltage: ElectricalPotential) {
     flywheelRightTalon.setVoltage(voltage.inVolts)
   }
+
   override fun setFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {
-    flywheelRightTalon.setControl(
-      VelocityVoltage(velocity, slot = 0, feedforward = feedforward).velocityVoltagePhoenix6
-    )
+    FlywheelIOTalon.velocityRequest.setFeedforward(feedforward)
+    FlywheelIOTalon.velocityRequest.setVelocity(velocity)
+    FlywheelIOTalon.flywheelRightTalon.setControl(FlywheelIOTalon.velocityRequest.velocityVoltagePhoenix6)
   }
   override fun updateInputs(inputs: FlywheelIO.FlywheelIOInputs) {
     inputs.rightFlywheelVelocity = flywheelRightSensor.velocity
-    inputs.rightFlywheelAppliedVoltage = rightFlywheelDutyCycle.value.volts
+    inputs.rightFlywheelAppliedVoltage = motorVoltage.value.volts
     inputs.rightFlywheelStatorCurrent = rightFlywheelStatorCurrentSignal.value.amps
     inputs.rightFlywheelSupplyCurrent = rightFlywheelSupplyCurrentSignal.value.amps
     inputs.rightFlywheelTemperature = rightFlywheelTempSignal.value.celsius
+    //TODO fix unit for torque
+    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
   }
 
   override fun setFlywheelBrakeMode(brake: Boolean) {
@@ -206,7 +172,7 @@ object FlywheelIOTalon : FlywheelIO {
     } else {
       motorOutputConfig.NeutralMode = NeutralModeValue.Coast
     }
-    flywheelLeftTalon.configurator.apply(motorOutputConfig)
+
     flywheelRightTalon.configurator.apply(motorOutputConfig)
   }
 }

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

@@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.wrist
 
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.lib.phoenix6.PositionVoltage
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.WristConstants
 import com.team4099.robot2023.subsystems.superstructure.Request

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

@@ -24,12 +24,14 @@ import org.team4099.lib.units.perSecond
 interface WristIO {
   class WristIOInputs : LoggableInputs {
 
-    var wristPostion = 0.degrees
-    var wristVelocity = 0.radians.perSecond
-    var wristAppliedVoltage = 0.volts
-    var wristStatorCurrent = 0.amps
-    var wristSupplyCurrent = 0.amps
-    var wristTemperature = 0.celsius
+    var wristPostion = 0.0.degrees
+    var wristVelocity = 0.0.radians.perSecond
+    var wristAppliedVoltage = 0.0.volts
+    var wristDutyCycle = 0.0.volts
+    var wristTorque = 0.0
+    var wristStatorCurrent = 0.0.amps
+    var wristSupplyCurrent = 0.0.amps
+    var wristTemperature = 0.0.celsius
 
     var isSimulated = false
 

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

@@ -30,6 +30,7 @@ import org.team4099.lib.units.derived.IntegralGain
 import org.team4099.lib.units.derived.ProportionalGain
 import org.team4099.lib.units.derived.Radian
 import org.team4099.lib.units.derived.Volt
+import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegrees
 import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.volts
@@ -43,6 +44,8 @@ object WristIOTalon : WristIO {
   private val absoluteEncoder: CANcoder = CANcoder(Constants.WRIST.CANCODER_ID)
   private val absoluteEncoderConfiguration: MagnetSensorConfigs = MagnetSensorConfigs()
 
+  var positionRequest = PositionVoltage(-1337.degrees, slot = 0, feedforward = -1337.volts)
+
   private val wristSensor =
     ctreAngularMechanismSensor(
       wristTalon,
@@ -54,6 +57,8 @@ object WristIOTalon : WristIO {
   var supplyCurrentSignal: StatusSignal<Double>
   var tempSignal: StatusSignal<Double>
   var dutyCycle: StatusSignal<Double>
+  var motorVoltage : StatusSignal<Double>
+  var motorTorque :StatusSignal<Double>
   init {
     wristTalon.configurator.apply(TalonFXConfiguration())
 
@@ -98,6 +103,8 @@ object WristIOTalon : WristIO {
     supplyCurrentSignal = wristTalon.supplyCurrent
     tempSignal = wristTalon.deviceTemp
     dutyCycle = wristTalon.dutyCycle
+    motorVoltage = wristTalon.motorVoltage
+    motorTorque = wristTalon.torqueCurrent
 
     MotorChecker.add(
       "Wrist",
@@ -130,14 +137,17 @@ object WristIOTalon : WristIO {
   }
 
   override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) {
-    wristTalon.setControl(
-      PositionVoltage(position, slot = 0, feedforward = feedforward).positionVoltagePhoenix6
-    )
+    positionRequest.setFeedforward(feedforward)
+    positionRequest.setPosition(position)
+    wristTalon.setControl(positionRequest.positionVoltagePhoenix6)
   }
   override fun updateInputs(inputs: WristIO.WristIOInputs) {
     inputs.wristPostion = wristSensor.position
     inputs.wristVelocity = wristSensor.velocity
-    inputs.wristAppliedVoltage = dutyCycle.value.volts
+    //TODO fix unit for torque
+    inputs.wristTorque = motorTorque.value
+    inputs.wristAppliedVoltage = motorVoltage.value.volts
+    inputs.wristDutyCycle = dutyCycle.value.volts
     inputs.wristStatorCurrent = statorCurrentSignal.value.amps
     inputs.wristSupplyCurrent = supplyCurrentSignal.value.amps
     inputs.wristTemperature = tempSignal.value.celsius