Should be done

FRC3636 · Jan 21, 2025 · 449d1c2 · 449d1c2
1 parent 3ff9944
commit 449d1c2
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 10 deletions.
diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt
@@ -35,12 +35,12 @@ object Elevator: Subsystem {
 //        sysID.dynamic(direction)!!
 
     enum class Position(val height: Distance) {
-        Stowed(Meters.of(0.0)),
-        Trough(Meters.of(0.0)),
-        LowBar(Meters.of(0.0)),
-        MidBar(Meters.of(0.0)),
-        HighBar(Meters.of(0.0)),
-        LowAlgae(Meters.of(0.0)),
-        HighAlgae(Meters.of(0.0)),
+        Stowed(Meters.of(0.0)), // Not true but will change with the design, make an actual constant after a real robot is built
+        Trough(Meters.of(0.418)),
+        LowBar(Meters.of(0.765124)),
+        MidBar(Meters.of(1.161953)),
+        HighBar(Meters.of(1.809750)),
+//        LowAlgae(Meters.of(0.0)), // No algae for now according to the cad people
+//        HighAlgae(Meters.of(0.0)),
     }
 }
diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt
@@ -94,6 +94,7 @@ class ElevatorIOReal: ElevatorIO {
     }
 
     override fun runToHeight(height: Distance) {
+        assert(height in MIN_HEIGHT..MAX_HEIGHT)
         Logger.recordOutput("Elevator/Height Setpoint", height)
         var desiredMotorAngle = (height / DISTANCE_PER_TURN) as Angle
         var controlRequest = MotionMagicTorqueCurrentFOC(desiredMotorAngle)
@@ -109,14 +110,16 @@ class ElevatorIOReal: ElevatorIO {
     }
 
     internal companion object Constants {
-        val DISTANCE_PER_TURN = Meters.per(Rotation).of(0.0)!!
+        private val DISTANCE_PER_TURN = Meters.per(Rotation).of(0.0)!!
         private const val GEAR_RATIO = 0.0
-        val PID_GAINS = PIDGains(0.0, 0.0, 0.0)
-        val FF_GAINS = MotorFFGains(0.0, 0.0, 0.0)
+        private val PID_GAINS = PIDGains(0.0, 0.0, 0.0)
+        private val FF_GAINS = MotorFFGains(0.0, 0.0, 0.0)
         private const val GRAVITY_GAIN = 0.0
         private const val PROFILE_ACCELERATION = 0.0
         private const val PROFILE_JERK = 0.0
         private const val PROFILE_VELOCITY = 0.0
+        private val MAX_HEIGHT = Meters.of(100.0)
+        private val MIN_HEIGHT = Meters.of(0.0)
     }
 
 }