SuperStructure Elevator

java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
ExampleElevator extends SubsystemBase{
	SparkMax motor = new SparkMax(42,MotorType.kBrushless);

	//Define our system's maximum motion constraints
	//When in doubt, 
	//- set maxVelocity at a low value. 12 in/s is a great starter value 
	//- set kMaxAccelleration high, at roughly 4*kMaxVelocity
	private final TrapezoidProfile.Constraints constraints =
		new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration);
	//Create a PID that obeys those constraints in it's motion
	//Likely, you will have kI=0 and kD=0
	private final ProfiledPIDController controller =
		new ProfiledPIDController(kP, kI, kD, constraints, 0.02);
	//Our feedforward object
	//When in doubt, set these to zero (equivilent to no feedforward)
	//Will be calculated as part of our tuning.
	private final ElevatorFeedforward feedforward = 
		new ElevatorFeedforward(kS, kG, kV);
	
	ExampleElevator(){
		//These documented below to better demonstrate the general structure
		configMotor();
		configProfiledPID();
		//DefaultCommands help avoid leaving a system in an uncontrolled
		//state. For elevators, we often want no motion
		setDefaultCommand(stop());
		//For systems using an I gain, you likely want to 
		//command the PID controller at all times rather than use stop(), 
		//which behaves better with the I term's summing behavior.
		//holdPosition() is better than stop() for this use case.
	}
	
	///Prepare Telemetry for tuning
	public void periodic(){
		var output=motor.getAppliedOutput();
		// We cannot get the applied voltage directly, but since
		// it's a percentage we can multiply by the provided battery voltage
		var voltage=motor.getAppliedOutput()*RobotController.getBatteryVoltage();
		var height=motor.getEncoder().getPosition();
		var velocity=motor.getEncoder().getVelocity();
		var setpoint=controller.getSetpoint();
		
		SmartDashboard.putNumber("elevator/voltage",voltage);
		SmartDashboard.putNumber("elevator/height",height);
		SmartDashboard.putNumber("elevator/velocity",velocity);
		SmartDashboard.putNumber("elevator/set position",setpoint.position);
		SmartDashboard.putNumber("elevator/set velocity",setpoint.velocity);
	}

	//Triggers form the optimal way to transfer subsystem states/conditions
	//to external subsystems.
	//The PID contains this method, so we can just check it
	//ProfiledPID objects's other useful indicators of completion as well,
	//such as .timeRemaining for the intended motion.
	public Trigger isAtPosition = new Trigger(controller::atGoal);
	
	/// Return the height of our elevator, using Unit measures
	public Distance getHeight(){
		return Inches.of(motor.getEncoder().getPosition());
	}
	
	/// Provide manual output. Should only be used for
	/// debugging, tuning, and homing
	public Command setPower(DoubleSupplier percentOutput){
		//Including the feed-forward here, but can be removed
		//if your testing is easier without it.
		return run(()->motor.setVoltage(
			percentOutput.getAsDouble()*RobotController.getBatteryVoltage()
			+ feedforward.getKg()
			)
		); 
	}
	
	/// Disable the elevator's motion entirely
	public Command stop(){
		//Note, an output of 0 usually falls down, which is *not* the 
		//same as stopping! So, we instead use kg to resist gravity.
		//A "stopped" subsystem can still be moved by hand or 
		//external forces though!
		return setPower(feedforward::getKg)
	}
	
	///Command the Elevator to go to the target position
	public Command setHeight(Supplier<Distance> position){
		return run(()->{
			//Update our goal with any new targets
			controller.setGoal(position.get().in(Inches));
			//Calculate the voltage contributing to the output
			var vff = feedforward.calculate(controller.getSetpoint().velocity);
			var vpid = controller.calculate(motor.getEncoder().getPosition());
			//Apply to the motor
			motor.setVoltage(vff+vpid);
		});
	}
	
	/** Set the angle to a fixed value */
	public Command setHeight(Distance position){
		return setAngle(()->position);
	};
	
	/** Actively maintain the position command was started at */
	public Command holdPosition(){
		//Deferred commands are needed here to capture the angle
		//at the time the command is initiated
		return defer(()->setHeight(getHeight()));
	}

}

