Skip to content

Commit 616968b

Browse files
Tortuga-AMTortuga-AM
authored
Merge pull request #21 from FRC-7525/Climber
Climber
2 parents 01de5a8 + 2619a87 commit 616968b

File tree

6 files changed

+247
-0
lines changed

6 files changed

+247
-0
lines changed

‎src/main/java/frc/robot/Subsystems/Climber/Climber.java‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
package frc.robot.Subsystems.Climber;
2+
3+
import static edu.wpi.first.units.Units.Rotations;
4+
5+
import frc.robot.GlobalConstants;
6+
import org.littletonrobotics.junction.Logger;
7+
import org.team7525.subsystem.Subsystem;
8+
9+
public class Climber extends Subsystem<ClimberStates> {
10+
11+
private static Climber instance;
12+
private final ClimberIO io;
13+
private ClimberIO.ClimberIOOutputs outputs;
14+
15+
public static Climber getInstance() {
16+
if (instance == null) {
17+
switch (GlobalConstants.ROBOT_MODE) {
18+
case REAL -> instance = new Climber(new ClimberIOReal());
19+
case SIM -> instance = new Climber(new ClimberIOSim());
20+
case TESTING -> instance = new Climber(new ClimberIOReal());
21+
}
22+
}
23+
return instance;
24+
}
25+
26+
private Climber(ClimberIO io) {
27+
super(ClimberConstants.SUBSYSTEM_NAME, ClimberStates.IDLE);
28+
this.io = io;
29+
this.outputs = new ClimberIO.ClimberIOOutputs();
30+
}
31+
32+
@Override
33+
public void runState() {
34+
io.setPosition(getState().getClimberSetpoint());
35+
io.logOutputs(outputs);
36+
37+
Logger.recordOutput(getName() + "/LeftPositionRot", outputs.leftPosition.in(Rotations));
38+
Logger.recordOutput(getName() + "/RightPositionRot", outputs.rightPosition.in(Rotations));
39+
Logger.recordOutput(getName() + "/SetpointRot", outputs.setpoint.in(Rotations));
40+
Logger.recordOutput(getName() + "/state", getState().getStateString());
41+
}
42+
43+
public boolean readyToClimb() {
44+
return io.atPositionSetpoint();
45+
}
46+
}

‎src/main/java/frc/robot/Subsystems/Climber/ClimberConstants.java‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
package frc.robot.Subsystems.Climber;
2+
3+
import static edu.wpi.first.units.Units.Rotations;
4+
5+
import edu.wpi.first.math.controller.PIDController;
6+
import edu.wpi.first.units.measure.Angle;
7+
import frc.robot.GlobalConstants;
8+
import java.util.function.Supplier;
9+
10+
public class ClimberConstants {
11+
12+
public static final String SUBSYSTEM_NAME = "Climber";
13+
public static final int LEFT_CLIMBER_MOTOR_ID = 20;
14+
public static final int RIGHT_CLIMBER_MOTOR_ID = 21;
15+
public static final Supplier<PIDController> CLIMB_PID = () ->
16+
switch (GlobalConstants.ROBOT_MODE) {
17+
case REAL -> new PIDController(1.0, 0.0, 0.0);
18+
case SIM -> new PIDController(0.05, 0.0, 0.0);
19+
case TESTING -> new PIDController(1.0, 0.0, 0.0);
20+
};
21+
public static final Angle CLIMB_POSITION_TOLERANCE = Rotations.of(0.01);
22+
public static final Angle IDLE_SETPOINT = Rotations.of(0.0);
23+
public static final Angle EXTEND_SETPOINT = Rotations.of(0.0833333333);
24+
public static final Angle RETRACT_SETPOINT = Rotations.of(-0.0138888889);
25+
public static final Angle HOLD_SETPOINT = Rotations.of(0.0013888889);
26+
public static final double CLIMBER_MOI = 0.05; // moment of inertia (placeholder)
27+
public static final double CLIMBER_GEARING = 1.0;
28+
public static final double CLIMBER_ARM_LENGTH_METERS = 0.1;
29+
30+
public static final double CLIMBER_MASS = 1.0; // kg (placeholder)
31+
public static final double CLIMBER_RADIUS = 0.02; // meters (placeholder)
32+
public static final double DRUM_RADIUS = 0.02; // meters (placeholder)
33+
public static final double START_HEIGHT = 0.0; // meters (placeholder)
34+
public static final double END_HEIGHT = 0.1; // meters (placeholder)
35+
}

