diff --git a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java index 4cc1fe5..d89a830 100644 --- a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java +++ b/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(); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java index 385a639..cfc206f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java +++ b/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; diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java index 8c6e024..4c91da9 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java +++ b/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(); diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java index ce7b236..5135a65 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java +++ b/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 leaderPosition = leader.getPosition(); private final StatusSignal leaderVelocity = leader.getVelocity(); private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); + private final StatusSignal followerAppliedVolts = follower.getMotorVoltage(); private final StatusSignal leaderCurrent = leader.getStatorCurrent(); private final StatusSignal followerCurrent = follower.getStatorCurrent(); private final StatusSignal leaderTemp = leader.getDeviceTemp(); private final StatusSignal followerTemp = follower.getDeviceTemp(); private final StatusSignal topLimitStatus = leader.getForwardLimit(); private final StatusSignal 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); } }