java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
// Important configuration parameters for an Elevator subsystem
void configMotor(){
	//These are the important ones for Elevators specifically
	SparkBaseConfig config = new SparkMaxConfig();
	//Default current limit should be low until the system is homed.
	//.. but this requires a homing process.
	config.smartCurrentLimit(40);
	//You want "forward" to be positive, with increasing encoder values
	config.inverted(false); 
	//Soft 
	config.softLimit
	.forwardSoftLimit(18) //Whatever max height is
	.forwardSoftLimitEnabled(true)
	.reverseSoftLimit(0) //Sometimes zero, or the lost attainable height
	.reverseSoftLimitEnabled(true);
	;
	//The conversion/gearing needed to put your system in a sensible
	// unit; Often Inches.
	var elevatorConversionfactor = 1;
	elevatorConfig.encoder
	.positionConversionFactor(elevatorConversionfactor)
	.velocityConversionFactor(elevatorConversionfactor / 60.0)
	;
	//Apply the configs
	elevatorMotor.configure(
		config,
		ResetMode.kResetSafeParameters,
		PersistMode.kPersistParameters
	);
}

void configProfiledPID(){
	//Configure the ProfiledPIDController
	
	//This value constraints the output added by the I term 
	//in the PID controller. This is very helpful to prevent damage in cases
	//where the elevator system jams or collides with the environment and 
	//cannot move.
	// This will be system dependent, and should (needs testing) be 
	// in PID ouptut units (normally Volts)
	controller.setIntegratorRange(-1, 1); //Configure to +/-1 volt
	
	//When the robot is disabled, the elevator might move, invalidating
	// the controller's internal state, and invalidating the I term's sum.
	//This needs to be reset whenever the robot is enabled, and a Trigger
	// provides a clean way of handling this
	new Trigger(DriverStation::isEnabled).onTrue(
		()->controller.reset(motor.getEncoder.getPosition())
	);
	
	//There's other methods for interacting with the PID values 
	// and constraints, should the need arise
}

java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53

private TrapezpoidProfile.Constraints constraints = 
new TrapezpoidProfile.Constraints(
	kElevatorMaxVelocity,
	kElevatorMaxAcceleration
);
private final TrapezoidProfile elevatorTrapezoidProfile = 
	new TrapezoidProfile(constraints);
	
//Needed to store the Final state of the motion so triggers can 
//calculate finished motion
TrapezoidProfile.State elevatorGoal=new TrapezoidProfile.State(0,0);
//We also need to retain the intermediate steps; When these match, 
//our planned motion has been completed.
TrapezoidProfile.State elevatorSetpoint=new TrapezoidProfile.State(0,0);

private Command setHeight(DoubleSupplier position){
	return new SequentialCommandGroup(
		//Seed the initial state/setpoint with the current state
		//Maybe optional?
		new InstantCommand( ()->{
			elevatorSetpoint = new TrapezoidProfile.State(
				getAngle().in(Degrees),
				elevatorMotor.getEncoder().getVelocity()
			);
		}),
		  
		new RunCommand(()->{
			//Make sure the goal is dynamically updated
			elevatorGoal = new TrapezoidProfile.State(
				position.getAsDouble(),
				0
			);
			//update our existing setpoint to the next achievable state
			elevatorSetpoint = elevatorTrapezoidProfile
				.calculate(0.02, elevatorSetpoint, elevatorGoal);
			//Query our feedforward for projected output
			var ff = elevatorFF.calculate(
				elevatorSetpoint.position,
				elevatorSetpoint.velocity
			);
			//Pass all our info out to the motor for output and PID control
			elevatorMotor.getClosedLoopController()
			.setReference(
				elevatorSetpoint.position,
				ControlType.kPosition, ClosedLoopSlot.kSlot0,
				ff, ArbFFUnits.kVoltage
			);
		})
	);
	}

}