Added init for Elevator.kt

team4099 · Jan 16, 2024 · 6946de7 · 6946de7
1 parent b161b28
commit 6946de7
Show file tree

Hide file tree

Showing 3 changed files with 53 additions and 12 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
@@ -8,7 +8,15 @@ import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.perSecond
 
 object ElevatorConstants {
-    // TODO: Change values later
+    // TODO: Change values later based on CAD
+    val REAL_KP = 0.0.volts / 1.inches
+    val REAL_KI = 0.0.volts / (1.inches * 1.seconds)
+    val REAL_KD = 0.0.volts / (1.inches.perSecond)
+
+    val SIM_KP = 0.0.volts / 1.inches
+    val SIM_KI = 0.0.volts / (1.inches * 1.seconds)
+    val SIM_KD = 0.0.volts / (1.inches.perSecond)
+
     val ELEVATOR_KS = 0.0.volts
     val ELEVATOR_KG = 0.0.volts
     val ELEVATOR_KV = 0.0.volts/0.0.inches.perSecond

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt
@@ -4,6 +4,7 @@ import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.LoggedTunableNumber
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.ElevatorConstants
+import edu.wpi.first.wpilibj.RobotBase
 import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest
 import org.team4099.lib.controller.ElevatorFeedforward
 import org.team4099.lib.controller.TrapezoidProfile
@@ -135,6 +136,7 @@ class Elevator(val io: ElevatorIO) {
 
     private var lastHomingStatorCurrentTripTime = -9999.seconds
 
+    // Trapezoidal Profile Constraints
     private var elevatorConstraints: TrapezoidProfile.Constraints<Meter> =
         TrapezoidProfile.Constraints(ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION)
     private var elevatorSetpoint: TrapezoidProfile.State<Meter> =
@@ -162,6 +164,38 @@ class Elevator(val io: ElevatorIO) {
                     ) && lastRequestedPosition == elevatorPositionTarget
 
 
+    init {
+        TunableElevatorHeights
+
+        // Initializes PID constants and changes FF depending on if sim or real
+        if(RobotBase.isReal()) {
+            isHomed = false
+
+            kP.initDefault(ElevatorConstants.REAL_KP)
+            kI.initDefault(ElevatorConstants.REAL_KI)
+            kD.initDefault(ElevatorConstants.REAL_KD)
+        } else {
+            isHomed = true
+
+            kP.initDefault(ElevatorConstants.SIM_KP)
+            kI.initDefault(ElevatorConstants.SIM_KI)
+            kD.initDefault(ElevatorConstants.SIM_KD)
+        }
+
+        elevatorFeedforward = ElevatorFeedforward(
+            ElevatorConstants.ELEVATOR_KS,
+            ElevatorConstants.ELEVATOR_KG,
+            ElevatorConstants.ELEVATOR_KV,
+            ElevatorConstants.ELEVATOR_KA
+        )
+
+        io.configPID(kP.get(), kI.get(), kD.get())
+    }
+
+    fun periodic() {
+
+    }
+
     companion object {
         enum class ElevatorState {
             UNINITIALIZED,

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt
@@ -99,16 +99,15 @@ interface ElevatorIO {
                 followerRawPosition = it
             }
         }
-
-        fun updateInputs(inputs: ElevatorInputs) {}
-        fun setOutputVoltage(voltage: ElectricalPotential) {}
-        fun setPosition(position: Length, feedForward: ElectricalPotential) {}
-        fun zeroEncoder() {}
-        fun configPID(
-                kP: ProportionalGain<Meter, Volt>,
-                kI: IntegralGain<Meter, Volt>,
-                kD: DerivativeGain<Meter, Volt>
-        ) {
-        }
     }
+
+    fun updateInputs(inputs: ElevatorInputs) {}
+    fun setOutputVoltage(voltage: ElectricalPotential) {}
+    fun setPosition(position: Length, feedForward: ElectricalPotential) {}
+    fun zeroEncoder() {}
+    fun configPID(
+        kP: ProportionalGain<Meter, Volt>,
+        kI: IntegralGain<Meter, Volt>,
+        kD: DerivativeGain<Meter, Volt>
+    ) {}
 }