From 057f500220e3b3cb3edc2d0f67d9f5b640e0194c Mon Sep 17 00:00:00 2001 From: Kyle-Eldridge <113394349+Kyle-Eldridge@users.noreply.github.com> Date: Fri, 30 May 2025 10:04:15 -0700 Subject: [PATCH] Remove m_ from variables in old commands Also add comments. This makes the naming convention more consistent. --- .../frc/robot/commands/vision/AimAtTag.java | 36 ++++----- .../commands/vision/CalculateStdDevs.java | 60 +++++++------- .../frc/robot/commands/vision/ReturnData.java | 14 ++-- .../commands/vision/TestVisionDistance.java | 78 +++++++++---------- .../frc/robot/util/AngledElevatorSim.java | 3 + src/main/java/frc/robot/util/ClimbArmSim.java | 28 ++++--- .../java/frc/robot/util/Vision/Vision.java | 14 ++-- 7 files changed, 120 insertions(+), 113 deletions(-) diff --git a/src/main/java/frc/robot/commands/vision/AimAtTag.java b/src/main/java/frc/robot/commands/vision/AimAtTag.java index 50586e0..9473d32 100644 --- a/src/main/java/frc/robot/commands/vision/AimAtTag.java +++ b/src/main/java/frc/robot/commands/vision/AimAtTag.java @@ -12,22 +12,22 @@ import frc.robot.subsystems.drivetrain.Drivetrain; * 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); } @@ -38,7 +38,7 @@ public class AimAtTag extends Command { 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); @@ -47,8 +47,8 @@ public class AimAtTag extends Command { 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())); } /** @@ -56,15 +56,15 @@ public class AimAtTag extends Command { */ @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); } /** @@ -73,7 +73,7 @@ public class AimAtTag extends Command { */ @Override public void end(boolean interrupted) { - m_drive.stop(); + drive.stop(); } /** @@ -82,7 +82,7 @@ public class AimAtTag extends Command { */ @Override public boolean isFinished() { - return m_pid.atSetpoint(); + return pid.atSetpoint(); } } diff --git a/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java b/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java index eea25aa..203d46c 100644 --- a/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java +++ b/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java @@ -13,11 +13,11 @@ import frc.robot.util.Vision.Vision; * Calculates standard deviations for vision */ public class CalculateStdDevs extends Command { - private final Vision m_vision; - private ArrayList m_poses; - private int m_arrayLength; - private Timer m_endTimer; - private Drivetrain m_drive; + private final Vision vision; + private ArrayList poses; + private int arrayLength; + private Timer endTimer; + private Drivetrain drive; /** * Constructor for CalculateStdDevs @@ -25,10 +25,10 @@ public class CalculateStdDevs extends Command { * @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(); } /** @@ -38,9 +38,9 @@ public class CalculateStdDevs extends Command { 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(); + poses = new ArrayList(); - m_drive.setVisionEnabled(false); + drive.setVisionEnabled(false); } /** @@ -48,22 +48,22 @@ public class CalculateStdDevs extends Command { */ @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(); } } @@ -74,24 +74,24 @@ public class CalculateStdDevs extends Command { */ @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 @@ -102,7 +102,7 @@ public class CalculateStdDevs extends Command { // 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; @@ -119,6 +119,6 @@ public class CalculateStdDevs extends Command { */ @Override public boolean isFinished() { - return m_poses.size() >= m_arrayLength; + return poses.size() >= arrayLength; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/ReturnData.java b/src/main/java/frc/robot/commands/vision/ReturnData.java index ae65672..a01bae2 100644 --- a/src/main/java/frc/robot/commands/vision/ReturnData.java +++ b/src/main/java/frc/robot/commands/vision/ReturnData.java @@ -11,7 +11,7 @@ import frc.robot.util.Vision.Vision; * 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(); /** @@ -19,7 +19,7 @@ public class ReturnData extends Command{ * @param vision The vision */ public ReturnData(Vision vision){ - m_vision = vision; + this.vision = vision; } @Override @@ -34,16 +34,16 @@ public class ReturnData extends Command{ @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); diff --git a/src/main/java/frc/robot/commands/vision/TestVisionDistance.java b/src/main/java/frc/robot/commands/vision/TestVisionDistance.java index 1810c13..91e6c40 100644 --- a/src/main/java/frc/robot/commands/vision/TestVisionDistance.java +++ b/src/main/java/frc/robot/commands/vision/TestVisionDistance.java @@ -12,23 +12,23 @@ import frc.robot.util.Vision.Vision; * 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 @@ -38,9 +38,9 @@ public class TestVisionDistance extends Command { */ 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; } /** @@ -49,16 +49,16 @@ public class TestVisionDistance extends Command { @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; } /** @@ -66,30 +66,30 @@ public class TestVisionDistance extends Command { */ @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(); } } @@ -98,8 +98,8 @@ public class TestVisionDistance extends Command { */ @Override public void end(boolean interrupted) { - m_drive.setVisionEnabled(true); - m_drive.stop(); + drive.setVisionEnabled(true); + drive.stop(); } /** @@ -108,6 +108,6 @@ public class TestVisionDistance extends Command { */ @Override public boolean isFinished() { - return m_endTimer.hasElapsed(kEndDelay); + return endTimer.hasElapsed(END_DELAY); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/AngledElevatorSim.java b/src/main/java/frc/robot/util/AngledElevatorSim.java index 0334792..e81e48d 100644 --- a/src/main/java/frc/robot/util/AngledElevatorSim.java +++ b/src/main/java/frc/robot/util/AngledElevatorSim.java @@ -8,6 +8,9 @@ import edu.wpi.first.math.system.NumericalIntegration; 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; diff --git a/src/main/java/frc/robot/util/ClimbArmSim.java b/src/main/java/frc/robot/util/ClimbArmSim.java index 30cde9d..cd2b753 100644 --- a/src/main/java/frc/robot/util/ClimbArmSim.java +++ b/src/main/java/frc/robot/util/ClimbArmSim.java @@ -10,11 +10,15 @@ import edu.wpi.first.math.system.plant.DCMotor; 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; @@ -49,10 +53,10 @@ public class ClimbArmSim extends SingleJointedArmSim { 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; @@ -143,8 +147,8 @@ public class ClimbArmSim extends SingleJointedArmSim { NumericalIntegration.rkdp( (Matrix x, Matrix _u) -> { Matrix 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; @@ -155,10 +159,10 @@ public class ClimbArmSim extends SingleJointedArmSim { // 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; } diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java index c55b1db..9af6c4f 100644 --- a/src/main/java/frc/robot/util/Vision/Vision.java +++ b/src/main/java/frc/robot/util/Vision/Vision.java @@ -39,7 +39,7 @@ import frc.robot.util.MathUtils; // 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; @@ -64,14 +64,14 @@ public class Vision { */ public Vision(ArrayList> 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(); -- 2.39.5