Started changing flywheel to have two motors minor PID and feedforwar…

…d implmentation in wrist

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

@@ -12,42 +12,20 @@ import org.team4099.lib.units.perSecond
 
 object FlywheelConstants {
     val FLYWHEEEL_INIT_VOLTAGE = 0.0.volts
-    val FLYWHEEL_VOLTAGE_COMPENSATION = 12.volts
+    val RIGHT_FLYWHEEL_VOLTAGE_COMPENSATION = 12.volts
+    val LEFT_FLYWHEEL_VOLTAGE_COMPENSATION = 12.volts
     val ROLLER_GEAR_RATIO = 0.0
 
-    object PID {
 
-        val AUTO_POS_KP: ProportionalGain<Meter, Velocity<Meter>>
-            get() {
-                if (RobotBase.isReal()) {
-                    return 8.0.meters.perSecond / 1.0.meters
-                } else {
-                    return 7.0.meters.perSecond / 1.0.meters
-                }
-            }
-        val AUTO_POS_KI: IntegralGain<Meter, Velocity<Meter>>
-            get() {
-                if (RobotBase.isReal()) {
-                    return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds)
-                } else {
-                    return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds)
-                }
-            }
+    val RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT = 0.0.amps
+    val RIGHT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 0.0.amps
+    val RIGHT_flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds
+    val RIGHT_FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps
 
-        val AUTO_POS_KD: DerivativeGain<Meter, Velocity<Meter>>
-            get() {
-                if (RobotBase.isReal()) {
-                    return (0.05.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond
-                } else {
-                    return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond
-                }
-
-            }
-    }
-    val FLYWHEEL_SUPPLY_CURRENT_LIMIT = 0.0.amps
-    val FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 0.0.amps
-    val flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds
-    val FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps
+    val LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT = 0.0.amps
+    val LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 0.0.amps
+    val LEFT_flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds
+    val LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps
 
     val SHOOTER_FLYWHEEL_KP: ProportionalGain<Velocity<Radian>, Volt> =
         0.001.volts / 1.0.rotations.perMinute

src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt

@@ -72,7 +72,7 @@ init{
 
                 nextState = fromRequestToState(currentRequest)
             }
