From 51c66edad7aa62d52cfb124a29556e6f35d921bb Mon Sep 17 00:00:00 2001
From: nbhog <146785661+nbhog@users.noreply.github.com>
Date: Thu, 18 Jan 2024 19:16:53 -0500
Subject: [PATCH] started kraken hardware file

---
 .../config/constants/ElevatorConstants.kt     | 24 +++++++++
 .../subsystems/elevator/ElevatorIOKraken.kt   | 52 +++++++++++++++++--
 2 files changed, 73 insertions(+), 3 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
index 4c27e1d5..204f827d 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt
@@ -5,7 +5,10 @@ import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.perInch
+import org.team4099.lib.units.derived.rotations
 import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.perMinute
 import org.team4099.lib.units.perSecond
 
 object ElevatorConstants {
@@ -46,4 +49,25 @@ object ElevatorConstants {
 
     val LEADER_VOLTAGE = 0.0.volts
     val LEADER_GEAR_RATIO = 0.0
+    val LEADER_SENSOR_CPR = 0
+    val LEADER_DIAMETER = 0.0.inches
+    val LEADER_KP = 0.0.volts/1.0.inches.perSecond
+    val LEADER_KI = 0.0.volts/(1.0.inches.perSecond*1.0.seconds)
+    val LEADER_KD = 0.0.volts/(1.0.inches.perSecond/1.0.seconds)
+    val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps
+    val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps
+    val LEADER_SUPPLY_TIME_THRESHOLD = 0.0.seconds
+    val LEADER_STATOR_CURRENT_LIMIT = 0.0.amps
+
+    val FOLLOWER_VOLTAGE = 0.0.volts
+    val FOLLOWER_GEAR_RATIO = 0.0
+    val FOLLOWER_SENSOR_CPR = 0
+    val FOLLOWER_DIAMETER = 0.0.inches
+    val FOLLOWER_KP = 0.0.volts/1.0.inches.perSecond
+    val FOLLOWER_KI = 0.0.volts/(1.0.inches.perSecond*1.0.seconds)
+    val FOLLOWER_KD = 0.0.volts/(1.0.inches.perSecond/1.0.seconds)
+    val FOLLOWER_SUPPLY_CURRENT_LIMIT = 0.0.amps
+    val FOLLOWER_THRESHOLD_CURRENT_LIMIT = 0.0.amps
+    val FOLLOWER_SUPPLY_TIME_THRESHOLD = 0.0.seconds
+    val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps
 }
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt
index 984c3bda..09a25724 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt
@@ -1,12 +1,58 @@
 package com.team4099.robot2023.subsystems.elevator
 
+import com.ctre.phoenix6.StatusSignal
+import com.ctre.phoenix6.configs.TalonFXConfiguration
 import com.ctre.phoenix6.hardware.TalonFX
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.ElevatorConstants
+import org.team4099.lib.units.base.inAmperes
+import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.ctreLinearMechanismSensor
 
 object ElevatorIOKraken: ElevatorIO {
-    private val elevatorLeaderKraken = TalonFX(Constants.Elevator.LEADER_MOTOR_ID)
-    private val elevatorFollowerKraken = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID)
-    private val leaderSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.LEADER_GEAR_RATIO, ElevatorConstants.LEADER_VOLTAGE)
+    private val elevatorLeaderKraken: TalonFX = TalonFX(Constants.Elevator.LEADER_MOTOR_ID)
+    private val elevatorFollowerKraken: TalonFX = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID)
+    private val leaderSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.LEADER_SENSOR_CPR, ElevatorConstants.LEADER_GEAR_RATIO, ElevatorConstants.LEADER_DIAMETER, ElevatorConstants.LEADER_VOLTAGE)
+    private val followerSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.FOLLOWER_SENSOR_CPR, ElevatorConstants.FOLLOWER_GEAR_RATIO, ElevatorConstants.FOLLOWER_DIAMETER, ElevatorConstants.FOLLOWER_VOLTAGE)
+    private val elevatorLeaderConfiguration: TalonFXConfiguration = TalonFXConfiguration()
+    private val elevatorFollowerConfiguration: TalonFXConfiguration = TalonFXConfiguration()
+
+    lateinit var elevatorLeaderStatorCurrentSignal: StatusSignal<Double>
+    lateinit var elevatorLeaderSupplyCurrentSignal: StatusSignal<Double>
+    lateinit var elevatorLeadertempSignal: StatusSignal<Double>
+    lateinit var elevatorLeaderDutyCycle: StatusSignal<Double>
+    lateinit var elevatorFollowerStatorCurrentSignal: StatusSignal<Double>
+    lateinit var elevatorFollowerSupplyCurrentSignal: StatusSignal<Double>
+    lateinit var elevatorFollowertempSignal: StatusSignal<Double>
+    lateinit var elevatorFollowerDutyCycle: StatusSignal<Double>
+
+    init {
+        elevatorLeaderKraken.clearStickyFaults()
+        elevatorFollowerKraken.clearStickyFaults()
+        elevatorLeaderConfiguration.Slot0.kP = leaderSensor.proportionalVelocityGainToRawUnits(ElevatorConstants.LEADER_KP)
+        elevatorLeaderConfiguration.Slot0.kI = leaderSensor.integralVelocityGainToRawUnits(ElevatorConstants.LEADER_KI)
+        elevatorLeaderConfiguration.Slot0.kD = leaderSensor.derivativeVelocityGainToRawUnits(ElevatorConstants.LEADER_KD)
+
+        elevatorFollowerConfiguration.Slot0.kP = followerSensor.proportionalVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KP)
+        elevatorFollowerConfiguration.Slot0.kI = followerSensor.integralVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KI)
+        elevatorFollowerConfiguration.Slot0.kD = followerSensor.derivativeVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KD)
+
+        elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimit = ElevatorConstants.LEADER_SUPPLY_CURRENT_LIMIT.inAmperes
+        elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentThreshold = ElevatorConstants.LEADER_THRESHOLD_CURRENT_LIMIT.inAmperes
+        elevatorLeaderConfiguration.CurrentLimits.SupplyTimeThreshold = ElevatorConstants.LEADER_SUPPLY_TIME_THRESHOLD.inSeconds
+        elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true
+        elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimit = ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes
+        elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimitEnable = false
+
+        elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimit = ElevatorConstants.FOLLOWER_SUPPLY_CURRENT_LIMIT.inAmperes
+        elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentThreshold = ElevatorConstants.FOLLOWER_THRESHOLD_CURRENT_LIMIT.inAmperes
+        elevatorFollowerConfiguration.CurrentLimits.SupplyTimeThreshold = ElevatorConstants.FOLLOWER_SUPPLY_TIME_THRESHOLD.inSeconds
+        elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true
+        elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimit = ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes
+        elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimitEnable = false
+
+        elevatorLeaderKraken.configurator.apply(elevatorLeaderConfiguration)
+        elevatorFollowerKraken.configurator.apply(elevatorFollowerConfiguration)
+
+    }
 }
\ No newline at end of file