fix: maybe make elevator work?

Author: mimizh2418

src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java

@@ -25,7 +25,7 @@ public void execute() {
             } else {
                 val -= DEADZONE;
             }
-            elevator.setVoltage(-val * 12 * 0.18);
+            elevator.setVoltage(-val * 12);
         } else {
             elevator.holdPosition();
         }

src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java

@@ -7,7 +7,7 @@ public interface ElevatorIO {
     class ElevatorIOInputs {
         public double positionMeters = 0.0;
         public double velocityMPS = 0.0;
-        public double voltage = 0.0;
+        public double[] voltage = new double[]{};
         public double[] currentAmps = new double[]{};
         public double[] tempCelsius = new double[]{};
         public boolean atUpperLimit = false;

src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java

@@ -47,7 +47,7 @@ public void updateInputs(ElevatorIOInputs inputs) {
 
         inputs.positionMeters = elevatorSim.getPositionMeters();
         inputs.velocityMPS = elevatorSim.getVelocityMetersPerSecond();
-        inputs.voltage = elevatorAppliedVolts;
+        inputs.voltage = new double[]{elevatorAppliedVolts};
         inputs.currentAmps = new double[]{elevatorSim.getCurrentDrawAmps()};
         inputs.atUpperLimit = elevatorSim.hasHitUpperLimit();
         inputs.atLowerLimit = elevatorSim.hasHitLowerLimit();

src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java

@@ -7,28 +7,34 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.*;
 import edu.wpi.first.wpilibj.Servo;
 import org.team1540.robot2024.Constants;
 
 public class ElevatorIOTalonFX implements ElevatorIO {
-
-    private final MotionMagicVoltage motionMagicOut = new MotionMagicVoltage(0).withEnableFOC(true);
     private final TalonFX leader = new TalonFX(Constants.Elevator.LEADER_ID);
     private final TalonFX follower = new TalonFX(Constants.Elevator.FOLLOWER_ID);
+
+    private final MotionMagicVoltage motionMagicOut = new MotionMagicVoltage(0).withEnableFOC(true);
+    private final VoltageOut voltageOut = new VoltageOut(0).withEnableFOC(true);
+    private final Follower followerReq = new Follower(leader.getDeviceID(), false);
+
     private final Servo leftFlipper = new Servo(Constants.Elevator.LEFT_FLIPPER_ID);
     private final Servo rightFlipper = new Servo(Constants.Elevator.RIGHT_FLIPPER_ID);
+
     private final StatusSignal<Double> leaderPosition = leader.getPosition();
     private final StatusSignal<Double> leaderVelocity = leader.getVelocity();
     private final StatusSignal<Double> leaderAppliedVolts = leader.getMotorVoltage();
+    private final StatusSignal<Double> followerAppliedVolts = follower.getMotorVoltage();
     private final StatusSignal<Double> leaderCurrent = leader.getStatorCurrent();
     private final StatusSignal<Double> followerCurrent = follower.getStatorCurrent();
     private final StatusSignal<Double> leaderTemp = leader.getDeviceTemp();
     private final StatusSignal<Double> followerTemp = follower.getDeviceTemp();
     private final StatusSignal<ForwardLimitValue> topLimitStatus = leader.getForwardLimit();
     private final StatusSignal<ReverseLimitValue> bottomLimitStatus = leader.getReverseLimit();
-    TalonFXConfiguration config;
+    private final TalonFXConfiguration config;
 
 
     public ElevatorIOTalonFX() {
@@ -45,6 +51,7 @@ public ElevatorIOTalonFX() {
                 leaderPosition,
                 leaderVelocity,
                 leaderAppliedVolts,
+                followerAppliedVolts,
                 leaderCurrent,
                 followerCurrent,
                 leaderTemp,
@@ -55,7 +62,7 @@ public ElevatorIOTalonFX() {
         follower.optimizeBusUtilization();
 
         // setting slot 0 gains
-        Slot0Configs slot0Configs = config.Slot0;
+        Slot0Configs slot0Configs = new Slot0Configs();
         slot0Configs.kS = Constants.Elevator.KS;
         slot0Configs.kV = Constants.Elevator.KV;
         slot0Configs.kA = Constants.Elevator.KA;
@@ -64,21 +71,22 @@ public ElevatorIOTalonFX() {
         slot0Configs.kD = Constants.Elevator.KD;
         slot0Configs.kG = Constants.Elevator.KG;
         slot0Configs.GravityType = GravityTypeValue.Elevator_Static;
+        config.Slot0 = slot0Configs;
 
         // setting Motion Magic Settings
-        MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
+        MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs();
         motionMagicConfigs.MotionMagicCruiseVelocity = Constants.Elevator.CRUISE_VELOCITY_MPS;
         motionMagicConfigs.MotionMagicAcceleration = Constants.Elevator.MAXIMUM_ACCELERATION_MPS2;
         motionMagicConfigs.MotionMagicJerk = Constants.Elevator.JERK_MPS3;
+        config.MotionMagic = motionMagicConfigs;
 
         config.HardwareLimitSwitch.ForwardLimitEnable = true;
         config.HardwareLimitSwitch.ReverseLimitEnable = true;
         config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = true;
         config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = 0;
         leader.getConfigurator().apply(config);
-        config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
         follower.getConfigurator().apply(config);
-        follower.setControl(new Follower(leader.getDeviceID(), false));
+        follower.setControl(followerReq);
     }
 
     @Override
@@ -87,6 +95,7 @@ public void updateInputs(ElevatorIOInputs inputs) {
                 leaderPosition,
                 leaderVelocity,
                 leaderAppliedVolts,
+                followerAppliedVolts,
                 leaderCurrent,
                 followerCurrent,
                 leaderTemp,
@@ -96,7 +105,7 @@ public void updateInputs(ElevatorIOInputs inputs) {
 
         inputs.positionMeters = leaderPosition.getValueAsDouble();
         inputs.velocityMPS = leaderVelocity.getValueAsDouble();
-        inputs.voltage = leaderAppliedVolts.getValueAsDouble();
+        inputs.voltage = new double[]{leaderAppliedVolts.getValueAsDouble(), followerAppliedVolts.getValueAsDouble()};
         inputs.currentAmps = new double[]{leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()};
         inputs.tempCelsius = new double[]{leaderTemp.getValueAsDouble(), followerTemp.getValueAsDouble()};
         inputs.atUpperLimit = topLimitStatus.getValue() == ForwardLimitValue.ClosedToGround;
@@ -111,13 +120,14 @@ public void setSetpointMeters(double positionMeters) {
 
     @Override
     public void setVoltage(double voltage) {
-        leader.set(voltage);
+        leader.setControl(voltageOut.withOutput(voltage));
     }
 
     @Override
     public void setBrakeMode(boolean isBrakeMode) {
         leader.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast);
         follower.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast);
+        follower.setControl(followerReq);
     }
 
     @Override
@@ -134,5 +144,6 @@ public void configPID(double kP, double kI, double kD) {
         configs.kI = kI;
         configs.kD = kD;
         leader.getConfigurator().apply(configs);
+        follower.setControl(followerReq);
     }
 }