-            //TODO do sensor logic (mayb ask pranav)
+            //TODO do sensor logic (mayb ask p)
             Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{
                 if (flywheelTargetVoltage != lastFlywheelVoltage){
                     val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity)

src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt

@@ -18,92 +18,174 @@ import org.team4099.lib.units.base.*
 import org.team4099.lib.units.ctreAngularMechanismSensor
 import org.team4099.lib.units.derived.*
 
-class FlywheelIOFalcon (private val flywheelFalcon : TalonFX) : FlywheelIO{
+class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val flywheelLeftFalcon: TalonFX) : FlywheelIO{
 
-    private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration()
-    private val flywheelSensor =
+    private val flywheelRightConfiguration: TalonFXConfiguration = TalonFXConfiguration()
+    private val flywheelLeftConfiguration: TalonFXConfiguration = TalonFXConfiguration()
+
+    private val flywheelRightSensor =
         ctreAngularMechanismSensor(
-            flywheelFalcon,
+            flywheelRightFalcon,
             FlywheelConstants.ROLLER_GEAR_RATIO,
-            FlywheelConstants.FLYWHEEL_VOLTAGE_COMPENSATION,
+            FlywheelConstants.RIGHT_FLYWHEEL_VOLTAGE_COMPENSATION,
 
             )
-    var flywheelStatorCurrentSignal: StatusSignal<Double>
-    var flywheelSupplyCurrentSignal: StatusSignal<Double>
-    var flywheelTempSignal: StatusSignal<Double>
-    var flywheelDutyCycle : StatusSignal<Double>
+
+    private val flywheelLeftSensor =
+        ctreAngularMechanismSensor(
+            flywheelLeftFalcon,
+            FlywheelConstants.ROLLER_GEAR_RATIO,
+            FlywheelConstants.LEFT_FLYWHEEL_VOLTAGE_COMPENSATION,
+
+            )
+
+    var rightFlywheelStatorCurrentSignal: StatusSignal<Double>
+    var rightFlywheelSupplyCurrentSignal: StatusSignal<Double>
+    var rightFlywheelTempSignal: StatusSignal<Double>
+    var rightFlywheelDutyCycle : StatusSignal<Double>
+
+    var leftFlywheelStatorCurrentSignal: StatusSignal<Double>
+    var leftFlywheelSupplyCurrentSignal: StatusSignal<Double>
+    var leftFlywheelTempSignal: StatusSignal<Double>
+    var leftFlywheelDutyCycle : StatusSignal<Double>
     init {
-        flywheelFalcon.configurator.apply(TalonFXConfiguration())
-
-        flywheelFalcon.clearStickyFaults()
-        flywheelFalcon.configurator.apply(flywheelConfiguration)
-//TODO fix PID
-        flywheelConfiguration.Slot0.kP =
-            flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KP)
-        flywheelConfiguration.Slot0.kI =
-            flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KI)
-        flywheelConfiguration.Slot0.kD =
-            flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KD)
-        flywheelConfiguration.Slot0.kV = 0.05425
+        flywheelRightFalcon.configurator.apply(TalonFXConfiguration())
+        flywheelLeftFalcon.configurator.apply(TalonFXConfiguration())
+
+        flywheelRightFalcon.clearStickyFaults()
+        flywheelRightFalcon.configurator.apply(flywheelRightConfiguration)
+
+        flywheelLeftFalcon.clearStickyFaults()
+        flywheelLeftFalcon.configurator.apply(flywheelLeftConfiguration)
+
+        flywheelRightConfiguration.Slot0.kP =
+            flywheelRightSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KP)
+        flywheelRightConfiguration.Slot0.kI =
+            flywheelRightSensor.integralVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KI)
+        flywheelRightConfiguration.Slot0.kD =
+            flywheelRightSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KD)
+        flywheelRightConfiguration.Slot0.kV = 0.05425
+
+        flywheelLeftConfiguration.Slot0.kP =
+            flywheelLeftSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KP)
+        flywheelRightConfiguration.Slot0.kI =
+            flywheelLeftSensor.integralVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KI)
+        flywheelRightConfiguration.Slot0.kD =
+            flywheelLeftSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KD)
+        flywheelRightConfiguration.Slot0.kV = 0.05425
         //      flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF)
-        flywheelConfiguration.CurrentLimits.SupplyCurrentLimit =
-            FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes
-        flywheelConfiguration.CurrentLimits.SupplyCurrentThreshold =
-            FlywheelConstants.FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes
-        flywheelConfiguration.CurrentLimits.SupplyTimeThreshold =
-            FlywheelConstants.flywheel_TRIGGER_THRESHOLD_TIME.inSeconds
-        flywheelConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true
-        flywheelConfiguration.CurrentLimits.StatorCurrentLimit =
-            FlywheelConstants.FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes
-        flywheelConfiguration.CurrentLimits.StatorCurrentLimitEnable = false
-
-        flywheelConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake
-        flywheelFalcon.configurator.apply(flywheelConfiguration)
-
-        flywheelStatorCurrentSignal = flywheelFalcon.statorCurrent
-        flywheelSupplyCurrentSignal = flywheelFalcon.supplyCurrent
-        flywheelTempSignal = flywheelFalcon.deviceTemp
-        flywheelDutyCycle = flywheelFalcon.dutyCycle
+
+        flywheelRightConfiguration.CurrentLimits.SupplyCurrentLimit =
+            FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes
+        flywheelRightConfiguration.CurrentLimits.SupplyCurrentThreshold =
+            FlywheelConstants.RIGHT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes
+        flywheelRightConfiguration.CurrentLimits.SupplyTimeThreshold =
+            FlywheelConstants.RIGHT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds
+        flywheelRightConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true
+        flywheelRightConfiguration.CurrentLimits.StatorCurrentLimit =
+            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
+
+        flywheelLeftFalcon.configurator.apply(flywheelRightConfiguration)
+        flywheelRightFalcon.configurator.apply(flywheelLeftConfiguration)
+
+        rightFlywheelStatorCurrentSignal = flywheelRightFalcon.statorCurrent
+        rightFlywheelSupplyCurrentSignal = flywheelRightFalcon.supplyCurrent
+        rightFlywheelTempSignal = flywheelRightFalcon.deviceTemp
+        rightFlywheelDutyCycle = flywheelRightFalcon.dutyCycle
+
+        leftFlywheelStatorCurrentSignal = flywheelLeftFalcon.statorCurrent
+        leftFlywheelSupplyCurrentSignal = flywheelLeftFalcon.supplyCurrent
+        leftFlywheelTempSignal = flywheelLeftFalcon.deviceTemp
+        leftFlywheelDutyCycle = flywheelLeftFalcon.dutyCycle
 
         MotorChecker.add(
             "Shooter","Flywheel",
             MotorCollection(
-                mutableListOf(Falcon500(flywheelFalcon, "Flywheel Motor")),
-                FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT,
+                mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")),
+                FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT,
                 90.celsius,
-                FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps,
+                FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps,
+
+                110.celsius
+            )
+        )
+        MotorChecker.add(
+            "Shooter","Flywheel",
+            MotorCollection(
+                mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")),
+                FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT,
+                90.celsius,
+                FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps,
+
+                110.celsius
+            )
+        )
+        MotorChecker.add(
+            "Shooter","Flywheel",
+            MotorCollection(
+                mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")),
+                FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT,
+                90.celsius,
+                FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps,
+
                 110.celsius
             )
         )
     }
