Also add comments. This makes the naming convention more consistent.
* Aims the robot at the closest April tag
*/
public class AimAtTag extends Command {
- Drivetrain m_drive;
- PIDController m_pid;
+ private Drivetrain drive;
+ private PIDController pid;
/**
* Aims the robot at the closest April tag
* @param drive The drivetrain
*/
public AimAtTag(Drivetrain drive){
- m_drive = drive;
+ this.drive = drive;
// Copy drive PID and changetolerance
- m_pid = new PIDController(
- m_drive.getRotationController().getP(),
- m_drive.getRotationController().getI(),
- m_drive.getRotationController().getD()
+ pid = new PIDController(
+ drive.getRotationController().getP(),
+ drive.getRotationController().getI(),
+ drive.getRotationController().getD()
);
- m_pid.setTolerance(Units.degreesToRadians(1));
+ pid.setTolerance(Units.degreesToRadians(1));
addRequirements(drive);
}
public void initialize(){
double dist = Double.POSITIVE_INFINITY;
Translation2d closest = new Translation2d();
- Translation2d driveTranslation = m_drive.getPose().getTranslation();
+ Translation2d driveTranslation = drive.getPose().getTranslation();
for(AprilTag tag : FieldConstants.APRIL_TAGS){
Translation2d translation = tag.pose.toPose2d().getTranslation();
double dist2 = driveTranslation.getDistance(translation);
closest = translation;
}
}
- m_pid.reset();
- m_pid.setSetpoint(Math.atan2(closest.getY() - driveTranslation.getY(), closest.getX() - driveTranslation.getX()));
+ pid.reset();
+ pid.setSetpoint(Math.atan2(closest.getY() - driveTranslation.getY(), closest.getX() - driveTranslation.getX()));
}
/**
*/
@Override
public void execute() {
- double angle = m_drive.getPose().getRotation().getRadians();
+ double angle = drive.getPose().getRotation().getRadians();
// If the distance between the angles is more than 180 degrees, use an identical angle ±360 degrees
- if(angle - m_pid.getSetpoint() > Math.PI){
+ if(angle - pid.getSetpoint() > Math.PI){
angle -= 2*Math.PI;
- }else if(angle - m_pid.getSetpoint() < -Math.PI){
+ }else if(angle - pid.getSetpoint() < -Math.PI){
angle += 2*Math.PI;
}
- double speed = m_pid.calculate(angle);
- m_drive.drive(0, 0, speed, true, false);
+ double speed = pid.calculate(angle);
+ drive.drive(0, 0, speed, true, false);
}
/**
*/
@Override
public void end(boolean interrupted) {
- m_drive.stop();
+ drive.stop();
}
/**
*/
@Override
public boolean isFinished() {
- return m_pid.atSetpoint();
+ return pid.atSetpoint();
}
}
* Calculates standard deviations for vision
*/
public class CalculateStdDevs extends Command {
- private final Vision m_vision;
- private ArrayList<Pose2d> m_poses;
- private int m_arrayLength;
- private Timer m_endTimer;
- private Drivetrain m_drive;
+ private final Vision vision;
+ private ArrayList<Pose2d> poses;
+ private int arrayLength;
+ private Timer endTimer;
+ private Drivetrain drive;
/**
* Constructor for CalculateStdDevs
* @param vision The vision
*/
public CalculateStdDevs(int posesToUse, Vision vision, Drivetrain drive) {
- m_vision = vision;
- m_drive = drive;
- m_arrayLength = posesToUse;
- m_endTimer = new Timer();
+ this.vision = vision;
+ this.drive = drive;
+ arrayLength = posesToUse;
+ endTimer = new Timer();
}
/**
public void initialize() {
// create the ArrayList of poses to store
// an ArrayList prevents issues if the command ends early, and makes checking if the command has finished easy
-
m_poses = new ArrayList<Pose2d>();
+ poses = new ArrayList<Pose2d>();
- m_drive.setVisionEnabled(false);
+ drive.setVisionEnabled(false);
}
/**
*/
@Override
public void execute() {
- Pose2d pose = m_vision.getPose2d(m_drive.getPose());
+ Pose2d pose = vision.getPose2d(drive.getPose());
// If the pose exists, add it to the first open spot in the array
if (pose != null) {
// if we see a pose, reset the timer (it will be started the next time it doesn't get a pose)
- m_endTimer.stop();
- m_endTimer.reset();
+ endTimer.stop();
+ endTimer.reset();
// add the pose to our data
- m_poses.add(pose);
- if(m_poses.size()%10==0){
- System.out.printf("%.0f%% done\n", ((double)m_poses.size())/m_arrayLength * 100);
+ poses.add(pose);
+ if(poses.size()%10==0){
+ System.out.printf("%.0f%% done\n", ((double)poses.size())/arrayLength * 100);
}
} else {
- m_endTimer.start();
+ endTimer.start();
// If kStdDevCommandEndTime seconds have passed since it saw an April tag, stop the command
// Prevents it from running forever
- if (m_endTimer.hasElapsed(10)) {
+ if (endTimer.hasElapsed(10)) {
cancel();
}
}
*/
@Override
public void end(boolean interrupted) {
- m_drive.setVisionEnabled(true);
+ drive.setVisionEnabled(true);
// If the array is empty, don't try to calculate std devs
- if (m_poses.size() == 0) {
+ if (poses.size() == 0) {
System.out.println("There are no poses in the array\nTry again where the robot can see an April tag.");
return;
}
// create arrays of the poses by X, Y, and Rotation for calculations
- double[] xArray = new double[m_poses.size()];
- double[] yArray = new double[m_poses.size()];
- double[] rotArray = new double[m_poses.size()];
+ double[] xArray = new double[poses.size()];
+ double[] yArray = new double[poses.size()];
+ double[] rotArray = new double[poses.size()];
// copy the values into the arrays
- for (int i = 0; i < m_poses.size(); i++) {
- xArray[i] = m_poses.get(i).getX();
- yArray[i] = m_poses.get(i).getY();
- rotArray[i] = m_poses.get(i).getRotation().getRadians();
+ for (int i = 0; i < poses.size(); i++) {
+ xArray[i] = poses.get(i).getX();
+ yArray[i] = poses.get(i).getY();
+ rotArray[i] = poses.get(i).getRotation().getRadians();
}
// Calculate the standard deviations
// Find distance to tag
double distance;
try{
- distance = m_vision.getEstimatedPoses(m_drive.getPose()).get(0).targetsUsed.get(0).getBestCameraToTarget().getTranslation().getNorm();
+ distance = vision.getEstimatedPoses(drive.getPose()).get(0).targetsUsed.get(0).getBestCameraToTarget().getTranslation().getNorm();
}catch(Exception e){
System.out.println("Could not see a target");
distance = -1;
*/
@Override
public boolean isFinished() {
- return m_poses.size() >= m_arrayLength;
+ return poses.size() >= arrayLength;
}
}
\ No newline at end of file
* Adds data from object detection vision to SmartDashboard
*/
public class ReturnData extends Command{
- private final Vision m_vision;
+ private final Vision vision;
private final Timer timer = new Timer();
/**
* @param vision The vision
*/
public ReturnData(Vision vision){
- m_vision = vision;
+ this.vision = vision;
}
@Override
@Override
public void execute() {
if(timer.advanceIfElapsed(2)){
- double[] xOffset = m_vision.getHorizontalOffset();
- double[] yOffset = m_vision.getVerticalOffset();
- // long[] objectClass = m_vision.getDetectedObjectClass();
+ double[] xOffset = vision.getHorizontalOffset();
+ double[] yOffset = vision.getVerticalOffset();
+ // long[] objectClass = vision.getDetectedObjectClass();
// //put the offsets and area on SmartDashboard for testing
// SmartDashboard.putNumberArray("Object X offsets degrees", xOffset);
// SmartDashboard.putNumberArray("Object Y offsets degrees", yOffset);
- // SmartDashboard.putNumberArray("Object Distances", m_vision.getDistance());
+ // SmartDashboard.putNumberArray("Object Distances", vision.getDistance());
- DetectedObject bestGamePiece = m_vision.getBestGamePiece(Math.PI, false);
+ DetectedObject bestGamePiece = vision.getBestGamePiece(Math.PI, false);
if(bestGamePiece!=null){
// SmartDashboard.putString("Vision best game piece", bestGamePiece.toString());
System.out.println("\n\nBest game piece: "+bestGamePiece);
* Gathers data on the distance limits of the camera used for vision.
*/
public class TestVisionDistance extends Command {
- private final Drivetrain m_drive;
- private final Vision m_vision;
- private Translation2d m_visionStartTranslation, m_driveStartTranslation;
- private Pose2d m_currentPose = null;
- private double m_driveDistance;
- private double m_visionDistance;
+ private final Drivetrain drive;
+ private final Vision vision;
+ private Translation2d visionStartTranslation, driveStartTranslation;
+ private Pose2d currentPose = null;
+ private double driveDistance;
+ private double visionDistance;
- private double m_speed;
+ private double speed;
- private final Timer m_endTimer = new Timer();
- private final Timer m_printTimer = new Timer();
+ private final Timer endTimer = new Timer();
+ private final Timer printTimer = new Timer();
// How many seconds of not seeing april tag before ending the command
- private static final double kEndDelay = 0.25;
+ private static final double END_DELAY = 0.25;
// How many seconds between each data print
- private static final double kPrintDelay = 1;
+ private static final double PRINT_DELAY = 1;
/**
* Constructor for TestVisionDistance
*/
public TestVisionDistance(double speed, Drivetrain drive, Vision vision){
addRequirements(drive);
- m_drive = drive;
- m_speed = speed;
- m_vision = vision;
+ this.drive = drive;
+ this.speed = speed;
+ this.vision = vision;
}
/**
@Override
public void initialize() {
- m_endTimer.reset();
- m_printTimer.restart();
+ endTimer.reset();
+ printTimer.restart();
- m_drive.setVisionEnabled(false);
+ drive.setVisionEnabled(false);
- m_currentPose = m_vision.getPose2d(m_drive.getPose());
- m_visionStartTranslation = m_currentPose.getTranslation();
- m_driveStartTranslation = m_drive.getPose().getTranslation();
- m_driveDistance = 0;
- m_visionDistance = 0;
+ currentPose = vision.getPose2d(drive.getPose());
+ visionStartTranslation = currentPose.getTranslation();
+ driveStartTranslation = drive.getPose().getTranslation();
+ driveDistance = 0;
+ visionDistance = 0;
}
/**
*/
@Override
public void execute() {
- m_drive.drive(m_speed, 0, 0, false, false);
- Pose2d newestPose = m_vision.getPose2d(m_currentPose, m_drive.getPose());
+ drive.drive(speed, 0, 0, false, false);
+ Pose2d newestPose = vision.getPose2d(currentPose, drive.getPose());
// If the camera can see the apriltag
if (newestPose != null) {
//update current pose
- m_currentPose = newestPose;
+ currentPose = newestPose;
// reset the timer
- m_endTimer.reset();
- m_driveDistance = m_drive.getPose().getTranslation().getDistance(m_driveStartTranslation);
- m_visionDistance = m_currentPose.getTranslation().getDistance(m_visionStartTranslation);
- SmartDashboard.putNumber("Vision test drive distance", m_driveDistance);
- SmartDashboard.putNumber("Vision test vision distnace", m_visionDistance);
- SmartDashboard.putNumber("Vision test error", m_visionDistance - m_driveDistance);
- SmartDashboard.putNumber("Vision test % error", (m_visionDistance-m_driveDistance) / m_driveDistance * 100);
+ endTimer.reset();
+ driveDistance = drive.getPose().getTranslation().getDistance(driveStartTranslation);
+ visionDistance = currentPose.getTranslation().getDistance(visionStartTranslation);
+ SmartDashboard.putNumber("Vision test drive distance", driveDistance);
+ SmartDashboard.putNumber("Vision test vision distnace", visionDistance);
+ SmartDashboard.putNumber("Vision test error", visionDistance - driveDistance);
+ SmartDashboard.putNumber("Vision test % error", (visionDistance-driveDistance) / driveDistance * 100);
// If kPrintDelay seconds have passed, print the data
- if (m_printTimer.advanceIfElapsed(kPrintDelay)) {
+ if (printTimer.advanceIfElapsed(PRINT_DELAY)) {
System.out.printf("\nDrive dist: %.2f\nVision dist: %.2f\nError: %.2f\n %% error: %.2f\n",
- m_driveDistance, m_visionDistance,
- m_visionDistance-m_driveDistance, (m_visionDistance-m_driveDistance) / m_driveDistance * 100
+ driveDistance, visionDistance,
+ visionDistance-driveDistance, (visionDistance-driveDistance) / driveDistance * 100
);
}
} else {
- m_endTimer.start();
+ endTimer.start();
}
}
*/
@Override
public void end(boolean interrupted) {
- m_drive.setVisionEnabled(true);
- m_drive.stop();
+ drive.setVisionEnabled(true);
+ drive.stop();
}
/**
*/
@Override
public boolean isFinished() {
- return m_endTimer.hasElapsed(kEndDelay);
+ return endTimer.hasElapsed(END_DELAY);
}
}
\ No newline at end of file
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+/**
+ * Exactly the same as ElevatorSim, except it can be angled and have a constant force spring
+ */
public class AngledElevatorSim extends ElevatorSim {
private double angle;
private boolean simulateGravity;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+/**
+ * Similar to SingleJointedArmSim, except it simulates an upward normal force on the end of the arm equal to the weight of the robot
+ * Use setIsClimbing() to change whether this normal force should be simulated
+ */
public class ClimbArmSim extends SingleJointedArmSim {
- private boolean m_simulateGravity;
- private double m_armLenMeters;
- private double m_minAngle;
- private double m_maxAngle;
+ private boolean simulateGravity;
+ private double armLenMeters;
+ private double minAngle;
+ private double maxAngle;
private double mass;
private double momentOfInertia;
private boolean isClimbing;
double armMassKilograms,
double... measurementStdDevs) {
super(plant, gearbox, gearing, armLengthMeters, minAngleRads, maxAngleRads, simulateGravity, startingAngleRads, measurementStdDevs);
- m_armLenMeters = armLengthMeters;
- m_minAngle = minAngleRads;
- m_maxAngle = maxAngleRads;
- m_simulateGravity = simulateGravity;
+ armLenMeters = armLengthMeters;
+ minAngle = minAngleRads;
+ maxAngle = maxAngleRads;
+ this.simulateGravity = simulateGravity;
mass = robotMasKilograms;
momentOfInertia = 1.0/3.0 * armMassKilograms * armLengthMeters * armLengthMeters;
isClimbing = false;
NumericalIntegration.rkdp(
(Matrix<N2, N1> x, Matrix<N1, N1> _u) -> {
Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
- if (m_simulateGravity) {
- double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLenMeters + (isClimbing ? mass * 9.8 * Math.cos(x.get(0, 0)) * m_armLenMeters / momentOfInertia : 0);
+ if (simulateGravity) {
+ double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / armLenMeters + (isClimbing ? mass * 9.8 * Math.cos(x.get(0, 0)) * armLenMeters / momentOfInertia : 0);
xdot = xdot.plus(VecBuilder.fill(0, alphaGrav));
}
return xdot;
// We check for collision after updating xhat
if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
- return VecBuilder.fill(m_minAngle, 0);
+ return VecBuilder.fill(minAngle, 0);
}
if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
- return VecBuilder.fill(m_maxAngle, 0);
+ return VecBuilder.fill(maxAngle, 0);
}
return updatedXhat;
}
// Vision and it's commands are adapted from Iron Claw's FRC2023
public class Vision {
- private NetworkTable m_objectDetectionTable;
+ private NetworkTable objectDetectionTable;
private NetworkTableEntry xOffset;
private NetworkTableEntry yOffset;
*/
public Vision(ArrayList<Pair<String, Transform3d>> camList) {
// Initialize object_detection NetworkTable
- m_objectDetectionTable = NetworkTableInstance.getDefault().getTable("object_detection");
+ objectDetectionTable = NetworkTableInstance.getDefault().getTable("object_detection");
// From the object detection NetworkTable, get the entries
- objectDistance = m_objectDetectionTable.getEntry("distance");
- xOffset = m_objectDetectionTable.getEntry("x_offset");
- yOffset = m_objectDetectionTable.getEntry("y_offset");
- objectClass = m_objectDetectionTable.getEntry("class");
- cameraIndex = m_objectDetectionTable.getEntry("index");
+ objectDistance = objectDetectionTable.getEntry("distance");
+ xOffset = objectDetectionTable.getEntry("x_offset");
+ yOffset = objectDetectionTable.getEntry("y_offset");
+ objectClass = objectDetectionTable.getEntry("class");
+ cameraIndex = objectDetectionTable.getEntry("index");
// Start NetworkTables server
NetworkTableInstance.getDefault().startServer();