fixed some programming issues

team4099 · Jan 23, 2024 · 686b95b · 686b95b
1 parent cf89177
commit 686b95b
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 14 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt
@@ -45,13 +45,17 @@ object ElevatorConstants {
   val ELEVATOR_KG = 0.0.volts
   val ELEVATOR_KV = 0.0.volts / 0.0.inches.perSecond
   val ELEVATOR_KA = 0.0.volts / 0.0.inches.perSecond.perSecond
+  val ELEVATOR_OPEN_LOOP_EXTEND_VOLTAGE = 8.0.volts
+  val ELEVATOR_OPEN_LOOP_RETRACT_VOLTAGE = -12.0.volts
 
-  val ENABLE_ELEVATOR = 1.0
+  val ENABLE_ELEVATOR = false
   val ELEVATOR_IDLE_HEIGHT = 0.0.inches
   val ELEVATOR_SOFT_LIMIT_EXTENSION = 0.0.inches
   val ELEVATOR_SOFT_LIMIT_RETRACTION = 0.0.inches
   val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION = 0.0.inches
   val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches
+  val ELEVATOR_SAFE_THRESHOLD = 5.0.inches
+
 
   val ELEVATOR_TOLERANCE = 0.0.inches
 
@@ -68,8 +72,7 @@ object ElevatorConstants {
   val ELEVATOR_GROUND_OFFSET = 0.0.inches
 
   val VOLTAGE_COMPENSATION = 12.0.volts
-  val GEAR_RATIO = 4.0 / 1 * 4.0 / 1
-  val SENSOR_CPR = 0
+  val ELEVATOR_PULLEY_TO_MOTOR = 4.0 / 1 * 4.0 / 1
   val SPOOL_DIAMETER = 1.5.inches
 
   val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps
@@ -81,4 +84,5 @@ object ElevatorConstants {
   val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps
   val FOLLOWER_SUPPLY_CURRENT_LIMIT = 0.0.amps
   val FOLLOWER_THRESHOLD_CURRENT_LIMIT = 0.0.amps
+
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt
@@ -88,11 +88,11 @@ class Elevator(val io: ElevatorIO) {
     // TODO: change voltages
     val openLoopExtendVoltage =
       LoggedTunableValue(
-        "Elevator/openLoopExtendVoltage", 8.volts, Pair({ it.inVolts }, { it.volts })
+        "Elevator/openLoopExtendVoltage", ElevatorConstants.ELEVATOR_OPEN_LOOP_EXTEND_VOLTAGE, Pair({ it.inVolts }, { it.volts })
       )
     val openLoopRetractVoltage =
       LoggedTunableValue(
-        "Elevator/openLoopRetractVoltage", -12.0.volts, Pair({ it.inVolts }, { it.volts })
+        "Elevator/openLoopRetractVoltage", ElevatorConstants.ELEVATOR_OPEN_LOOP_RETRACT_VOLTAGE, Pair({ it.inVolts }, { it.volts })
       )
 
     val shootSpeakerPosition =
@@ -187,7 +187,7 @@ class Elevator(val io: ElevatorIO) {
     get() =
       currentRequest is ElevatorRequest.TargetingPosition &&
         (
-          ((inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= 5.inches) ||
+          ((inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= ElevatorConstants.ELEVATOR_SAFE_THRESHOLD) ||
             elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt)
           ) &&
         lastRequestedPosition == elevatorPositionTarget
@@ -208,6 +208,8 @@ class Elevator(val io: ElevatorIO) {
       kP.initDefault(ElevatorConstants.SIM_KP)
       kI.initDefault(ElevatorConstants.SIM_KI)
       kD.initDefault(ElevatorConstants.SIM_KD)
+
+      io.configPID(kP.get(), kI.get(), kD.get())
     }
 
     elevatorFeedforward =
@@ -218,7 +220,7 @@ class Elevator(val io: ElevatorIO) {
         ElevatorConstants.ELEVATOR_KA
       )
 
-    io.configPID(kP.get(), kI.get(), kD.get())
+
   }
 
   fun periodic() {
@@ -316,7 +318,7 @@ class Elevator(val io: ElevatorIO) {
               ElevatorConstants.HOMING_STALL_TIME_THRESHOLD
             )
         ) {
-          setHomeVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE)
+          io.setVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE)
         } else {
           zeroEncoder()
           isHomed = true
@@ -355,7 +357,7 @@ class Elevator(val io: ElevatorIO) {
     if ((forwardLimitReached) && (setpoint.position > inputs.elevatorPosition) ||
       (reverseLimitReached) && (setpoint.position < inputs.elevatorPosition)
     ) {
-      io.setOutputVoltage(0.volts)
+      io.setPosition(inputs.elevatorPosition, feedForward)
     } else {
       io.setPosition(setpoint.position, feedForward)
     }

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt
@@ -37,8 +37,8 @@ interface ElevatorIO {
     var leaderTempCelcius = 0.0.celsius
     var followerTempCelcius = 0.0.celsius
 
-    var leaderRawPosition = 0.0
-    var followerRawPosition = 0.0
+    var leaderRawPosition = 0.0.rotations
+    var followerRawPosition = 0.0.rotations
 
     var isSimulating = false
 

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt
@@ -34,15 +34,13 @@ object ElevatorIOKraken : ElevatorIO {
   private val leaderSensor =
     ctreLinearMechanismSensor(
       elevatorLeaderKraken,
-      ElevatorConstants.SENSOR_CPR,
       ElevatorConstants.GEAR_RATIO,
       ElevatorConstants.SPOOL_DIAMETER,
       ElevatorConstants.VOLTAGE_COMPENSATION
     )
   private val followerSensor =
     ctreLinearMechanismSensor(
       elevatorLeaderKraken,
-      ElevatorConstants.SENSOR_CPR,
       ElevatorConstants.GEAR_RATIO,
       ElevatorConstants.SPOOL_DIAMETER,
       ElevatorConstants.VOLTAGE_COMPENSATION
@@ -169,7 +167,7 @@ object ElevatorIOKraken : ElevatorIO {
   override fun setPosition(position: Length, feedForward: ElectricalPotential) {
     elevatorLeaderKraken.setControl(
       PositionVoltage(
-        leaderSensor.positionToRawUnits(position),
+        leaderSensor.getRawPosition(),
         true,
         feedForward,
         0,