+    //TODO do the checks for pid and feedforward change
     override fun configPID(
-        kP: ProportionalGain<Velocity<Radian>, Volt>,
-        kI: IntegralGain<Velocity<Radian>, Volt>,
-        kD: DerivativeGain<Velocity<Radian>, Volt>
+        rightkP: ProportionalGain<Velocity<Radian>, Volt>,
+        rightkI: IntegralGain<Velocity<Radian>, Volt>,
+        rightkD: DerivativeGain<Velocity<Radian>, Volt>,
+        leftkP: ProportionalGain<Velocity<Radian>, Volt>,
+        leftkI: IntegralGain<Velocity<Radian>, Volt>,
+        leftkD: DerivativeGain<Velocity<Radian>, Volt>
     ) {
         val PIDConfig = Slot0Configs()
 
-        PIDConfig.kP = flywheelSensor.proportionalVelocityGainToRawUnits(kP)
-        PIDConfig.kI = flywheelSensor.integralVelocityGainToRawUnits(kI)
-        PIDConfig.kD = flywheelSensor.derivativeVelocityGainToRawUnits(kD)
+        PIDConfig.kP = flywheelRightSensor.proportionalVelocityGainToRawUnits(rightkP)
+        PIDConfig.kI = flywheelRightSensor.integralVelocityGainToRawUnits(rightkI)
+        PIDConfig.kD = flywheelRightSensor.derivativeVelocityGainToRawUnits(rightkD)
         PIDConfig.kV = 0.05425
 
-        flywheelFalcon.configurator.apply(PIDConfig)
+        PIDConfig.kP = flywheelLeftSensor.proportionalVelocityGainToRawUnits(leftkP)
+        PIDConfig.kI = flywheelLeftSensor.integralVelocityGainToRawUnits(leftkI)
+        PIDConfig.kD = flywheelLeftSensor.derivativeVelocityGainToRawUnits(leftkD)
+        PIDConfig.kV = 0.05425
+
+        flywheelRightFalcon.configurator.apply(PIDConfig)
+        flywheelLeftFalcon.configurator.apply(PIDConfig)
     }
     override fun setFlywheelVelocity(angularVelocity: AngularVelocity, feedforward: ElectricalPotential){
-        flywheelFalcon.setControl(0,
-            flywheelSensor.velocityToRawUnits(angularVelocity),
+        flywheelRightFalcon.setControl(0,
+            flywheelRightSensor.velocityToRawUnits(angularVelocity),
             DemandType.ArbitraryFeedForward,
             feedforward.inVolts
             )
     }
-
+//TODO create IO for the left falcon
     override fun updateInputs(inputs: FlywheelIO.FlywheelIOInputs) {
-        inputs.flywheelVelocity = flywheelSensor.velocity
-        inputs.flywheelAppliedVoltage = flywheelDutyCycle.value.volts
-        inputs.flywheelStatorCurrent = flywheelStatorCurrentSignal.value.amps
-        inputs.flywheelSupplyCurrent = flywheelSupplyCurrentSignal.value.amps
-        inputs.flywheelTempreature = flywheelTempSignal.value.celsius
+        inputs.flywheelVelocity = flywheelRightSensor.velocity
+        inputs.flywheelAppliedVoltage = rightFlywheelDutyCycle.value.volts
+        inputs.flywheelStatorCurrent = rightFlywheelStatorCurrentSignal.value.amps
+        inputs.flywheelSupplyCurrent = rightFlywheelSupplyCurrentSignal.value.amps
+        inputs.flywheelTempreature = rightFlywheelTempSignal.value.celsius
     }
 
     override fun setFlywheelBrakeMode(brake: Boolean) {
@@ -114,10 +196,10 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX) : FlywheelIO{
         } else {
             motorOutputConfig.NeutralMode = NeutralModeValue.Coast
         }
-        flywheelFalcon.configurator.apply(motorOutputConfig)
+        flywheelRightFalcon.configurator.apply(motorOutputConfig)
     }
     override fun zeroEncoder(){
         //TODO finish zero encoder fun (ask sumone what the encoder for falcon is)
-        flywheelFalcon
+
     }
 }

