Arm
What can an arm do?
An arm subsystem can accomplish many unique tasks, some of which include Climbing, Placing game pieces, and raising other subsystems. Arms commonly use Motors and encoders to rotate and keep track of rotation when in use.
Example Arm Code
public class Arm extends SubsystemBase {
public static TalonFXConfiguration GetArmMotorConfiguration() {
TalonFXConfiguration configs = new TalonFXConfiguration();
MotorOutputConfigs motorOutputConfigs = configs.MotorOutput;
motorOutputConfigs.NeutralMode = NeutralModeValue.Brake; //When output of the motor is neutral or disabled, Put the motor in brake mode.
return configs;
}
public static PIDController GetArmPID() {
PIDController pid = new PIDController(0.195, 0.015, 0);
pid.setTolerance(1); //Error that is considered Tolerable as the setpoint
pid.setIZone(Double.POSITIVE_INFINITY); //Disables Izone because value is infinity
pid.setIntegratorRange(-0.05, 0.05); //Limits I term to a minimum of -0.05 and maximum of 0.05
return pid;
}
private final TalonFX armRotation;
private final DutyCycleEncoder armRotationEncoder;
private final PIDController pid = GetArmPID();
public Arm() {
armRotation = new TalonFX(CANConstants.Arm.kArm);
armRotation.getConfigurator().apply(GetArmMotorConfiguration());
armRotationEncoder = new DutyCycleEncoder(DIOConstants.Arm.kArmRotateEncoder);
}
}
-
This code creates an TalonFX motor, Duty Cycle Encoder, and PID Controller, All of which are used to control an arm.
-
The TalonFX motor and DutyCycleEncoder both use ID's to differentiate themselves from other encoders / motors on a robot. In this case, the IDs are stored in seperate constants files for organization.
Setting Arm Limits
Arms usually have a limited range of motion when they are a part of a robot. However, the motor does not know this by default, and moving the arm further than its range would almost definitely break the robot or the arm. This can be combatted by setting an Arm rotation limit.
Example code
public void rotateOrHold(double power) {
SmartDashboard.putNumber("power to the arm", power);
if(Math.abs(power) < ArmConstants.kHoldThreshold) { // if there is no power to the controller, hold
hold();
} else if(getArmAngle() < 5 && power < 0) { // if you are falling and the angle is less than 5, then dont move
setOutput(0);
setArmHoldActive(false);
} else if (getArmAngle() > 90 && power > 0) { // if you are rising and the angle is greater than 90, then hold 90
hold(90);
} else { // else move using the output from the controller
setOutput(power);
setArmHoldActive(false);
}
}
- The
Hold
function sets the PID setpoint to the given angle.
public void hold(double angle) {
if(!getArmHoldActive()) {
setArmHoldAngle(angle);
setArmHoldActive(true);
setPIDTarget(getArmHoldAngle());
}
- The parameter
double Angle
is set as the Arm hold angle and is then set as the PID target for the arm.💡Look at all of the arm code from 201's 2024 robot here (opens in a new tab).
Arm Commands
To create a command for the arm, we implement the arm subsystem into the command to use the already created motor, encoder, PID controller, and functions.
Example code:
public class RotateArmToPosition extends Command {
/** Creates a new RotateArm. */
private final Arm c_arm;
private final DoubleSupplier c_angle;
private final boolean c_failure;
public RotateArmToPosition(Arm arm, DoubleSupplier desiredAngle) {
c_arm = arm;
c_angle = desiredAngle;
addRequirements(c_arm);
if(desiredAngle.getAsDouble() < -20) {
c_failure = true;
} else {
c_failure = false;
}
SmartDashboard.putBoolean("ARM FAILURE", c_failure);
// Use addRequirements() here to declare subsystem dependencies.
}
- This code creates a command class for
RotateArmToPosition
and uses thearm
subsystem. - It also uses a failsafe technique to prevent the arm from breaking. The code uses -20 as the threshold for
desiredAngle
beforec_failure
returns true. As long as thedesiredAngle
stays below -20, the command continues as normal.❗Failsafes and limits are based on encoder values, which are uniqe to each robot. Make sure to tune these values to each unique robot! - The
addRequirements(c_arm)
function requires the arm subsystem to end all other commands using it. This can help prevent breaking of the robot because it does not allow two commands to try to use the same subsystem.👀Note thatc_angle
is set to be equal todesiredAngle
.
Initialize, Excecute, and End
When a new Command class is created, you will see initialize
, excecute
, end
, and isFinished
functions. The excecute
funtion is where what the command will do happens. initialize
is for things that need to happen before the command, and end
is for things that need to happen before the command finishes. The isFinished
function determines when the command ends. It can determine this in itself by returning true, or when the command is interrupted.
Example Code
@Override
public void initialize() {
}
@Override
public void execute() {
}
@Override
public void end(boolean interrupted) {
}
@Override
public boolean isFinished() {
return false;
}
- The
initialize
,excecute
,end
, andisFinished
functions are shown here.
initialize, excecute, and end for RotateArmToPosition
// Called when the command is initially scheduled.
@Override
public void initialize() {
c_arm.setPIDTarget(c_angle.getAsDouble());
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(!c_failure) {
c_arm.rotateArmToTarget();
} else {
c_arm.setFailure(true);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
c_arm.rotateOrHold(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return c_failure || c_arm.isArmAtTarget();
}
RotateArmToPosition
Command uses all of the functions to complete its' purpose, but not all commands will have to. - In
initialize
, the arm's PID target is set toc_angle
. - The
excecute
function checks if faliure mode is true. If it is false, therotateArmToTarget
function is run which uses the arm PID to move it to the previously stated setpoint. If true,c_arm.setFailure
is set to true. - The
end
function sets the power ofrotateOrHold
to 0, keeping the arm at its setpoint. This is the same function that was detailed further up on this page. - Finally, the
isFinished
function ends the command either when faliure mode is true or when the arm reaches its' target.🤔Notice the use ofc_failure
as seen in therotateOrHold()
method, used to end the command.
Mapping commands to an Xbox controller
Now that our example command to move the arm is finished, we can move to RobotContainer
to use the command on a robot. RobotContainer
is where button bindings will be created, as well as adding autonomous paths. If all we wanted to do is move our arm, we could do it like this:
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.commands.arm.RotateArmToPosition;
import frc.robot.subsystems.arm.Arm;
public class RobotContainer {
private final Arm arm;
public final CommandXboxController operatorController;
}
public RobotContainer {
arm = new Arm();
operatorController = new CommandXboxController(2);
}
public void configureOperatorController{
operatorController.x(){
.onTrue(
new RotateArmToPosition(arm, () -> 30)
)
}
}
- First, we imported the
arm
subsystem,RotateArmToPosition
command, and Xbox controller libraries so they could be used inRobotContainer
. - Configuring the command to X on the
operatorController
was very simple, all that had to be done was pass in thearm
subsystem and desired angle. In this case, the desired angle was 30. - The () -> symbol is called a lambda function. it is used to create functions without a specific name. () defines the parameters the function takes. Because they are empty, the function doesn't take arguments. the -> is used to seperate perameters from the function body.
- Similar to a motor that needs an ID, an Xbox controller needs a port number. In this case, we set the port number to two. The port of the Xbox controller can be manipulated in the DriverStation.
💡Usually, a robot will be controlled by two controllers, an operator controller and a driver controller. The driver controller will usually be used for driving and functions that should be done while on the move. The operator controller is for operating all of the other subsystems. Make sure to work with the Drive team when binding buttons. The most important thing is that they are comfortable with the button bindings!
Conclusion
Now, our example arm code is all done. We created an arm subsystem to create a motor, encoder, PID controller, and functions for the arm. We made a RotateArmToPosition using the arm subsystem to move the arm to an angle. Then we bound the command to a button in RobotContainer and set the arm setpoint to 30 degrees. this is just one example of what can be done with an arm. Many more things are possible, so always make sure to explore your options when programming anything. Happy coding!
-
All example code was sampled from the FEDS 201 Stewie Repository (opens in a new tab).