feat: Motion Magic settings

flamingchickens1540 · Jan 25, 2024 · 69508b0 · 69508b0
1 parent c098a8c
commit 69508b0
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 5 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -57,9 +57,16 @@ public static class Drivetrain {
     }
 
     public static class Elevator {
-
         public static int talonId1;
         public static int talonId2;
-
+        public static double kS = 0.25;
+        public static double kV = 0.12;
+        public static double kA = 0.01;
+        public static double kP = 4.8;
+        public static double kI = 0;
+        public static double kD = 0.1;
+        public static int motionMagicCruiseVelocity = 80;
+        public static int motionMagicAcceleration = 160;
+        public static int motionMagicJerk = 1600;
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java
@@ -1,5 +1,6 @@
 package org.team1540.robot2024.subsystems.elevator;
 
+import org.littletonrobotics.junction.Logger;
 import org.team1540.robot2024.Constants;
 
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -9,12 +10,20 @@
 public class Elevator extends SubsystemBase {
     private final TalonFX talon1 = new TalonFX(Constants.Elevator.talonId1);
     private final TalonFX talon2 = new TalonFX(Constants.Elevator.talonId2);
-    private final ElevatorIO inputs;
+    private final ElevatorIO elevatorIO;
+    private final ElevatorIOInputsAutoLogged elevatorInputs = new ElevatorIOInputsAutoLogged();
 
 
-    public Elevator(ElevatorIO inputs) {
-        this.inputs = inputs;
+    public Elevator(ElevatorIO elevatorIO) {
+        this.elevatorIO = elevatorIO;
     }
 
+    // periodic
+    public void periodic(){
+        elevatorIO.updateInputs(elevatorInputs);
+        Logger.processInputs("Elevator", elevatorInputs);
+
+    }
     // do positional control stuff here
+
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java
@@ -36,6 +36,23 @@ public ElevatorIOTalonFX() {
             50.0, leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent);
         leader.optimizeBusUtilization();
         follower.optimizeBusUtilization();
+
+        // setting slot 0 gains
+        var slot0Configs = config.Slot0;
+        slot0Configs.kS = Constants.Elevator.kS;
+        slot0Configs.kV = Constants.Elevator.kV;
+        slot0Configs.kA = Constants.Elevator.kA;
+        slot0Configs.kP = Constants.Elevator.kP;
+        slot0Configs.kI = Constants.Elevator.kI;
+        slot0Configs.kD = Constants.Elevator.kD;
+
+        // setting Motion Magic Settings
+        var motionMagicConfigs = config.MotionMagic;
+        motionMagicConfigs.MotionMagicCruiseVelocity = Constants.Elevator.motionMagicCruiseVelocity;
+        motionMagicConfigs.MotionMagicAcceleration = Constants.Elevator.motionMagicAcceleration;
+        motionMagicConfigs.MotionMagicJerk = Constants.Elevator.motionMagicJerk;
+
+        leader.getConfigurator().apply(config);
     }
 
 }