src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt → src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Wrist.kt

@@ -3,36 +3,34 @@ package com.team4099.robot2023.subsystems.Shooter
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.ShooterConstants
-import com.team4099.robot2023.subsystems.Shooter.ShooterIONeo.setWristPosition
+import com.team4099.robot2023.subsystems.Shooter.WristIONeo.setWristPosition
 import com.team4099.robot2023.subsystems.superstructure.Request
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.SimpleMotorFeedforward
 import org.team4099.lib.controller.TrapezoidProfile
-import org.team4099.lib.units.Velocity
 
-import org.team4099.lib.units.base.Meter
 import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.*
 import org.team4099.lib.units.perSecond
 
-class Shooter (val io: ShooterIO){
+class Wrist (val io: ShooterIO){
     val inputs = ShooterIO.ShooterIOInputs()
     //TODO do feedforward
     private val wristkS =
         LoggedTunableValue("Wrist/kS", Pair({ it.inVolts }, { it.volts})
         )
     private val wristlkV =
         LoggedTunableValue(
-            "Wrist/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotationPerMinute })
+            "Wrist/kV", Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond })
         )
     private val wristkA =
         LoggedTunableValue(
-            "Wrist/kA", Pair({ it.inVoltsPerRotationPerMinutePerSecond}, { it.volts.perRotationPerMinutePerSecond })
+            "Wrist/kA", Pair({ it.inVoltsPerDegreePerSecond.perSecond}, { it.volts.perDegreePerSecond.perSecond })
         )
-    val flywheelFeedForward = SimpleMotorFeedforward<Radian, Volt>(wristkS.get(), wristlkV.get(), wristkA.get())
-
+    val wristFeedForward = SimpleMotorFeedforward<Radian, Volt>(wristkS.get(), wristlkV.get(), wristkA.get())
 
+    override fun ()
 
 
     private val wristflywheelkP =
@@ -81,7 +79,9 @@ class Shooter (val io: ShooterIO){
             TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond),
             TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond)
         )
+     fun setWristPosition(setPoint : TrapezoidProfile.State<Radian>){
 
+     }
 fun periodic(){
     io.updateInputs(inputs)
     var nextState = currentState
@@ -124,9 +124,11 @@ fun periodic(){
                 Logger.recordOutput("/Shooter/ProfileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds)
                 timeProfileGeneratedAt = Clock.fpgaTime
                 lastWristPositionTarget = wristPositionTarget
+
             }
             val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt
-            setWristPosition(wristProfile.calculate(timeElapsed))
+            val setPoint: TrapezoidProfile.State= wristProfile.calculate(timeElapsed)
+            setWristPosition(setPoint)
             //TODO fix this error
             Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed))
             nextState = fromRequestToState(currentRequest)

src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt → src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/WristIONeo.kt

@@ -6,14 +6,13 @@ import com.revrobotics.SparkMaxPIDController
 import com.team4099.lib.math.clamp
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.ShooterConstants
-import org.team4099.lib.units.base.Meter
 import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.celsius
 import org.team4099.lib.units.base.inAmperes
 import org.team4099.lib.units.derived.*
 import org.team4099.lib.units.sparkMaxAngularMechanismSensor
 import kotlin.math.absoluteValue
-object ShooterIONeo : ShooterIO{
+object WristIONeo : ShooterIO{
     //private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless)
     //private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax,
         //ShooterConstants.ROLLER_GEAR_RATIO,