diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 3a7fa37..16a9819 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -12,10 +12,8 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; -import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -34,6 +32,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; import org.firstinspires.ftc.teamcode.pedroPathing.util.PIDFController; @@ -63,6 +62,7 @@ public class Follower { private DriveVectorScaler driveVectorScaler; private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; private Pose closestPose; @@ -122,7 +122,7 @@ public class Follower { private PIDFController largeDrivePIDF = new PIDFController(FollowerConstants.largeDrivePIDFCoefficients); - public static boolean drawStuff = true; + public static boolean drawOnDashboard = true; public static boolean useTranslational = true; public static boolean useCentripetal = true; public static boolean useHeading = true; @@ -189,7 +189,8 @@ public class Follower { accelerations.add(new Vector()); } calculateAveragedVelocityAndAcceleration(); - draw(); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); } /** @@ -427,6 +428,11 @@ public class Follower { */ public void update() { poseUpdater.update(); + + if (drawOnDashboard) { + dashboardPoseTracker.update(); + } + if (auto) { if (holdingPosition) { closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); @@ -495,14 +501,6 @@ public class Follower { motors.get(i).setPower(drivePowers[i]); } } - draw(); - } - - public void draw() { - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), getPose()); - FtcDashboard.getInstance().sendTelemetryPacket(packet); } /** @@ -893,14 +891,9 @@ public class Follower { telemetry.addData("velocity magnitude", getVelocity().getMagnitude()); telemetry.addData("velocity heading", getVelocity().getTheta()); telemetry.update(); - TelemetryPacket packet = new TelemetryPacket(); - //Draws the current target path that the robot would like to follow - packet.fieldOverlay().setStroke("#4CAF50"); - Drawing.drawRobot(packet.fieldOverlay(), new Pose(getPose().getX(), getPose().getY(), getPose().getHeading())); - //Draws the current path that the robot is following - packet.fieldOverlay().setStroke("#3F51B5"); - if (currentPath != null) Drawing.drawRobot(packet.fieldOverlay(), getPointFromPath(currentPath.getClosestPointTValue())); - FtcDashboard.getInstance().sendTelemetryPacket(packet); + if (drawOnDashboard) { + Drawing.drawDebug(this); + } } /** @@ -921,4 +914,22 @@ public class Follower { public double getTotalHeading() { return poseUpdater.getTotalHeading(); } + + /** + * This returns the current Path the Follower is following. This can be null. + * + * @return returns the current Path. + */ + public Path getCurrentPath() { + return currentPath; + } + + /** + * This returns the pose tracker for the robot to draw on the Dashboard. + * + * @return returns the pose tracker + */ + public DashboardPoseTracker getDashboardPoseTracker() { + return dashboardPoseTracker; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java index 3a10761..15501cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java @@ -35,10 +35,23 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod public static double ROBOT_WIDTH = 1; public static double ROBOT_LENGTH = 1; + /** + * This creates a new DriveEncoderLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ public DriveEncoderLocalizer(HardwareMap map) { this(map, new Pose()); } + /** + * This creates a new DriveEncoderLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { hardwareMap = map; @@ -62,26 +75,52 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod currentVelocity = new Pose(); } + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ @Override public Pose getPose() { return MathFunctions.addPoses(startPose, displacementPose); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ @Override public Pose getVelocity() { return currentVelocity.copy(); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ @Override public Vector getVelocityVector() { return currentVelocity.getVector(); } + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ @Override public void setStartPose(Pose setStart) { startPose = setStart; } + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ public void setPrevRotationMatrix(double heading) { prevRotationMatrix = new Matrix(3,3); prevRotationMatrix.set(0, 0, Math.cos(heading)); @@ -91,12 +130,23 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod prevRotationMatrix.set(2, 2, 1.0); } + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ @Override public void setPose(Pose setPose) { displacementPose = MathFunctions.subtractPoses(setPose, startPose); resetEncoders(); } + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders. Then, the robot's global change in position is calculated + * using the pose exponential method. + */ @Override public void update() { deltaTimeNano = timer.getElapsedTime(); @@ -130,6 +180,9 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod totalHeading += globalDeltas.get(2, 0); } + /** + * This updates the Encoders. + */ public void updateEncoders() { leftFront.update(); rightFront.update(); @@ -137,6 +190,9 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod rightRear.update(); } + /** + * This resets the Encoders. + */ public void resetEncoders() { leftFront.reset(); rightFront.reset(); @@ -144,6 +200,12 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod rightRear.reset(); } + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders. + * + * @return returns a Matrix containing the robot relative movement. + */ public Matrix getRobotDeltas() { Matrix returnMatrix = new Matrix(3,1); // x/forward movement @@ -155,18 +217,42 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod return returnMatrix; } + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ public double getTotalHeading() { return totalHeading; } + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ public double getForwardMultiplier() { return FORWARD_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ public double getLateralMultiplier() { return STRAFE_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ public double getTurningMultiplier() { return TURN_TICKS_TO_RADIANS; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java index 3f8f72f..573c4cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java @@ -5,7 +5,8 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; /** - * This is the Encoder class. + * This is the Encoder class. This tracks the position of a motor of class DcMotorEx. The motor + * must have an encoder attached. It can also get changes in position. * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 4/2/2024 @@ -18,16 +19,31 @@ public class Encoder { public final static double FORWARD = 1, REVERSE = -1; + /** + * This creates a new Encoder from a DcMotorEx. + * + * @param setMotor the motor this will be tracking + */ public Encoder(DcMotorEx setMotor) { motor = setMotor; multiplier = FORWARD; reset(); } + /** + * This sets the direction/multiplier of the Encoder. Setting 1 or -1 will make the Encoder track + * forward or in reverse, respectively. Any multiple of either one will scale the Encoder's output + * by that amount. + * + * @param setMultiplier the multiplier/direction to set + */ public void setDirection(double setMultiplier) { multiplier = setMultiplier; } + /** + * This resets the Encoder's position and the current and previous position in the code. + */ public void reset() { motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); previousPosition = motor.getCurrentPosition(); @@ -35,15 +51,32 @@ public class Encoder { motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } + /** + * This updates the Encoder's tracked current position and previous position. + */ public void update() { previousPosition = currentPosition; currentPosition = motor.getCurrentPosition(); } + /** + * This returns the multiplier/direction of the Encoder. + * + * @return returns the multiplier + */ public double getMultiplier() { return multiplier * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); } + /** + * This returns the change in position from the previous position to the current position. One + * important thing to note is that this encoder does not track velocity, only change in position. + * This is because I am using a pose exponential method of localization, which doesn't need the + * velocity of the encoders. Velocity of the robot is calculated in the localizer using an elapsed + * time timer there. + * + * @return returns the change in position of the Encoder + */ public double getDeltaPosition() { return getMultiplier() * (currentPosition - previousPosition); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java index 4aa7702..2fa2173 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java @@ -12,23 +12,78 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; */ public abstract class Localizer { + /** + * This returns the current pose estimate from the Localizer. + * + * @return returns the pose as a Pose object. + */ public abstract Pose getPose(); + /** + * This returns the current velocity estimate from the Localizer. + * + * @return returns the velocity as a Pose object. + */ public abstract Pose getVelocity(); + /** + * This returns the current velocity estimate from the Localizer as a Vector. + * + * @return returns the velocity as a Vector. + */ public abstract Vector getVelocityVector(); + /** + * This sets the start pose of the Localizer. Changing the start pose should move the robot as if + * all its previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ public abstract void setStartPose(Pose setStart); + /** + * This sets the current pose estimate of the Localizer. Changing this should just change the + * robot's current pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ public abstract void setPose(Pose setPose); + /** + * This calls an update to the Localizer, updating the current pose estimate and current velocity + * estimate. + */ public abstract void update(); + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ public abstract double getTotalHeading(); + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ public abstract double getForwardMultiplier(); + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ public abstract double getLateralMultiplier(); + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ public abstract double getTurningMultiplier(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java index a16a3db..c93ab7e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java @@ -14,22 +14,49 @@ import java.util.Arrays; public class Matrix { private double[][] matrix; + /** + * This creates a new Matrix of width and height 0. + */ public Matrix() { matrix = new double[0][0]; } + /** + * This creates a new Matrix with a specified width and height. + * + * @param rows the number of rows, or height + * @param columns the number of columns, or width + */ public Matrix(int rows, int columns) { matrix = new double[rows][columns]; } + /** + * This creates a new Matrix from a 2D matrix of doubles. Please only enter rectangular 2D + * Arrays of doubles or else things mess up. + * + * @param setMatrix the 2D Array of doubles + */ public Matrix(double[][] setMatrix) { setMatrix(setMatrix); } + /** + * This creates a new Matrix from another Matrix. + * + * @param setMatrix the Matrix input. + */ public Matrix(Matrix setMatrix) { setMatrix(setMatrix); } + /** + * This creates a copy of a 2D Array of doubles that references entirely new memory locations + * from the original 2D Array of doubles, so no issues with mutability. + * + * @param copyMatrix the 2D Array of doubles to copy + * @return returns a deep copy of the input Array + */ public static double[][] deepCopy(double[][] copyMatrix) { double[][] returnMatrix = new double[copyMatrix.length][copyMatrix[0].length]; for (int i = 0; i < copyMatrix.length; i++) { @@ -38,30 +65,72 @@ public class Matrix { return returnMatrix; } + /** + * This returns a deep copy of the 2D Array that this Matrix is based on. + * + * @return returns the 2D Array of doubles this Matrix is built on + */ public double[][] getMatrix() { return deepCopy(matrix); } + /** + * This returns a specified row of the Matrix in the form of an Array of doubles. + * + * @param row the index of the row to return + * @return returns the row of the Matrix specified + */ public double[] get(int row) { return Arrays.copyOf(matrix[row], matrix[row].length); } + /** + * This returns a specified element of the Matrix as a double. + * + * @param row the index of the row of the element + * @param column the index of the column of the element + * @return returns the element of the Matrix specified + */ public double get(int row, int column) { return get(row)[column]; } + /** + * This returns the number of rows of the Matrix, as determined by the length of the 2D Array. + * If the Matrix/2D Array is not rectangular, issues arise. + * + * @return returns the number of rows in the Matrix + */ public int getRows() { return matrix.length; } + /** + * This returns the number of columns of the Matrix, as determined by the length of the first Array + * in the 2D Array. If the Matrix/2D Array is not rectangular, issues arise. + * + * @return returns the number of columns in the Matrix + */ public int getColumns() { return matrix[0].length; } + /** + * This sets the 2D Array of this Matrix to a copy of the 2D Array of another Matrix. + * + * @param setMatrix the Matrix to copy from + * @return returns if the operation was successful + */ public boolean setMatrix(Matrix setMatrix) { return setMatrix(setMatrix.getMatrix()); } + /** + * This sets the 2D Array of this Matrix to a copy of a specified 2D Array. + * + * @param setMatrix the 2D Array to copy from + * @return returns if the operation was successful + */ public boolean setMatrix(double[][] setMatrix) { int columns = setMatrix[0].length; for (int i = 0; i < setMatrix.length; i++) { @@ -73,6 +142,13 @@ public class Matrix { return true; } + /** + * This sets a row of the Matrix to a copy of a specified Array of doubles. + * + * @param row the row to be written over + * @param input the Array input + * @return returns if the operation was successful + */ public boolean set(int row, double[] input) { if (input.length != getColumns()) { return false; @@ -81,11 +157,25 @@ public class Matrix { return true; } + /** + * This sets a specified element of the Matrix to an input value. + * + * @param row the index of the row of the specified element + * @param column the index of the column of the specified element + * @param input the input value + * @return returns if the operation was successful + */ public boolean set(int row, int column, double input) { matrix[row][column] = input; return true; } + /** + * This adds a Matrix to this Matrix. + * + * @param input the Matrix to add to this. Nothing will change in this Matrix + * @return returns if the operation was successful + */ public boolean add(Matrix input) { if (input.getRows() == getRows() && input.getColumns() == getColumns()) { for (int i = 0; i < getRows(); i++) { @@ -98,6 +188,12 @@ public class Matrix { return false; } + /** + * This subtracts a Matrix from this Matrix. + * + * @param input the Matrix to subtract from this. Nothing will change in this Matrix + * @return returns if the operation was successful + */ public boolean subtract(Matrix input) { if (input.getRows() == getRows() && input.getColumns() == getColumns()) { for (int i = 0; i < getRows(); i++) { @@ -110,6 +206,12 @@ public class Matrix { return false; } + /** + * This multiplies this Matrix with a scalar. + * + * @param scalar the scalar number + * @return returns if the operation was successful + */ public boolean scalarMultiply(double scalar) { for (int i = 0; i < getRows(); i++) { for (int j = 0; j < getColumns(); j++) { @@ -119,10 +221,21 @@ public class Matrix { return true; } + /** + * This multiplies the Matrix by -1, flipping the signs of all the elements within. + * + * @return returns if the operation was successful + */ public boolean flipSigns() { return scalarMultiply(-1); } + /** + * This multiplies a Matrix to this Matrix. + * + * @param input the Matrix to multiply to this. Nothing will change in this Matrix + * @return returns if the operation was successful + */ public boolean multiply(Matrix input) { if (getColumns() == input.getRows()) { Matrix product = new Matrix(getRows(), input.getColumns()); @@ -141,6 +254,14 @@ public class Matrix { return false; } + /** + * This multiplies a Matrix to another Matrix. This will not change any data in the two input + * Matrices. + * + * @param one the first Matrix to multiply. + * @param two the second Matrix to multiply + * @return returns if the operation was successful + */ public static Matrix multiply(Matrix one, Matrix two) { Matrix returnMatrix = new Matrix(one); if (returnMatrix.multiply(two)) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java index 667196c..d5ac720 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java @@ -17,92 +17,194 @@ public class Pose { private double y; private double heading; + /** + * This creates a new Pose from a x, y, and heading inputs. + * + * @param setX the initial x value + * @param setY the initial y value + * @param setHeading the initial heading value + */ public Pose(double setX, double setY, double setHeading) { setX(setX); setY(setY); setHeading(setHeading); } + /** + * This creates a new Pose from x and y inputs. The heading is set to 0. + * + * @param setX the initial x value + * @param setY the initial y value + */ public Pose(double setX, double setY) { this(setX, setY, 0); } + /** + * This creates a new Pose with no inputs and 0 for all values. + */ public Pose() { this(0,0,0); } + /** + * This sets the x value. + * + * @param set the x value + */ public void setX(double set) { x = set; } + /** + * This sets the y value. + * + * @param set the y value + */ public void setY(double set) { y = set; } + /** + * This sets the heading value. + * + * @param set the heading value + */ public void setHeading(double set) { heading = MathFunctions.normalizeAngle(set); } + /** + * This returns the x value. + * + * @return returns the x value + */ public double getX() { return x; } + /** + * This returns the y value. + * + * @return returns the y value + */ public double getY() { return y; } + /** + * This returns the heading value. + * + * @return returns the heading value + */ public double getHeading() { return heading; } + /** + * This returns the Pose as a Vector. Naturally, the heading data in the Pose cannot be included + * in the Vector. + * + * @return returns the Pose as a Vector + */ public Vector getVector() { Vector returnVector = new Vector(); returnVector.setOrthogonalComponents(x, y); return returnVector; } + /** + * This returns a new Vector with magnitude 1 pointing in the direction of the heading. + * + * @return returns a unit Vector in the direction of the heading + */ public Vector getHeadingVector() { return new Vector(1, heading); } + /** + * This adds all the values of an input Pose to this Pose. The input Pose's data will not be + * changed. + * + * @param pose the input Pose + */ public void add(Pose pose) { setX(x + pose.getX()); setY(y + pose.getY()); setHeading(heading + pose.getHeading()); } + /** + * This subtracts all the values of an input Pose from this Pose. The input Pose's data will not + * be changed. + * + * @param pose the input Pose + */ public void subtract(Pose pose) { setX(x - pose.getX()); setY(y - pose.getY()); setHeading(heading - pose.getHeading()); } + /** + * This multiplies all the values of this Pose by a scalar. + * + * @param scalar the scalar + */ public void scalarMultiply(double scalar) { setX(x * scalar); setY(y * scalar); setHeading(heading * scalar); } + /** + * This divides all the values of this Pose by a scalar. + * + * @param scalar the scalar + */ public void scalarDivide(double scalar) { setX(x / scalar); setY(y / scalar); setHeading(heading / scalar); } + /** + * This flips the signs of all values in this Pose by multiplying them by -1. Heading values are + * still normalized to be between 0 and 2 * pi in value. + */ public void flipSigns() { setX(-x); setY(-y); setHeading(-heading); } + /** + * This returns if a Pose is within a specified accuracy of this Pose in terms of x position, + * y position, and heading. + * + * @param pose the input Pose to check + * @param accuracy the specified accuracy necessary to return true + * @return returns if the input Pose is within the specified accuracy of this Pose + */ public boolean roughlyEquals(Pose pose, double accuracy) { return MathFunctions.roughlyEquals(x, pose.getX(), accuracy) && MathFunctions.roughlyEquals(y, pose.getY(), accuracy) && MathFunctions.roughlyEquals(MathFunctions.getSmallestAngleDifference(heading, pose.getHeading()), 0, accuracy); } + /** + * This checks if the input Pose is within 0.0001 in all values to this Pose. + * + * @param pose the input Pose + * @return returns if the input Pose is within 0.0001 of this Pose + */ public boolean roughlyEquals(Pose pose) { return roughlyEquals(pose, 0.0001); } + /** + * This creates a copy of this Pose that points to a new memory location. + * + * @return returns a deep copy of this Pose + */ public Pose copy() { return new Pose(getX(), getY(), getHeading()); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index a010f6a..d667f40 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -44,19 +44,29 @@ public class PoseUpdater { private long currentPoseTime; /** - * Creates a new PoseUpdater from a HardwareMap. + * Creates a new PoseUpdater from a HardwareMap and a Localizer. * * @param hardwareMap the HardwareMap + * @param localizer the Localizer */ - public PoseUpdater(HardwareMap hardwareMap) { + public PoseUpdater(HardwareMap hardwareMap, Localizer localizer) { this.hardwareMap = hardwareMap; for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } - // TODO: change this to your preferred localizer - localizer = new ThreeWheelLocalizer(hardwareMap); + this.localizer = localizer; + } + + /** + * Creates a new PoseUpdater from a HardwareMap. + * + * @param hardwareMap the HardwareMap + */ + public PoseUpdater(HardwareMap hardwareMap) { + // TODO: replace the second argument with your preferred localizer + this(hardwareMap, new ThreeWheelLocalizer(hardwareMap)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java index 5ad8cd1..19c6382 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java @@ -23,6 +23,12 @@ public class RRToPedroThreeWheelLocalizer extends Localizer { private Pose startPose; private Pose previousPose; + /** + * This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously + * used Road Runner localization system to the new Pedro Pathing localization system. + * + * @param hardwareMap the HardwareMap + */ public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { List lastTrackingEncPositions = new ArrayList<>(); List lastTrackingEncVels = new ArrayList<>(); @@ -32,18 +38,34 @@ public class RRToPedroThreeWheelLocalizer extends Localizer { startPose = new Pose(); previousPose = new Pose(); } + + /** + * This returns the current pose estimate as a Pose. + * + * @return returns the current pose estimate + */ @Override public Pose getPose() { Pose2d pose = localizer.getPoseEstimate(); return new Pose(pose.getX(), pose.getY(), pose.getHeading()); } + /** + * This returns the current velocity estimate as a Pose. + * + * @return returns the current velocity estimate + */ @Override public Pose getVelocity() { Pose2d pose = localizer.getPoseVelocity(); return new Pose(pose.getX(), pose.getY(), pose.getHeading()); } + /** + * This returns the current velocity estimate as a Vector. + * + * @return returns the current velocity estimate + */ @Override public Vector getVelocityVector() { Pose2d pose = localizer.getPoseVelocity(); @@ -52,6 +74,12 @@ public class RRToPedroThreeWheelLocalizer extends Localizer { return returnVector; } + /** + * This sets the start pose. Any movement of the robot is treated as a displacement from the + * start pose, so moving the start pose will move the current pose estimate the same amount. + * + * @param setStart the new start pose + */ @Override public void setStartPose(Pose setStart) { Pose oldStart = startPose; @@ -60,32 +88,70 @@ public class RRToPedroThreeWheelLocalizer extends Localizer { localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); } + /** + * This sets the current pose estimate. This has no effect on the start pose. + * + * @param setPose the new current pose estimate + */ @Override public void setPose(Pose setPose) { localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); } + /** + * This updates the total heading and previous pose estimate. Everything else is handled by the + * Road Runner localizer on its own, but updating this tells you how far the robot has really + * turned. + */ @Override public void update() { totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); previousPose = getPose(); } + /** + * This returns how far the robot has actually turned. + * + * @return returns the total angle turned, in degrees. + */ @Override public double getTotalHeading() { return totalHeading; } + /** + * This returns the forward multiplier of the Road Runner localizer, which converts from ticks + * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything + * multiplied together should be. If you do use that, then do be aware that the value returned is + * the product of the Road Runner ticks to inches and the x multiplier. + * + * @return returns the forward multiplier + */ @Override public double getForwardMultiplier() { return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; } + /** + * This returns the lateral multiplier of the Road Runner localizer, which converts from ticks + * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything + * multiplied together should be. If you do use that, then do be aware that the value returned is + * the product of the Road Runner ticks to inches and the y multiplier. + * + * @return returns the lateral multiplier + */ @Override public double getLateralMultiplier() { return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; } + /** + * This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist. + * There really isn't a point in tuning the turning for the Road Runner localizer. This will + * actually just return the average of the two other multipliers. + * + * @return returns the turning multiplier + */ @Override public double getTurningMultiplier() { return (getForwardMultiplier() + getLateralMultiplier()) / 2; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java index edbe337..24ea188 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java @@ -5,8 +5,15 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; /** - * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding - * slot's motor direction + * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a + * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected + * velocity counts and allow reversing independently of the corresponding slot's motor direction. + * + * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have + * import statements, so I'm not crediting myself as an author for this. + * + * @author Road Runner dev team + * @version 1.0, 5/9/2024 */ public class RoadRunnerEncoder { private final static int CPS_STEP = 0x10000; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java index 93d13bb..c2e9b94 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java @@ -31,7 +31,13 @@ import java.util.List; /** * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will - * be replaced with a custom localizer. + * be replaced with a custom localizer. I made some minor changes, so I'm crediting myself as an + * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to + * Pedro Pathing to avoid having imports. + * + * @author Road Runner dev team + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/9/2024 */ @Config public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java index cdbc35d..a7e6548 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java @@ -52,10 +52,23 @@ public class ThreeWheelLocalizer extends Localizer { public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5; + /** + * This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ public ThreeWheelLocalizer(HardwareMap map) { this(map, new Pose()); } + /** + * This creates a new ThreeWheelLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) { // TODO: replace these with your encoder positions leftEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); @@ -84,26 +97,52 @@ public class ThreeWheelLocalizer extends Localizer { resetEncoders(); } + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ @Override public Pose getPose() { return MathFunctions.addPoses(startPose, displacementPose); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ @Override public Pose getVelocity() { return currentVelocity.copy(); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ @Override public Vector getVelocityVector() { return currentVelocity.getVector(); } + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ @Override public void setStartPose(Pose setStart) { startPose = setStart; } + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ public void setPrevRotationMatrix(double heading) { prevRotationMatrix = new Matrix(3,3); prevRotationMatrix.set(0, 0, Math.cos(heading)); @@ -113,12 +152,23 @@ public class ThreeWheelLocalizer extends Localizer { prevRotationMatrix.set(2, 2, 1.0); } + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ @Override public void setPose(Pose setPose) { displacementPose = MathFunctions.subtractPoses(setPose, startPose); resetEncoders(); } + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders. Then, the robot's global change in position is calculated + * using the pose exponential method. + */ @Override public void update() { deltaTimeNano = timer.getElapsedTime(); @@ -152,18 +202,30 @@ public class ThreeWheelLocalizer extends Localizer { totalHeading += globalDeltas.get(2, 0); } + /** + * This updates the Encoders. + */ public void updateEncoders() { leftEncoder.update(); rightEncoder.update(); strafeEncoder.update(); } + /** + * This resets the Encoders. + */ public void resetEncoders() { leftEncoder.reset(); rightEncoder.reset(); strafeEncoder.reset(); } + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders. + * + * @return returns a Matrix containing the robot relative movement. + */ public Matrix getRobotDeltas() { Matrix returnMatrix = new Matrix(3,1); // x/forward movement @@ -175,18 +237,42 @@ public class ThreeWheelLocalizer extends Localizer { return returnMatrix; } + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ public double getTotalHeading() { return totalHeading; } + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ public double getForwardMultiplier() { return FORWARD_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ public double getLateralMultiplier() { return STRAFE_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ public double getTurningMultiplier() { return TURN_TICKS_TO_RADIANS; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java index 26e46bc..5c4e932 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java @@ -55,10 +55,23 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963; public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + /** + * This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) + * facing 0 heading. + * + * @param map the HardwareMap + */ public TwoWheelLocalizer(HardwareMap map) { this(map, new Pose()); } + /** + * This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose + * specifying the starting pose of the localizer. + * + * @param map the HardwareMap + * @param setStartPose the Pose to start from + */ public TwoWheelLocalizer(HardwareMap map, Pose setStartPose) { // TODO: replace these with your encoder positions forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); @@ -87,26 +100,52 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w deltaRadians = 0; } + /** + * This returns the current pose estimate. + * + * @return returns the current pose estimate as a Pose + */ @Override public Pose getPose() { return MathFunctions.addPoses(startPose, displacementPose); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Pose + */ @Override public Pose getVelocity() { return currentVelocity.copy(); } + /** + * This returns the current velocity estimate. + * + * @return returns the current velocity estimate as a Vector + */ @Override public Vector getVelocityVector() { return currentVelocity.getVector(); } + /** + * This sets the start pose. Changing the start pose should move the robot as if all its + * previous movements were displacing it from its new start pose. + * + * @param setStart the new start pose + */ @Override public void setStartPose(Pose setStart) { startPose = setStart; } + /** + * This sets the Matrix that contains the previous pose's heading rotation. + * + * @param heading the rotation of the Matrix + */ public void setPrevRotationMatrix(double heading) { prevRotationMatrix = new Matrix(3,3); prevRotationMatrix.set(0, 0, Math.cos(heading)); @@ -116,12 +155,23 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w prevRotationMatrix.set(2, 2, 1.0); } + /** + * This sets the current pose estimate. Changing this should just change the robot's current + * pose estimate, not anything to do with the start pose. + * + * @param setPose the new current pose estimate + */ @Override public void setPose(Pose setPose) { displacementPose = MathFunctions.subtractPoses(setPose, startPose); resetEncoders(); } + /** + * This updates the elapsed time timer that keeps track of time between updates, as well as the + * change position of the Encoders and the IMU readings. Then, the robot's global change in + * position is calculated using the pose exponential method. + */ @Override public void update() { deltaTimeNano = timer.getElapsedTime(); @@ -155,6 +205,9 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w totalHeading += globalDeltas.get(2, 0); } + /** + * This updates the Encoders as well as the IMU. + */ public void updateEncoders() { forwardEncoder.update(); strafeEncoder.update(); @@ -164,11 +217,20 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w previousIMUOrientation = currentIMUOrientation; } + /** + * This resets the Encoders. + */ public void resetEncoders() { forwardEncoder.reset(); strafeEncoder.reset(); } + /** + * This calculates the change in position from the perspective of the robot using information + * from the Encoders and IMU. + * + * @return returns a Matrix containing the robot relative movement. + */ public Matrix getRobotDeltas() { Matrix returnMatrix = new Matrix(3,1); // x/forward movement @@ -180,18 +242,42 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w return returnMatrix; } + /** + * This returns how far the robot has turned in radians, in a number not clamped between 0 and + * 2 * pi radians. This is used for some tuning things and nothing actually within the following. + * + * @return returns how far the robot has turned in total, in radians. + */ public double getTotalHeading() { return totalHeading; } + /** + * This returns the multiplier applied to forward movement measurement to convert from encoder + * ticks to inches. This is found empirically through a tuner. + * + * @return returns the forward ticks to inches multiplier + */ public double getForwardMultiplier() { return FORWARD_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to lateral/strafe movement measurement to convert from + * encoder ticks to inches. This is found empirically through a tuner. + * + * @return returns the lateral/strafe ticks to inches multiplier + */ public double getLateralMultiplier() { return STRAFE_TICKS_TO_INCHES; } + /** + * This returns the multiplier applied to turning movement measurement to convert from encoder + * ticks to radians. This is found empirically through a tuner. + * + * @return returns the turning ticks to radians multiplier + */ public double getTurningMultiplier() { return 1; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java index 525d4ad..ff59830 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java @@ -3,11 +3,14 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; /** * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the @@ -26,24 +29,44 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; @Autonomous(name = "Forward Localizer Tuner", group = "Autonomous Pathing Tuning") public class FowardTuner extends OpMode { private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; private Telemetry telemetryA; public static double DISTANCE = 30; + /** + * This initializes the PoseUpdater as well as the FTC Dashboard telemetry. + */ @Override public void init() { poseUpdater = new PoseUpdater(hardwareMap); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA.addLine("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); telemetryA.update(); + + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } + /** + * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the + * calculated multiplier and draws the robot. + */ @Override public void loop() { poseUpdater.update(); + telemetryA.addData("distance moved", poseUpdater.getPose().getX()); telemetryA.addLine("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getX() / poseUpdater.getLocalizer().getForwardMultiplier())); + telemetryA.update(); + + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java index 2995a2e..282577d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java @@ -3,11 +3,14 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; /** * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the @@ -26,24 +29,44 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; @Autonomous(name = "Lateral Localizer Tuner", group = "Autonomous Pathing Tuning") public class LateralTuner extends OpMode { private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; private Telemetry telemetryA; public static double DISTANCE = 30; + /** + * This initializes the PoseUpdater as well as the FTC Dashboard telemetry. + */ @Override public void init() { poseUpdater = new PoseUpdater(hardwareMap); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA.addLine("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); telemetryA.update(); + + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } + /** + * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the + * calculated multiplier and draws the robot. + */ @Override public void loop() { poseUpdater.update(); + telemetryA.addData("distance moved", poseUpdater.getPose().getY()); telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getX() / poseUpdater.getLocalizer().getLateralMultiplier())); + telemetryA.update(); + + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java index a6c19e3..103c0b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -13,6 +12,7 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigu import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; import java.util.Arrays; @@ -30,6 +30,7 @@ import java.util.List; @TeleOp(group = "Pedro Pathing Tuning", name = "Localization Test") public class LocalizationTest extends OpMode { private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; private Telemetry telemetryA; private DcMotorEx leftFront; @@ -38,10 +39,15 @@ public class LocalizationTest extends OpMode { private DcMotorEx rightRear; private List motors; + /** + * This initializes the PoseUpdater, the mecanum drive motors, and the FTC Dashboard telemetry. + */ @Override public void init() { poseUpdater = new PoseUpdater(hardwareMap); + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); @@ -67,20 +73,18 @@ public class LocalizationTest extends OpMode { + "allowing robot control through a basic mecanum drive on gamepad 1."); telemetryA.update(); - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), poseUpdater.getPose()); - FtcDashboard.getInstance().sendTelemetryPacket(packet); - } - - @Override - public void start() { - //poseUpdater.setPose(new Pose()); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } + /** + * This updates the robot's pose estimate, the simple mecanum drive, and updates the FTC + * Dashboard telemetry with the robot's position as well as draws the robot's position. + */ @Override public void loop() { poseUpdater.update(); + dashboardPoseTracker.update(); double y = -gamepad1.left_stick_y; // Remember, this is reversed! double x = gamepad1.left_stick_x; // this is strafing @@ -106,9 +110,8 @@ public class LocalizationTest extends OpMode { telemetryA.addData("total heading", poseUpdater.getTotalHeading()); telemetryA.update(); - TelemetryPacket packet = new TelemetryPacket(); - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), poseUpdater.getPose()); - FtcDashboard.getInstance().sendTelemetryPacket(packet); + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java index 4b2838d..1693990 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java @@ -3,11 +3,14 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; /** * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the @@ -26,24 +29,43 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; @Autonomous(name = "Turn Localizer Tuner", group = "Autonomous Pathing Tuning") public class TurnTuner extends OpMode { private PoseUpdater poseUpdater; + private DashboardPoseTracker dashboardPoseTracker; private Telemetry telemetryA; public static double ANGLE = 2 * Math.PI; + /** + * This initializes the PoseUpdater as well as the FTC Dashboard telemetry. + */ @Override public void init() { poseUpdater = new PoseUpdater(hardwareMap); + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA.addLine("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); telemetryA.update(); + + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } + /** + * This updates the robot's pose estimate, and updates the FTC Dashboard telemetry with the + * calculated multiplier and draws the robot. + */ @Override public void loop() { poseUpdater.update(); + telemetryA.addData("total angle", poseUpdater.getTotalHeading()); telemetryA.addLine("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); telemetryA.addData("multiplier", ANGLE / (poseUpdater.getTotalHeading() / poseUpdater.getLocalizer().getTurningMultiplier())); + + Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); + Drawing.drawRobot(poseUpdater.getPose(), "#4CAF50"); + Drawing.sendPacket(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java index 190bf8a..f5bb12a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java @@ -4,6 +4,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; import java.util.ArrayList; +import java.util.Arrays; /** * This is the BezierCurve class. This class handles the creation of Bezier curves, which are used @@ -28,6 +29,10 @@ public class BezierCurve { private final int APPROXIMATION_STEPS = FollowerConstants.APPROXIMATION_STEPS; + private final int DASHBOARD_DRAWING_APPROXIMATION_STEPS = 100; + + private double[][] dashboardDrawingPoints; + private double UNIT_TO_TIME; private double length; @@ -55,11 +60,7 @@ public class BezierCurve { } } this.controlPoints = controlPoints; - generateBezierCurve(); - length = approximateLength(); - UNIT_TO_TIME = 1/length; - endTangent.setOrthogonalComponents(controlPoints.get(controlPoints.size()-1).getX()-controlPoints.get(controlPoints.size()-2).getX(), controlPoints.get(controlPoints.size()-1).getY()-controlPoints.get(controlPoints.size()-2).getY()); - endTangent = MathFunctions.normalizeVector(endTangent); + initialize(); } /** @@ -80,11 +81,41 @@ public class BezierCurve { e.printStackTrace(); } } + initialize(); + } + + /** + * This handles most of the initialization of the BezierCurve that is called from the constructor. + */ + public void initialize() { generateBezierCurve(); length = approximateLength(); UNIT_TO_TIME = 1/length; - endTangent.setOrthogonalComponents(this.controlPoints.get(this.controlPoints.size()-1).getX()-this.controlPoints.get(this.controlPoints.size()-2).getX(), this.controlPoints.get(this.controlPoints.size()-1).getY()-this.controlPoints.get(this.controlPoints.size()-2).getY()); + endTangent.setOrthogonalComponents(controlPoints.get(controlPoints.size()-1).getX()-controlPoints.get(controlPoints.size()-2).getX(), controlPoints.get(controlPoints.size()-1).getY()-controlPoints.get(controlPoints.size()-2).getY()); endTangent = MathFunctions.normalizeVector(endTangent); + initializeDashboardDrawingPoints(); + } + + /** + * This creates the Array that holds the Points to draw on the Dashboard. + */ + public void initializeDashboardDrawingPoints() { + dashboardDrawingPoints = new double[2][DASHBOARD_DRAWING_APPROXIMATION_STEPS + 1]; + for (int i = 0; i <= DASHBOARD_DRAWING_APPROXIMATION_STEPS; i++) { + Point currentPoint = getPoint(i/(double) (DASHBOARD_DRAWING_APPROXIMATION_STEPS)); + dashboardDrawingPoints[0][i] = currentPoint.getX(); + dashboardDrawingPoints[1][i] = currentPoint.getY(); + } + } + + /** + * This returns a 2D Array of doubles containing the x and y positions of points to draw on FTC + * Dashboard. + * + * @return returns the 2D Array to draw on FTC Dashboard + */ + public double[][] getDashboardDrawingPoints() { + return dashboardDrawingPoints; } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java index c2a2c52..04c7979 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java @@ -37,6 +37,7 @@ public class BezierLine extends BezierCurve { length = approximateLength(); UNIT_TO_TIME = 1 / length; endTangent = MathFunctions.normalizeVector(getDerivative(1)); + super.initializeDashboardDrawingPoints(); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java index b97700b..cd2382e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java @@ -32,6 +32,7 @@ public class BezierPoint extends BezierCurve { super(); this.point = point; length = approximateLength(); + super.initializeDashboardDrawingPoints(); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java index 5a2f453..9d88d0a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java @@ -473,4 +473,14 @@ public class Path { public String pathType() { return curve.pathType(); } + + /** + * This returns a 2D Array of doubles containing the x and y positions of points to draw on FTC + * Dashboard. + * + * @return returns the 2D Array to draw on FTC Dashboard + */ + public double[][] getDashboardDrawingPoints() { + return curve.getDashboardDrawingPoints(); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java index 7322447..0da8df8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java @@ -172,4 +172,14 @@ public class Point { public double getY() { return y; } + + /** + * This creates a new Point with the same information as this Point, just pointing to a different + * memory location. In other words, a deep copy. + * + * @return returns a copy of this Point. + */ + public Point copy() { + return new Point(getX(), getY(), CARTESIAN); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index bf0c302..08dc365 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -116,7 +116,7 @@ public class FollowerConstants { // Centripetal force to power scaling // todo: there are currently issues with the centripetal force correction, so just don't use it for now // i will fix these in another commit soon - public static double centripetalScaling = 0.001; + public static double centripetalScaling = 0.0005; // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) // if not negative, then the robot thinks that its going to go faster under 0 power @@ -158,9 +158,9 @@ public class FollowerConstants { public static double pathEndTValueConstraint = 0.995; // When the Path is considered at its end parametrically, then the Follower has this many - // seconds to further correct by default. + // milliseconds to further correct by default. // This can be custom set for each Path. - public static double pathEndTimeoutConstraint = 1.5; + public static double pathEndTimeoutConstraint = 500; // This is how many steps the BezierCurve class uses to approximate the length of a BezierCurve. public static int APPROXIMATION_STEPS = 1000; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java new file mode 100644 index 0000000..250c7ce --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java @@ -0,0 +1,76 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; + +import java.util.ArrayList; + +/** + * This is the DashboardPoseTracker class. This tracks the pose history of the robot through a + * PoseUpdater, adding to the pose history at specified increments of time and storing the history + * for a specified length of time. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/13/2024 + */ +public class DashboardPoseTracker { + private double[] xPositions; + private double[] yPositions; + private PoseUpdater poseUpdater; + private long lastUpdateTime; + private final int TRACKING_LENGTH = 1000; + private final long UPDATE_TIME = 5; + private final int TRACKING_SIZE = TRACKING_LENGTH / (int) UPDATE_TIME; + + /** + * This creates a new DashboardPoseTracker from a PoseUpdater. + * + * @param poseUpdater the PoseUpdater + */ + public DashboardPoseTracker(PoseUpdater poseUpdater) { + this.poseUpdater = poseUpdater; + xPositions = new double[TRACKING_SIZE]; + yPositions = new double[TRACKING_SIZE]; + + for (int i = 0; i < TRACKING_SIZE; i++) { + xPositions[i] = poseUpdater.getPose().getX(); + yPositions[i] = poseUpdater.getPose().getY(); + } + + lastUpdateTime = System.currentTimeMillis() - UPDATE_TIME; + } + + /** + * This updates the DashboardPoseTracker. When the specified update time has passed from the last + * pose history log, another pose can be logged. The least recent log is also removed. + */ + public void update() { + if (System.currentTimeMillis() - lastUpdateTime > UPDATE_TIME) { + lastUpdateTime = System.currentTimeMillis(); + for (int i = TRACKING_SIZE - 1; i > 0; i--) { + xPositions[i] = xPositions[i - 1]; + yPositions[i] = yPositions[i - 1]; + } + xPositions[0] = poseUpdater.getPose().getX(); + yPositions[0] = poseUpdater.getPose().getY(); + } + } + + /** + * This returns the x positions of the pose history as an Array of doubles. + * + * @return returns the x positions of the pose history + */ + public double[] getXPositionsArray() { + return xPositions; + } + + /** + * This returns the y positions of the pose history as an Array of doubles. + * + * @return returns the y positions of the pose history + */ + public double[] getYPositionsArray() { + return yPositions; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java index 6d9af9c..9a2d956 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java @@ -1,20 +1,119 @@ package org.firstinspires.ftc.teamcode.pedroPathing.util; +import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -/* -* Drawing class needed for putting your robot onto the dashboard -* Edit the radius needed to align to your robot the best possible simulation represented +/** + * This is the Drawing class. It handles the drawing of stuff on FTC Dashboard, like the robot. + * + * @author Logan Nash + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/22/2024 */ public class Drawing { - private Drawing() {} + private static TelemetryPacket packet; - public static void drawRobot(Canvas c, Point t) { + /** + * This draws everything that will be used in the Follower's telemetryDebug() method. This takes + * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * @param follower + */ + public static void drawDebug(Follower follower) { + if (follower.getCurrentPath() != null) { + drawPath(follower.getCurrentPath(), "#3F51B5"); + Point closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); + drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), "#3F51B5"); + } + drawPoseHistory(follower.getDashboardPoseTracker(), "#4CAF50"); + drawRobot(follower.getPose(), "#4CAF50"); + sendPacket(); + } + + /** + * This adds instructions to the current packet to draw a robot at a specified Pose with a specified + * color. If no packet exists, then a new one is created. + * + * @param pose the Pose to draw the robot at + * @param color the color to draw the robot with + */ + public static void drawRobot(Pose pose, String color) { + if (packet == null) packet = new TelemetryPacket(); + + packet.fieldOverlay().setStroke(color); + Drawing.drawRobotOnCanvas(packet.fieldOverlay(), pose.copy()); + } + + /** + * This adds instructions to the current packet to draw a Path with a specified color. If no + * packet exists, then a new one is created. + * + * @param path the Path to draw + * @param color the color to draw the Path with + */ + public static void drawPath(Path path, String color) { + if (packet == null) packet = new TelemetryPacket(); + + packet.fieldOverlay().setStroke(color); + Drawing.drawPath(packet.fieldOverlay(), path.getDashboardDrawingPoints()); + } + + /** + * This adds instructions to the current packet to draw all the Paths in a PathChain with a + * specified color. If no packet exists, then a new one is created. + * + * @param pathChain the PathChain to draw + * @param color the color to draw the PathChain with + */ + public static void drawPath(PathChain pathChain, String color) { + for (int i = 0; i < pathChain.size(); i++) { + drawPath(pathChain.getPath(i), color); + } + } + + /** + * This adds instructions to the current packet to draw the pose history of the robot. If no + * packet exists, then a new one is created. + * + * @param poseTracker the DashboardPoseTracker to get the pose history from + * @param color the color to draw the pose history with + */ + public static void drawPoseHistory(DashboardPoseTracker poseTracker, String color) { + if (packet == null) packet = new TelemetryPacket(); + + packet.fieldOverlay().setStroke(color); + packet.fieldOverlay().strokePolyline(poseTracker.getXPositionsArray(), poseTracker.getYPositionsArray()); + } + + /** + * This tries to send the current packet to FTC Dashboard. + * + * @return returns if the operation was successful. + */ + public static boolean sendPacket() { + if (packet != null) { + FtcDashboard.getInstance().sendTelemetryPacket(packet); + packet = null; + return true; + } + return false; + } + + /** + * This draws a robot on the Dashboard at a specified Point. + * + * @param c the Canvas on the Dashboard on which this will draw + * @param t the Point to draw at + */ + public static void drawRobotOnCanvas(Canvas c, Point t) { final double ROBOT_RADIUS = 9; c.setStrokeWidth(1); @@ -26,7 +125,14 @@ public class Drawing { c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent()); } - public static void drawRobot(Canvas c, Pose t) { + /** + * This draws a robot on the Dashboard at a specified Pose. This is more useful for drawing the + * actual robot, since the Pose contains the direction the robot is facing as well as its position. + * + * @param c the Canvas on the Dashboard on which this will draw + * @param t the Pose to draw at + */ + public static void drawRobotOnCanvas(Canvas c, Pose t) { final double ROBOT_RADIUS = 9; c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); @@ -36,4 +142,14 @@ public class Drawing { double x2 = t.getX() + v.getXComponent(), y2 = t.getY() + v.getYComponent(); c.strokeLine(x1, y1, x2, y2); } + + /** + * This draws a Path on the Dashboard from a specified Array of Points. + * + * @param c the Canvas on the Dashboard on which this will draw + * @param points the Points to draw + */ + public static void drawPath(Canvas c, double[][] points) { + c.strokePolyline(points[0], points[1]); + } }