‎src/main/java/frc/robot/Subsystems/Climber/ClimberIO.java‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
package frc.robot.Subsystems.Climber;
2+
3+
import static edu.wpi.first.units.Units.Rotations;
4+
5+
import edu.wpi.first.units.measure.Angle;
6+
7+
public interface ClimberIO {
8+
class ClimberIOOutputs {
9+
10+
public Angle leftPosition = Rotations.of(0);
11+
public Angle rightPosition = Rotations.of(0);
12+
public Angle setpoint = Rotations.of(0);
13+
}
14+
15+
void logOutputs(ClimberIOOutputs outputs);
16+
17+
void setPosition(Angle position);
18+
19+
boolean atPositionSetpoint();
20+
}

‎src/main/java/frc/robot/Subsystems/Climber/ClimberIOReal.java‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
package frc.robot.Subsystems.Climber;
2+
3+
import static edu.wpi.first.units.Units.Rotations;
4+
5+
import com.ctre.phoenix6.controls.Follower;
6+
import com.ctre.phoenix6.hardware.TalonFX;
7+
import com.ctre.phoenix6.signals.MotorAlignmentValue;
8+
import edu.wpi.first.math.controller.PIDController;
9+
import edu.wpi.first.units.measure.Angle;
10+
import org.littletonrobotics.junction.Logger;
11+
12+
public class ClimberIOReal implements ClimberIO {
13+
14+
protected TalonFX leftMotor;
15+
protected TalonFX rightMotor;
16+
protected Angle positionSetpoint;
17+
protected PIDController pid;
18+
19+
public ClimberIOReal() {
20+
leftMotor = new TalonFX(ClimberConstants.LEFT_CLIMBER_MOTOR_ID);
21+
rightMotor = new TalonFX(ClimberConstants.RIGHT_CLIMBER_MOTOR_ID);
22+
rightMotor.setControl(new Follower(leftMotor.getDeviceID(), MotorAlignmentValue.Aligned));
23+
24+
pid = ClimberConstants.CLIMB_PID.get();
25+
positionSetpoint = ClimberConstants.IDLE_SETPOINT;
26+
}
27+
28+
@Override
29+
public void logOutputs(ClimberIOOutputs outputs) {
30+
outputs.leftPosition = leftMotor.getPosition().getValue();
31+
outputs.rightPosition = rightMotor.getPosition().getValue();
32+
outputs.setpoint = positionSetpoint;
33+
34+
Logger.recordOutput(ClimberConstants.SUBSYSTEM_NAME + "/LeftPositionRot", outputs.leftPosition.in(Rotations));
35+
Logger.recordOutput(ClimberConstants.SUBSYSTEM_NAME + "/RightPositionRot", outputs.rightPosition.in(Rotations));
36+
Logger.recordOutput(ClimberConstants.SUBSYSTEM_NAME + "/SetpointRot", outputs.setpoint.in(Rotations));
37+
}
38+
39+
@Override
40+
public void setPosition(Angle position) {
41+
this.positionSetpoint = position;
42+
leftMotor.set(pid.calculate(leftMotor.getPosition().getValue().in(Rotations), positionSetpoint.in(Rotations)));
43+
}
44+
45+
@Override
46+
public boolean atPositionSetpoint() {
47+
double currentRot = leftMotor.getPosition().getValue().in(Rotations);
48+
return Math.abs(currentRot - positionSetpoint.in(Rotations)) < ClimberConstants.CLIMB_POSITION_TOLERANCE.in(Rotations);
49+
}
50+
}

‎src/main/java/frc/robot/Subsystems/Climber/ClimberIOSim.java‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
package frc.robot.Subsystems.Climber;
2+
3+
import static edu.wpi.first.units.Units.Rotations;
4+
5+
import com.ctre.phoenix6.controls.Follower;
6+
import com.ctre.phoenix6.signals.MotorAlignmentValue;
7+
import com.ctre.phoenix6.sim.TalonFXSimState;
8+
import edu.wpi.first.math.system.plant.DCMotor;
9+
import edu.wpi.first.math.system.plant.LinearSystemId;
10+
import edu.wpi.first.units.measure.Angle;
11+
import edu.wpi.first.wpilibj.simulation.ElevatorSim;
12+
import frc.robot.GlobalConstants;
13+
import org.littletonrobotics.junction.Logger;
14+
15+
public class ClimberIOSim extends ClimberIOReal {
16+
17+
protected TalonFXSimState leftMotorSim;
18+
protected TalonFXSimState rightMotorSim;
19+
protected ElevatorSim climbSim;
20+
protected Angle positionSetpoint;
21+
22+
public ClimberIOSim() {
23+
rightMotor.setControl(new Follower(leftMotor.getDeviceID(), MotorAlignmentValue.Aligned));
24+
25+
leftMotorSim = new TalonFXSimState(leftMotor);
26+
rightMotorSim = new TalonFXSimState(rightMotor);
27+
climbSim = new ElevatorSim(
28+
LinearSystemId.createElevatorSystem(DCMotor.getFalcon500(1), ClimberConstants.CLIMBER_MASS, ClimberConstants.CLIMBER_RADIUS, ClimberConstants.CLIMBER_GEARING),
29+
DCMotor.getFalcon500(1),
30+
ClimberConstants.START_HEIGHT,
31+
ClimberConstants.END_HEIGHT,
32+
true,
33+
ClimberConstants.START_HEIGHT
34+
);
35+
36+
positionSetpoint = ClimberConstants.IDLE_SETPOINT;
37+
}
38+
39+
@Override
40+
public void logOutputs(ClimberIOOutputs outputs) {
41+
climbSim.update(GlobalConstants.SIMULATION_PERIOD);
42+
43+
leftMotorSim.setRawRotorPosition(climbSim.getPositionMeters());
44+
leftMotorSim.setRotorVelocity(climbSim.getVelocityMetersPerSecond());
45+
rightMotorSim.setRawRotorPosition(climbSim.getPositionMeters());
46+
rightMotorSim.setRotorVelocity(climbSim.getVelocityMetersPerSecond());
47+
48+
outputs.leftPosition = leftMotor.getPosition().getValue();
49+
outputs.rightPosition = rightMotor.getPosition().getValue();
50+
outputs.setpoint = positionSetpoint;
51+
52+
Logger.recordOutput(ClimberConstants.SUBSYSTEM_NAME + "/SimLeftRot", outputs.leftPosition.in(Rotations));
53+
Logger.recordOutput(ClimberConstants.SUBSYSTEM_NAME + "/SimRightRot", outputs.rightPosition.in(Rotations));
54+
Logger.recordOutput(ClimberConstants.SUBSYSTEM_NAME + "/SimSetpointRot", outputs.setpoint.in(Rotations));
55+
}
56+
57+
@Override
58+
public void setPosition(Angle position) {
59+
this.positionSetpoint = position;
60+
climbSim.setInput(pid.calculate(leftMotor.getPosition().getValue().in(Rotations), positionSetpoint.in(Rotations)));
61+
}
62+
63+
@Override
64+
public boolean atPositionSetpoint() {
65+
double currentRot = leftMotor.getPosition().getValue().in(Rotations);
66+
return Math.abs(currentRot - positionSetpoint.in(Rotations)) < ClimberConstants.CLIMB_POSITION_TOLERANCE.in(Rotations);
67+
}
68+
}

‎src/main/java/frc/robot/Subsystems/Climber/ClimberStates.java‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
package frc.robot.Subsystems.Climber;
2+
3+
import edu.wpi.first.units.measure.Angle;
4+
import org.team7525.subsystem.SubsystemStates;
5+
6+
public enum ClimberStates implements SubsystemStates {
7+
IDLE("IDLE", ClimberConstants.IDLE_SETPOINT),
8+
EXTEND("EXTEND", ClimberConstants.EXTEND_SETPOINT),
9+
RETRACT("RETRACT", ClimberConstants.RETRACT_SETPOINT),
10+
HOLD("HOLD", ClimberConstants.HOLD_SETPOINT);
11+
12+
private final String stateString;
13+
private final Angle setpoint;
14+
15+
ClimberStates(String stateString, Angle setpoint) {
16+
this.stateString = stateString;
17+
this.setpoint = setpoint;
18+
}
19+
20+
@Override
21+
public String getStateString() {
22+
return stateString;
23+
}
24+
25+
public Angle getClimberSetpoint() {
26+
return setpoint;
27+
}
28+
}

0 commit comments

Comments
 (0)