From 449d1c2647c934cae9dde43394666277748f5066 Mon Sep 17 00:00:00 2001
From: taiMorioka <thetaimorioka@gmail.com>
Date: Tue, 21 Jan 2025 14:49:28 -0800
Subject: [PATCH] Should be done

---
 .../frc2025/subsystems/elevator/Elevator.kt        | 14 +++++++-------
 .../frc2025/subsystems/elevator/ElevatorIO.kt      |  9 ++++++---
 2 files changed, 13 insertions(+), 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
index 65b77c9..0dc4d98 100644
--- 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)),
     }
 }
\ No newline at end of file
diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt
index 4c003d7..75fc3f3 100644
--- 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)
     }
 
 }