Code for Gear It Forward's 2023 robot, Zephyr.
- Written in Java
- Timed Robot Format
- Season tldr; Thank you, thank you so much for a wonderful season! Subdivision finalists is a fantastic result for the fantastic bot that we built this year. As always thank you to our amazing alliance partners, starting with 2506 and 5934 at the Midwest Regional, 111 and 5586 at 7 rivers, and 1727, 498, and 610 at Houston!
- VendorDeps:
- PhoenixProAnd5
- REVLib
- WPILibNewCommands
- PathPlanner
- If you have any questions, feel free to contact us at
- Team Email
- Lead Programmer Email
- GitHub - add questions as issues or whatever else you need
- Collector
- Motor System
- Pneumatic System
- Arm
- Angle Control
- Set Positions
- Telescoping Arm
- Arm telescoping
- Set Positions
- Elevator
- Elevator control
- Set Positions
- Swerve Drivetrain
- MK4 Swerve Modules
- Swerve Drivetrain Control
- Drive Modes
- LED Subsystem
- LED Control and Management
- Vision
- Vision Processing
- Vision Commands
- Spoiler
- Spoiler?
- Spoiler!
- Spoiler?
Our collector is made up of two elements - a pneumatic system to set the shape of the game piece carried and a
set of wheels to quickly intake said game piece.
public class CollectorWheels {
private static final DoubleSolenoid solenoid = new DoubleSolenoid( PneumaticsModuleType.REVPH, RobotMap.SOLENOID_COLLECTOR_FORWARD, RobotMap.SOLENOID_COLLECTOR_REVERSE);
private DoubleSolenoid.Value state = DoubleSolenoid.Value.kForward;
public void wheelsOut() {
state = DoubleSolenoid.Value.kReverse;
Robot.ledSubsystem.LEDWheelsOut();
}
public void wheelsIn() {
state = DoubleSolenoid.Value.kForward;
Robot.ledSubsystem.LEDWheelsIn();
}
}Primary Subsystems and Commands to reference:
- Collector.java
- CollectorWheels.java
- WheelsIn.java
- WheelsOut.java
- CollectorEject.java
- CollectorCollect.java
Our arm is a subsystem controlled by a single motor, used to control the angle. It allows us
to maximize the reach of the arm without having to compensate with telescoping distance and
collector eject speed. It also allows us to quickly store the arm away whenever on the go.
In other words, it's our primary wrist of motion.
public class Arm extends SubsystemBase {
public void move(double percent) {
if (Robot.oi.aux.getHID().getRightStickButton()) {
armMotor.configReverseSoftLimitThreshold(0);
} else {
armMotor.configReverseSoftLimitThreshold(Constants.Arm.MIN_POS);
}
// soft limits will keep the robot arm in allowable range
armMotor.set(percent);
}
public void PIDMove() {
armMotor.set(ControlMode.Position, armTargetPos);
}
}Primary Subsystems and Commands to reference:
Our telescoping arm is a subsystem controlled by a single motor, used to control the extension
of the arm. Whenever we need to place high, it requires a longer distance to be covered, both vertically
and horizontally - the telescoping arm takes care of the horizontal extension, while the vertical
can be taken care of by the angle of the arm and the elevator height.
public class TelescopingArm extends SubsystemBase {
public void setMotorSpeed(double percent) {
// do not allow for arm to go past soft limits
if ( (percent > 0 && telescopingMotor.getEncoder().getPosition() > Constants.TelescopingArm.MAX_POS ) ||
(percent < 0 && telescopingMotor.getEncoder().getPosition() < Constants.TelescopingArm.MIN_POS ) )
percent = 0;
telescopingMotor.set(percent);
}
public double getPosition() {
return telescopingMotor.getEncoder().getPosition();
}
}Primary Subsystems and Commands to reference:
Our elevator is a subsystem controlled by a single motor, used to lift the arm and the
telescoping arm for placement at any level, as well as collection from both the single and
double human player substations, primary movement method shown.
public class Elevator extends SubsystemBase {
public void move(double percent) {
elevatorMotor.set(ControlMode.PercentOutput, percent);
}
public void PIDHold() {
elevatorMotor.selectProfileSlot(1,0);
// the elevator needs a different kF when it is lower to the ground, otherwise it doesn't stay at the position
if( elevatorTargetPos < Constants.Elevator.PLACE_CUBE_MID_POS) {
elevatorMotor.config_kF(1, Constants.Elevator.F_HOLD_LOW);
}
else
elevatorMotor.config_kF(1, Constants.Elevator.F_HOLD);
elevatorMotor.set(ControlMode.Position, elevatorTargetPos); // closed loop position control
}
}Primary Subsystems and Commands to reference:
This year, we decided on using a swerve chassis to take advantage of its ability to strafe in the community, as well as to get around defence in the center of the field as fast as possible. We use SDS MK4's as our chosen swerve modules, configured with NEOs to turn, Falcons to drive, and CANCoders to track the direction of the wheel. Unlike other examples you may encounter, we opted not to use WPILibs tools to optimize the state, as we found it was far too slow and inaccurate on our bot. Instead, we found the fastest way to get to any given state on a disconnected 0->360 degree axis.
public class SwerveModuleMK4 {
/**
* Optimize the swerve module state by setting it to the closest equivalent vector
* @param original the original swerve module state
* @return the optimized swerve module state
*/
private SwerveModuleState optimizeState(SwerveModuleState original) {
// Compute all options for a setpoint
double position = getTurningHeading();
double setpoint = original.angle.getRadians();
double forward = setpoint + (2 * Math.PI);
double reverse = setpoint - (2 * Math.PI);
double antisetpoint = findRevAngle(setpoint);
double antiforward = antisetpoint + (2 * Math.PI);
double antireverse = antisetpoint - (2 * Math.PI);
// Find setpoint option with minimum distance
double[] alternatives = { forward, reverse, antisetpoint, antiforward, antireverse };
double min = setpoint;
double minDistance = getDistance(setpoint, position);
int minIndex = -1;
for (int i = 0; i < alternatives.length; i++) {
double dist = getDistance(alternatives[i], position);
if (dist < minDistance) {
min = alternatives[i];
minDistance = dist;
minIndex = i;
}
}
// Figure out the speed. Anti- directions should be negative.
double speed = original.speedMetersPerSecond;
if (minIndex > 1) {
speed *= -1;
}
return new SwerveModuleState(speed, new Rotation2d(min));
}
}We decoupled the SwerveModule class from the SwerveDrivetrain class due to the base SwerveModule code's
usability across multiple module types and motors.
In addition to what a normal Swerve Drive would entail, we also added OTF (on-the-fly) drive modes as "strats" that
our drivers could use to get around various situations (e.g. defence, opponent robots, etc.). We structured these as an enum so that
we could have a single selection of a preset list.
public enum drivePace {
COAST_FR(Constants.Drivetrain.COAST_SPEED_METERS_PER_SECOND, true),
COAST_RR(Constants.Drivetrain.COAST_SPEED_METERS_PER_SECOND, false),
SLOW_FR(Constants.Drivetrain.SLOW_SPEED_METERS_PER_SECOND, true),
SLOW_RR(Constants.Drivetrain.SLOW_SPEED_METERS_PER_SECOND, false),
BOOST_FR(Constants.Drivetrain.BOOST_SPEED_METERS_PER_SECOND, true),
BOOST_RR(Constants.Drivetrain.BOOST_SPEED_METERS_PER_SECOND, false);
private double value;
private boolean isFieldRelative;
drivePace(double value, boolean isFieldRelative) {
this.value = value;
this.isFieldRelative = isFieldRelative;
}
public double getValue() {
return this.value;
}
public boolean getIsFieldRelative() {
return this.isFieldRelative;
}
}For path generation during autonomous, we found out pretty early that the included TrajectoryGenerator class cannot
generate holonomic paths. Our solution was Team 3015 Ranger Robotics' fantastic PathPlanner application, allowing us to both
rotate and translate at the same time, just like in tele-op with field-relative instruction.
public class RobotTrajectory {
public PPSwerveControllerCommand baseSwerveCommand(PathPlannerTrajectory trajectory) {
return new PPSwerveControllerCommand(
trajectory,
Robot.swervetrain::getPose,
Constants.Drivetrain.DRIVE_KINEMATICS,
new PIDController(Constants.AutoConstants.PX_CONTROLLER, 0, 0),
new PIDController(Constants.AutoConstants.PY_CONTROLLER, 0, 0),
new PIDController(Constants.AutoConstants.P_THETA_CONTROLLER, 0, 0),
Robot.swervetrain::setModuleStates,
Robot.swervetrain
);
}
}Primary Subsystems and Commands to reference:
- SwerveModuleMK4.java
- SwerveDrivetrain.java
- drivePace.java
- EnableBoost.java
- DriveSwerve.java
- ResetHeading.java
- ResetWheels.java
For the first time in a long time, we've decided to put LEDs on our robot.
Primarily used for alerting the human player of what game piece the driver would like,
whether purple or yellow and vice versa, whenever the driver picks up a game piece,
the LEDs flash green to confirm collection. We set the value as a subsystem member,
then periodically call a method to set the member variables to the actual LEDs.
public class LEDSubsystem extends SubsystemBase {
public void setColors() {
for (int i = 0; i < RobotMap.HP_LEDS.length; i++) {
ledBuffer.setRGB(RobotMap.HP_LEDS[i],HP[0],HP[1], HP[2]);
led.setData(ledBuffer);
}
for (int j = 0; j < RobotMap.GAME_PIECE_LEDS.length; j++) {
ledBuffer.setRGB(RobotMap.GAME_PIECE_LEDS[j], GamePiece[0], GamePiece[1], GamePiece[2]);
led.setData(ledBuffer);
}
for (int k = 0; k < RobotMap.WHEEL_STATE_LEDS.length; k++) {
ledBuffer.setRGB(RobotMap.WHEEL_STATE_LEDS[k], WheelState[0], WheelState[1], WheelState[2]);
led.setData(ledBuffer);
}
}
public void setLEDHPColor(int r, int g, int b) {
HP[0] = r;
HP[1] = g;
HP[2] = b;
}
}Primary Subsystems and Commands to reference:
We've been using the Limelight for a few years now, and this year was no different, except for one thing - YOLO.
YOLO (You Only Look Once) is a neural network algorithm that can detect objects in real time, and we've been using it to detect and navigate to game pieces
from the double substation. To support YOLO, we need previous data, as well as, Neural Network acceleration to process.
Limelight kindly provides us with the previous data, and to support acceleration, we've used a Google Coral. Shown is navigating to auto collect a game piece.
public class LimeLightAutoCollect extends CommandBase {
@Override
public void execute() {
double rotationSpeed = 0;
if (count < 20) {
if (Robot.limelightHigh.hasTarget()) {
xOffset = Robot.limelightHigh.getXOffset();
rotationSpeed = (Math.abs(xOffset) < yTolerence) ? 0 : ((xOffset > 0) ? -0.4 : 0.4);
if (rotationSpeed == 0) {
count++;
} else {
count = 0;
}
}
ChassisSpeeds chassisSpeeds = new ChassisSpeeds(0,0,rotationSpeed); //turns towards xoffset and drives forward
SwerveModuleState[] moduleStates = Constants.Drivetrain.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds);
Robot.swervetrain.setModuleStates(moduleStates);
}
if (Robot.elevator.getPosition() > (Constants.Elevator.LOAD_FROM_DOUBLE_SUBSTATION_POS - 3 * Constants.Elevator.EL_TICKS_PER_INCH) &&
Robot.arm.getPosition() > (Constants.Arm.LOAD_FROM_DOUBLE_SUBSTATION_POS - 5 * Constants.Arm.TICKS_PER_DEGREE) &&
Robot.arm.getPosition() < (Constants.Arm.LOAD_FROM_DOUBLE_SUBSTATION_POS + 3 * Constants.Arm.TICKS_PER_DEGREE)) {
Robot.ledSubsystem.clearLEDGamePieceColor();
if (count == 20 && Robot.telescopingArm.getPosition() < Constants.TelescopingArm.MAX_POS) {
Robot.telescopingArm.setMotorSpeed(Constants.TelescopingArm.HIGH_VELOCITY);
} else {
Robot.telescopingArm.setMotorSpeed(0);
}
} else {
Robot.ledSubsystem.setLEDAutoAlignError();
}
}
}We can also auto align to the cone nodes using a retroreflective limelight pipeline,
referenced in LimeLightAutoAlign.java by moving left or right
at a constant velocity until the tx is about 0.
Primary Subsystems and Commands to reference:
Developer comment: Spoiler
Due to the limited velocity of our swervy boi relative to other bots,
we've decided to add a spoiler to our robot to increase our top speed by adding an approximate
25 tonnes of downforce on the straight. It also helps us to easily get up and balance during endgame.