SuperStructure Arm

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
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
public class ExampleArm 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. 30 deg/s is a great starter value 
	//- set kMaxAccelleration somewhat low, at roughly 2*kmaxVelocity
	private final TrapezoidProfile.Constraints constraints =
		new TrapezoidProfile.Constraints(0, 0);
	//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(0, 0, 0, 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 ArmFeedforward feedforward = 
		new ArmFeedforward(0, 0, 0);
	
	ExampleArm(){
		//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 angle=motor.getEncoder().getPosition();
		var velocity=motor.getEncoder().getVelocity();
		var setpoint=controller.getSetpoint();
		var goal=controller.getGoal();
		
		//Generally useful values
		SmartDashboard.putNumber("arm/output",output);
		SmartDashboard.putNumber("arm/voltage",voltage);
		SmartDashboard.putBoolean("arm/atSetpoint",controller.atSetpoint());
		SmartDashboard.putBoolean("arm/atGoal",controller.atGoal());
		//Our relevant Positional values
		SmartDashboard.putNumber("arm/angle/measured",angle);
		SmartDashboard.putNumber("arm/angle/setpoint",setpoint.position);
		SmartDashboard.putNumber("arm/angle/goal",goal.position);
		SmartDashboard.putNumber("arm/angle/error",
			controller.getPositionError());
		//Our relevant Velocity values
		SmartDashboard.putNumber("arm/velocity/measured",velocity);
		SmartDashboard.putNumber("arm/velocity/setpoint",setpoint.velocity);
		SmartDashboard.putNumber("arm/velocity/goal",goal.velocity);
		SmartDashboard.putNumber("arm/velocity/error",
			controller.getVelocityError());
	}

	//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 Angle getAngle(){
		return Degrees.of(motor.getEncoder().getPosition());
	}
	
	/** Provide manual output. Should generally only be used internally
	 * for debugging, other commands, or occasional special cases.
	 */
	private Command setPower(DoubleSupplier percentOutput){
		// Due to the non-linear effect of a fixed power on an arm, 
		// the feedforward with kg is critical for sensible control.
		return run(()->{
			var vpercent=
				percentOutput.getAsDouble()*RobotController.getBatteryVoltage();
			var vff = feedforward.calculate(getAngle().in(Radian), 0);	
			motor.setVoltage(vpercent+vff);
		});
	}
	
	/** Disable the elevator's motion entirely */
	public Command stop(){
		//Since we handle kG in setPower, a value of 0 represents "stop" effectively
		return setPower(()->0);
	}
	
	/** Command the Elevator to go to the target position */
	public Command setAngle(Supplier<Angle> position){
		return run(()->{
			//Update our goal with any new targets
			controller.setGoal(position.get().in(Degrees));
			//Calculate the voltage contributing to the output
			var vpid = controller.calculate(motor.getEncoder().getPosition());
			var vff = feedforward.calculateWithVelocities(
				motor.getEncoder().getPosition(),
				motor.getEncoder().getVelocity(),
				controller.getSetpoint().velocity);
			//Apply to the motor
			motor.setVoltage(vff+vpid);
		});
	}
	
	/** Set the angle to a fixed value */
	public Command setAngle(Angle 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(()->setAngle(getAngle()));
	}
}

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
// 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 angle is
	.forwardSoftLimitEnabled(true)
	.reverseSoftLimit(0) //Sometimes zero, or the lowest attainable angle
	.reverseSoftLimitEnabled(true);
	;
	//The conversion/gearing needed to put your system in a sensible
	// unit; Degrees is generaly preferable.
	// It's recommended to set this, even if you use the absolute encoder
	// as your primary input
	var conversionfactor = 1;
	config.encoder
	.positionConversionFactor(conversionfactor)
	.velocityConversionFactor(conversionfactor / 60.0)
	;
	//The absolute encoder will *also* need configuration. 
	//However, since it avoids gearing, it's generally easy
	config.absoluteEncoder
	.zeroCentered(false)// false ->0...360 range. True ->-180...180 range
	.positionConversionFactor(360) //Configure for Degrees
	.velocityConversionFactor(360/60)
	;
	
	//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
}