SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

added comments to all new files and increased FTC Dashboard field drawing functionality, will add a readme for the localization section

Browse Source
This commit is contained in:
brotherhobo
2024-05-13 23:15:20 -04:00
parent ccc74c8120
commit 168537cb28
24 changed files with 1042 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java
View File

@ -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;
}
}

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java
View File

@ -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;
}

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java
View File

@ -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);
}

55
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java
View File

@ -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();
}

121
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java
View File

@ -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)) {

102
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java
View File

@ -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());
}

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java
View File

@ -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));
}
/**

66
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java
View File

@ -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<Integer> lastTrackingEncPositions = new ArrayList<>();
List<Integer> 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;

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java
View File

@ -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;

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java
View File

@ -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 {

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java
View File

@ -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;
}

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java
View File

@ -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;
}

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java
View File

@ -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();
}
}

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java
View File

@ -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();
}
}

31
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java
View File

@ -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<DcMotorEx> 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();
}
}

22
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java
View File

@ -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();
}
}

43
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java
View File

@ -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;
}
/**

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierLine.java
View File

@ -37,6 +37,7 @@ public class BezierLine extends BezierCurve {
length = approximateLength();
UNIT_TO_TIME = 1 / length;
endTangent = MathFunctions.normalizeVector(getDerivative(1));
super.initializeDashboardDrawingPoints();
}
/**

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierPoint.java
View File

@ -32,6 +32,7 @@ public class BezierPoint extends BezierCurve {
super();
this.point = point;
length = approximateLength();
super.initializeDashboardDrawingPoints();
}
/**

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java
View File

@ -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();
}
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java
View File

@ -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);
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java
View File

@ -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;

76
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java Normal file
View File

@ -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;
}
}

128
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java
View File

@ -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]);
}
}