fixed more programming issues

team4099 · Jan 23, 2024 · cf89177 · cf89177
1 parent 4d50bec
commit cf89177
Show file tree

Hide file tree

Showing 4 changed files with 51 additions and 17 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt
@@ -15,6 +15,7 @@ import org.team4099.lib.units.base.Meter
 import org.team4099.lib.units.base.inInches
 import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegrees
@@ -27,6 +28,7 @@ import org.team4099.lib.units.derived.perInchSeconds
 import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.inInchesPerSecond
 import org.team4099.lib.units.perSecond
+import kotlin.time.Duration.Companion.seconds
 import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest
 
 class Elevator(val io: ElevatorIO) {
@@ -49,6 +51,22 @@ class Elevator(val io: ElevatorIO) {
     LoggedTunableValue(
       "Elevator/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond })
     )
+  private val kS =
+    LoggedTunableValue(
+      "Elevator/kS", Pair({ it.inVolts}, {it.volts})
+    )
+  private val kG =
+    LoggedTunableValue(
+      "Elevator/kG", Pair({ it.inVolts}, {it.volts})
+    )
+  private val kV =
+    LoggedTunableValue(
+      "Elevator/kG", Pair({it.inVoltsPerInchPerSecond}, {it.volts / 1.0.inches.perSecond})
+    )
+  private val kA =
+    LoggedTunableValue(
+      "Elevator/kA", Pair({it.inVolts.perMetersPerSecondPerSecond}, {it.volts / 1.0.meters.perSecond.perSecond})
+    )
 
   object TunableElevatorHeights {
     val enableElevator =

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt
@@ -18,6 +18,7 @@ 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.base.inSeconds
+import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.ctreLinearMechanismSensor
 import org.team4099.lib.units.derived.DerivativeGain
 import org.team4099.lib.units.derived.ElectricalPotential
@@ -156,16 +157,21 @@ object ElevatorIOKraken : ElevatorIO {
   }
 
   override fun setOutputVoltage(voltage: ElectricalPotential) {
-    elevatorLeaderKraken.setVoltage(voltage.inVolts)
+    if (((leaderSensor.position < 0.5.inches) && (voltage < 0.volts)) ||
+      (leaderSensor.position > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts))) {
+      elevatorLeaderKraken.setVoltage(0.0)
+    }
+    else {
+      elevatorLeaderKraken.setVoltage(voltage.inVolts)
+    }
   }
 
   override fun setPosition(position: Length, feedForward: ElectricalPotential) {
     elevatorLeaderKraken.setControl(
-      PositionDutyCycle(
+      PositionVoltage(
         leaderSensor.positionToRawUnits(position),
-        0.0,
         true,
-        feedForward.inVolts,
+        feedForward,
         0,
         false,
         false,

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt
@@ -16,6 +16,7 @@ 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.base.inPercentOutputPerSecond
+import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.derived.DerivativeGain
 import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.IntegralGain
@@ -146,7 +147,13 @@ object ElevatorIONEO : ElevatorIO {
    */
   override fun setOutputVoltage(voltage: ElectricalPotential) {
     // divide by 2 cause full power elevator is scary
-    leaderSparkMax.setVoltage(voltage.inVolts)
+    if (((leaderSensor.position < 0.5.inches) && (voltage < 0.volts)) ||
+      (leaderSensor.position > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts))
+    ) {
+      leaderSparkMax.setVoltage(0.0)
+    } else {
+      leaderSparkMax.setVoltage(voltage.inVolts)
+    }
   }
 
   /**

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt
@@ -18,6 +18,7 @@ import org.team4099.lib.units.base.celsius
 import org.team4099.lib.units.base.inKilograms
 import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.DerivativeGain
 import org.team4099.lib.units.derived.ElectricalPotential
@@ -99,16 +100,19 @@ object ElevatorIOSim : ElevatorIO {
    * @param voltage the voltage to set the motor to
    */
   override fun setOutputVoltage(voltage: ElectricalPotential) {
-    val clampedVoltage =
-      clamp(
-        voltage,
-        -ElevatorConstants.VOLTAGE_COMPENSATION,
-        ElevatorConstants.VOLTAGE_COMPENSATION
-      )
-
-    lastAppliedVoltage = clampedVoltage
-
-    elevatorSim.setInputVoltage(clampedVoltage.inVolts)
+    if (!((elevatorSim.positionMeters.meters < 0.5.inches) && (voltage < 0.volts)) &&
+      !(elevatorSim.positionMeters.meters > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts))
+    ) {
+      val clampedVoltage =
+        clamp(
+          voltage,
+          -ElevatorConstants.VOLTAGE_COMPENSATION,
+          ElevatorConstants.VOLTAGE_COMPENSATION
+        )
+      lastAppliedVoltage = clampedVoltage
+
+      elevatorSim.setInputVoltage(clampedVoltage.inVolts)
+    }
   }
 
   /**
@@ -127,13 +131,12 @@ object ElevatorIOSim : ElevatorIO {
       )
     val feedback = elevatorController.calculate(elevatorSim.positionMeters.meters, position)
 
-    lastAppliedVoltage = ff + feedback
+    setOutputVoltage(ff + feedback)
     elevatorSim.setInputVoltage((ff + feedback).inVolts)
   }
 
   /** set the current encoder position to be the encoders zero value */
   override fun zeroEncoder() {
-    println("don't work right now")
   }
 
   /**