Skip to content

Commit

Permalink
fix: maybe make elevator work?
Browse files Browse the repository at this point in the history
  • Loading branch information
@mimizh2418
mimizh2418 committed Sep 28, 2024
1 parent 86f747e commit 86ba0fb
Show file tree
Hide file tree
Showing 4 changed files with 23 additions and 12 deletions.
2 changes: 1 addition & 1 deletion src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ public void execute() {
} else {
val -= DEADZONE;
}
elevator.setVoltage(-val * 12 * 0.18);
elevator.setVoltage(-val * 12);
} else {
elevator.holdPosition();
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
29 changes: 20 additions & 9 deletions src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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() {
Expand All @@ -45,6 +51,7 @@ public ElevatorIOTalonFX() {
leaderPosition,
leaderVelocity,
leaderAppliedVolts,
followerAppliedVolts,
leaderCurrent,
followerCurrent,
leaderTemp,
Expand All @@ -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;
Expand All @@ -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
Expand All @@ -87,6 +95,7 @@ public void updateInputs(ElevatorIOInputs inputs) {
leaderPosition,
leaderVelocity,
leaderAppliedVolts,
followerAppliedVolts,
leaderCurrent,
followerCurrent,
leaderTemp,
Expand All @@ -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;
Expand All @@ -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
Expand All @@ -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);
}
}

0 comments on commit 86ba0fb

Please sign in to comment.