Wrist

The first part of our intake subsystem is out wrist. Using the wrist motor and encoder, the end goal of this part is to be able to rotate the intake to angle setpoints using the wrist motor with feedback from the encoder. Below is example code for a basic wrist:

Example Code

package frc.robot.subsystems.intake;
 
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;
 
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.constants.CANConstants;
import frc.robot.constants.DIOConstants;
import frc.robot.constants.IntakeConstants;
 
public class Wrist extends SubsystemBase {
  private final CANSparkMax wristRotation;
  private final DutyCycleEncoder wristRotationEncoder;
 
  private final PIDController pid = IntakeConstants.WristPID.GetWristPID();
  private static double encoderAngle;
  private static double rawEncoderValue;
 
  /** Creates a new intake. */
  public Wrist() {
    wristRotation = new CANSparkMax(CANConstants.Intake.kIntakeWrist, MotorType.kBrushless);
    wristRotationEncoder = new DutyCycleEncoder(DIOConstants.Intake.kIntakeRotateEncoder);
 
    wristRotationEncoder.setPositionOffset(0);
  }
 
  @Override
  public void periodic() {
    wristRotationEncoder.setPositionOffset(0);
 
    /*Encoder returns 0 when disconnected
    Prevents wrist from suddenly jolting to a "zero" position*/
     if(!wristRotationEncoder.isConnected()) {
      wristRotation.setVoltage(0);
    }
 
    // This method will be called once per scheduler run
    writePeriodicOutputs();
  }
 
  @Override
  public void writePeriodicOutputs() {
    readWristEncoder();
  }
 
  public void readWristEncoder(){
    rawEncoderValue = wristRotationEncoder.get();
    encoderAngle = rawEncoderValue * 360;
  }
 
  // GETTERS
  public static double getEncoderValue() {
    return rawEncoderValue;
  }
 
  public double getTarget() {
    return pid.getSetpoint();
  }
 
 public boolean pidAtSetpoint() {
    return pid.atSetpoint();
  }
 public static double getEncoderAngle(){
    return encoderAngle;
 }
 
 
  // SETTERS
  public void setWristVoltage(double voltage) {
    wristRotation.set(voltage); 
  }
 
    public void setPIDTarget(double target) {
    pid.setSetpoint(target);
  }
 
    public void rotateWristPID() {
    double output = pid.calculate(getEncoderAngle());
    setWristVoltage(output);
  }
 
 
}

Code Explanation

let's now break down each part of this code and better understand what its doing:

Imports:

com.revrobotics.CANSparkMax: A class from the REV Robotics library (vendordep) that provides control over a brushless motor.
edu.wpi.first.math.controller.PIDController: Implements a PID controller, as seen previously on the Arm page.
edu.wpi.first.wpilibj.DutyCycleEncoder: A class that reads the position of an encoder. In this case, it's used to monitor the wrist's angle of rotation.
frc.robot.constants.CANConstants, DIOConstants, IntakeConstants: These constants hold values for motor IDs, encoder ports, and PID controller settings (defined elsewhere in the code).

Class Definition:

public class Wrist extends SubsystemBase {

The Wrist class extends SubsystemBase, meaning it is part of the robot's subsystem structure. A subsystem is a component of the robot (such as the intake, arm, or wheels) that is responsible for a specific function.

Instance Variables:

private final CANSparkMax wristRotation;
private final DutyCycleEncoder wristRotationEncoder;
private final PIDController pid = IntakeConstants.WristPID.GetWristPID();
private static double encoderAngle;
private static double rawEncoderValue;

wristRotation: A CANSparkMax object that controls the wrist motor using a brushless motor. This is the motor that rotates the wrist.
wristRotationEncoder: A DutyCycleEncoder that reads the wrist's rotational position to track the angle of the wrist.
pid: A PIDController used to control the wrist's rotation.
encoderAngle: A static variable to store the wrist's angle (calculated from the encoder's value).
rawEncoderValue: A static variable to store the raw value from the encoder, which will be used to calculate the angle.

🤔

Unlike the rest of this documentation, Wrist uses CANSparkMax motors instead of talonFX. This is another brushless motor used in FRC, and uses different libraries that are exemplified here.

Constructor:

public Wrist() {
  wristRotation = new CANSparkMax(CANConstants.Intake.kIntakeWrist, MotorType.kBrushless);
  wristRotationEncoder = new DutyCycleEncoder(DIOConstants.Intake.kIntakeRotateEncoder);
  wristRotationEncoder.setPositionOffset(0);
}

wristRotation: Initializes the wrist motor using the CAN ID defined in CANConstants.Intake.kIntakeWrist and sets it to a brushless motor type.
wristRotationEncoder: Initializes the encoder using the DIO port defined in DIOConstants.Intake.kIntakeRotateEncoder.
setPositionOffset(0): Resets the encoder's position to 0, ensuring that the encoder starts at a consistent reference position.

`periodic()` Method:

@Override
public void periodic() {
  wristRotationEncoder.setPositionOffset(0);
  if (!wristRotationEncoder.isConnected()) {
    wristRotation.setVoltage(0);
  }
  writePeriodicOutputs();
}

periodic(): This method is called periodically by the robot framework (every 20 milliseconds).
- wristRotationEncoder.setPositionOffset(0): Resets the encoder position offset to zero during every periodic call.
- if (!wristRotationEncoder.isConnected()): Checks if the encoder is connected. If not, it sets the wrist motor voltage to 0 to avoid erratic behavior due to a disconnected encoder.

`writePeriodicOutputs()` Method:

@Override
public void writePeriodicOutputs() {
  readWristEncoder();
}

This method calls readWristEncoder().

`readWristEncoder()` Method:

public void readWristEncoder(){
  rawEncoderValue = wristRotationEncoder.get();
  encoderAngle = rawEncoderValue * 360;
}

readWristEncoder(): This method reads the raw encoder value from wristRotationEncoder and calculates the wrist's angle.
- rawEncoderValue = wristRotationEncoder.get(): Gets the raw encoder value (Where 360 degrees of movement is equal to 1), representing the position of the wrist in its rotation.
- encoderAngle = rawEncoderValue * 360: Converts the raw encoder value to an angle in degrees (since the encoder value represents a full 360° rotation).

Getters:

public static double getEncoderValue() {
  return rawEncoderValue;
}
 
public double getTarget() {
  return pid.getSetpoint();
}
 
public boolean pidAtSetpoint() {
  return pid.atSetpoint();
}
 
public static double getEncoderAngle() {
  return encoderAngle;
}

getEncoderValue(): Returns the raw encoder value, which is a normalized value representing the wrist’s position.
getTarget(): Returns the target setpoint (the desired wrist angle) that the PID controller is trying to reach.
pidAtSetpoint(): Checks if the wrist is at the desired position (i.e., if the PID controller has reached the target setpoint).
getEncoderAngle(): Returns the calculated wrist angle in degrees.

Setters:

public void setWristVoltage(double voltage) {
  wristRotation.set(voltage); 
}
 
public void setPIDTarget(double target) {
  pid.setSetpoint(target);
}
 
public void rotateWristPID() {
  double output = pid.calculate(getEncoderAngle());
  setWristVoltage(output);
}

setWristVoltage(double voltage): Sets the wrist motor to a specific voltage. This controls the wrist's movement by providing power to the motor.
setPIDTarget(double target): Sets the target position (angle) for the wrist. The PID controller will attempt to move the wrist to this target.
rotateWristPID(): Calculates the output of the PID controller based on the current wrist angle and the target setpoint. The motor's voltage is adjusted accordingly to move the wrist toward the target position.

`RotateWristToPosition` Command

In the above code, several methods have been created to allow the wrist to move to a given position. The below code is an example command created from these methods to allow for the wrist to be moved to a certain position.

package frc.robot.commands.intake;
 
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.intake.Wrist;
 
public class RotateWristToPosition extends Command {
  
  private final Wrist c_intake;
  private final double c_target;
  private final boolean c_failure;
 
  public RotateWristToPosition(Wrist intake, double target) {
    c_intake = intake;
    c_target = target;
    addRequirements(c_intake);
 
    if(target < -20) {
      c_failure = true;
    } else {
      c_failure = false;
    }
    SmartDashboard.putBoolean("WRIST FAILURE", c_failure);
    // Use addRequirements() here to declare subsystem dependencies.
 
  }
 
  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    c_intake.setPIDTarget(c_target);
  }
 
  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    if(!c_failure) {
      c_intake.rotateWristPID();
    } else {
      c_intake.setWristVoltage(0.0);
    }
  }
 
  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    c_intake.setWristVoltage(0.0);
  }
 
  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return c_failure || c_intake.pidAtSetpoint();
    // return false;
  }
}

Code explanation

Instance Variables:

c_intake: A reference to the Wrist subsystem. This allows the command to control the wrist and interact with the motor.
c_target: The target position (in degrees or encoder units) the wrist should rotate to.
c_failure: A boolean flag that determines whether a failure condition is triggered. If the target is invalid (e.g., too small), movement is halted.

Constructor:

c_intake = intake: Initializes the reference to the Wrist subsystem.
c_target = target: Sets the target position for the wrist.
addRequirements(c_intake): Registers the Wrist subsystem as a requirement, ensuring no other command can control the wrist simultaneously.
Failure Condition: If the target is less than -20, it sets the c_failure flag to true. This condition represents an out-of-range or invalid position.
SmartDashboard.putBoolean("WRIST FAILURE", c_failure): Displays the failure condition on the SmartDashboard for real-time monitoring.

⚠️

Note that these failure thresholds are for the example only. Failure thresholds are unique to every robot.

Moving past the Constructor, lets break down the initialize, excecute, end, and isFinished functions

💡

For a more in-depth dive into these methods, see the Arm page.

`initialize()` Method:

c_intake.setPIDTarget(c_target): Sets the target position for the wrist in the PID controller.

`execute()` Method:

If no failure (c_failure == false):
- c_intake.rotateWristPID(): This method (As seen in the Wrist class) uses the PID controller to adjust the wrist motor’s voltage and rotate the wrist towards the target position.
**If a failure is detected:
- c_intake.setWristVoltage(0.0): This stops the wrist motor by setting the motor's voltage to 0, preventing any movement.

`end()` Method:

c_intake.setWristVoltage(0.0): Ensures that the wrist motor stops moving when the command ends.

`isFinished()` Method:

The command ends if:
- Failure condition (c_failure == true) is triggered.
- PID controller reaches the setpoint (i.e., the wrist reaches the target position). This is checked by c_intake.pidAtSetpoint().

Summary

In the above example code, we created A wrist class and command, both of which will be used later as a part of the intake subsystem.

Intake Overview IR Sensor

Wrist

Example Code

Code Explanation

Imports:

Class Definition:

Instance Variables:

Constructor:

periodic() Method:

writePeriodicOutputs() Method:

readWristEncoder() Method:

Getters:

Setters:

RotateWristToPosition Command

Code explanation

Instance Variables:

Constructor:

initialize() Method:

execute() Method:

end() Method:

isFinished() Method:

Summary

`periodic()` Method:

`writePeriodicOutputs()` Method:

`readWristEncoder()` Method:

`RotateWristToPosition` Command

`initialize()` Method:

`execute()` Method:

`end()` Method:

`isFinished()` Method: