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 the arm subsystem.
• It also uses a failsafe technique to prevent the arm from breaking. The code uses -20 as the threshold for desiredAngle before c_failure returns true. As long as the desiredAngle 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 that c_angle is set to be equal to desiredAngle.

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, and isFinished 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();
    }

• In initialize, the arm's PID target is set to c_angle.
• The excecute function checks if faliure mode is true. If it is false, the rotateArmToTarget 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 of rotateOrHold 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 of c_failure as seen in the rotateOrHold() 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 in RobotContainer.
• Configuring the command to X on the operatorController was very simple, all that had to be done was pass in the arm 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).