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

fixed centripetal force correction issues and added localizer stuff. an interface for the roadrunner localizer will come in the next commit

Browse Source
This commit is contained in:
brotherhobo
2024-05-09 21:40:36 -04:00
parent bdf183fe62
commit 55b4b6b7f1
19 changed files with 1431 additions and 284 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md
View File

@ -1,11 +1,11 @@
## Prerequisites
Obviously, you have to have a robot to use Pedro Pathing. Also, Pedro Pathing is only able to work
with omnidirectional drives, like mecanum drive. There is currently no support for swerve drives.
You must also have a localizer of some sort. Pedro Pathing currently has the three-wheel localizer
from Road Runner, but we have plans to create our own localizer soon. You will need to have your
localizer tuned before starting tuning Pedro Pathing, since Pedro Pathing needs to know where your
robot is at all times. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash) will help
a lot in tuning, and we have a slightly scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd).
You must also have a localizer of some sort. Pedro Pathing has both a three-wheel localizer and a
two-wheel localizer. You will need to have your localizer tuned before starting to tune PedroPathing.
Check out the tuning guide under the localization tab if you're planning on using one of the
localizers available in Pedro Pathing. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash)
will help a lot in tuning, and we have a slightly scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd).
One last thing to note is that Pedro Pathing operates in inches and radians.
## Tuning

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

@ -12,9 +12,10 @@ 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.roadrunner.geometry.Pose2d;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@ -22,6 +23,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierPoint;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
@ -29,8 +31,10 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathCallback;
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.Drawing;
import org.firstinspires.ftc.teamcode.pedroPathing.util.PIDFController;
import java.util.ArrayList;
@ -60,7 +64,7 @@ public class Follower {
private PoseUpdater poseUpdater;
private Pose2d closestPose;
private Pose closestPose;
private Path currentPath;
@ -118,6 +122,7 @@ public class Follower {
private PIDFController largeDrivePIDF = new PIDFController(FollowerConstants.largeDrivePIDFCoefficients);
public static boolean drawStuff = true;
public static boolean useTranslational = true;
public static boolean useCentripetal = true;
public static boolean useHeading = true;
@ -184,6 +189,7 @@ public class Follower {
accelerations.add(new Vector());
}
calculateAveragedVelocityAndAcceleration();
draw();
}
/**
@ -206,21 +212,34 @@ public class Follower {
}
}
/**
* This gets a Point from the current Path from a specified t-value.
*
* @return returns the Point.
*/
public Point getPointFromPath(double t) {
if (currentPath != null) {
return currentPath.getPoint(t);
} else {
return null;
}
}
/**
* This returns the current pose from the PoseUpdater.
*
* @return returns the pose
*/
public Pose2d getPose() {
public Pose getPose() {
return poseUpdater.getPose();
}
/**
* This sets the current pose in the PoseUpdater using Road Runner's setPoseEstimate() method.
* This sets the current pose in the PoseUpdater without using offsets.
*
* @param pose The pose to set the current pose to.
*/
public void setPose(Pose2d pose) {
public void setPose(Pose pose) {
poseUpdater.setPose(pose);
}
@ -256,7 +275,7 @@ public class Follower {
*
* @param pose the pose to set the starting pose to.
*/
public void setStartingPose(Pose2d pose) {
public void setStartingPose(Pose pose) {
poseUpdater.setStartingPose(pose);
}
@ -267,7 +286,7 @@ public class Follower {
*
* @param set The pose to set the current pose to.
*/
public void setCurrentPoseWithOffset(Pose2d set) {
public void setCurrentPoseWithOffset(Pose set) {
poseUpdater.setCurrentPoseWithOffset(set);
}
@ -327,7 +346,7 @@ public class Follower {
/**
* This resets all offsets set to the PoseUpdater. If you have reset your pose using the
* setCurrentPoseUsingOffset(Pose2d set) method, then your pose will be returned to what the
* setCurrentPoseUsingOffset(Pose set) method, then your pose will be returned to what the
* PoseUpdater thinks your pose would be, not the pose you reset to.
*/
public void resetOffset() {
@ -476,6 +495,14 @@ 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);
}
/**
@ -757,19 +784,18 @@ public class Follower {
* @return returns the centripetal force correction vector.
*/
public Vector getCentripetalForceCorrection() {
return new Vector();
// if (!useCentripetal) return new Vector();
// double curvature;
// if (auto) {
// curvature = currentPath.getClosestPointCurvature();
// } else {
// double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent();
// double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent();
// curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime);
// }
// if (Double.isNaN(curvature)) return new Vector();
// centripetalVector = new Vector(MathFunctions.clamp(Math.abs(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature), -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta()));
// return centripetalVector;
if (!useCentripetal) return new Vector();
double curvature;
if (auto) {
curvature = currentPath.getClosestPointCurvature();
} else {
double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent();
double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent();
curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime);
}
if (Double.isNaN(curvature)) return new Vector();
centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta()));
return centripetalVector;
}
/**
@ -779,7 +805,7 @@ public class Follower {
*
* @return returns the closest pose.
*/
public Pose2d getClosestPose() {
public Pose getClosestPose() {
return closestPose;
}
@ -863,9 +889,18 @@ public class Follower {
telemetry.addData("x", getPose().getX());
telemetry.addData("y", getPose().getY());
telemetry.addData("heading", getPose().getHeading());
telemetry.addData("total heading", poseUpdater.getTotalHeading());
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);
}
/**
@ -877,4 +912,13 @@ public class Follower {
public void telemetryDebug(Telemetry telemetry) {
telemetryDebug(new MultipleTelemetry(telemetry));
}
/**
* This returns the total number of radians the robot has turned.
*
* @return the total heading.
*/
public double getTotalHeading() {
return poseUpdater.getTotalHeading();
}
}

173
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java Normal file
View File

@ -0,0 +1,173 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
/**
* This is the DriveEncoderLocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the drive encoder set up.
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 4/2/2024
*/
@Config
public class DriveEncoderLocalizer extends Localizer { // todo: make drive encoders work
private HardwareMap hardwareMap;
private Pose startPose;
private Pose displacementPose;
private Pose currentVelocity;
private Matrix prevRotationMatrix;
private NanoTimer timer;
private long deltaTimeNano;
private Encoder leftFront;
private Encoder rightFront;
private Encoder leftRear;
private Encoder rightRear;
private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = 1;
public static double STRAFE_TICKS_TO_INCHES = 1;
public static double TURN_TICKS_TO_RADIANS = 1;
public static double ROBOT_WIDTH = 1;
public static double ROBOT_LENGTH = 1;
public DriveEncoderLocalizer(HardwareMap map) {
this(map, new Pose());
}
public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) {
hardwareMap = map;
// TODO: replace these with your encoder ports
leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront"));
rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear"));
// TODO: reverse any encoders necessary
leftFront.setDirection(Encoder.REVERSE);
rightRear.setDirection(Encoder.REVERSE);
leftRear.setDirection(Encoder.FORWARD);
rightRear.setDirection(Encoder.FORWARD);
setStartPose(setStartPose);
timer = new NanoTimer();
deltaTimeNano = 1;
displacementPose = new Pose();
currentVelocity = new Pose();
}
@Override
public Pose getPose() {
return MathFunctions.addPoses(startPose, displacementPose);
}
@Override
public Pose getVelocity() {
return currentVelocity.copy();
}
@Override
public Vector getVelocityVector() {
return currentVelocity.getVector();
}
@Override
public void setStartPose(Pose setStart) {
startPose = setStart;
}
public void setPrevRotationMatrix(double heading) {
prevRotationMatrix = new Matrix(3,3);
prevRotationMatrix.set(0, 0, Math.cos(heading));
prevRotationMatrix.set(0, 1, -Math.sin(heading));
prevRotationMatrix.set(1, 0, Math.sin(heading));
prevRotationMatrix.set(1, 1, Math.cos(heading));
prevRotationMatrix.set(2, 2, 1.0);
}
@Override
public void setPose(Pose setPose) {
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
resetEncoders();
}
@Override
public void update() {
deltaTimeNano = timer.getElapsedTime();
timer.resetTimer();
updateEncoders();
Matrix robotDeltas = getRobotDeltas();
Matrix globalDeltas;
setPrevRotationMatrix(getPose().getHeading());
Matrix transformation = new Matrix(3,3);
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(2, 2, 1.0);
} else {
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(2, 2, 1.0);
}
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
totalHeading += globalDeltas.get(2, 0);
}
public void updateEncoders() {
leftFront.update();
rightFront.update();
leftRear.update();
rightRear.update();
}
public void resetEncoders() {
leftFront.reset();
rightFront.reset();
leftRear.reset();
rightRear.reset();
}
public Matrix getRobotDeltas() {
Matrix returnMatrix = new Matrix(3,1);
// x/forward movement
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition()));
//y/strafe movement
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition()));
// theta/turning
returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH)));
return returnMatrix;
}
public double getTotalHeading() {
return totalHeading;
}
public double getForwardMultiplier() {
return FORWARD_TICKS_TO_INCHES;
}
public double getLateralMultiplier() {
return STRAFE_TICKS_TO_INCHES;
}
public double getTurningMultiplier() {
return TURN_TICKS_TO_RADIANS;
}
}

50
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java Normal file
View File

@ -0,0 +1,50 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
/**
* This is the Encoder class.
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 4/2/2024
*/
public class Encoder {
private DcMotorEx motor;
private double previousPosition;
private double currentPosition;
private double multiplier;
public final static double FORWARD = 1, REVERSE = -1;
public Encoder(DcMotorEx setMotor) {
motor = setMotor;
multiplier = FORWARD;
reset();
}
public void setDirection(double setMultiplier) {
multiplier = setMultiplier;
}
public void reset() {
motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
previousPosition = motor.getCurrentPosition();
currentPosition = motor.getCurrentPosition();
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
public void update() {
previousPosition = currentPosition;
currentPosition = motor.getCurrentPosition();
}
public double getMultiplier() {
return multiplier * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
}
public double getDeltaPosition() {
return getMultiplier() * (currentPosition - previousPosition);
}
}

152
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java Normal file
View File

@ -0,0 +1,152 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization;
import java.util.Arrays;
/**
* This is the Matrix class. This defines matrices, primarily for use in the localizers. However, if
* matrices and matrix operations are necessary, this class as well as some operations in the
* MathFunctions class can absolutely be used there as well. It's similar to Mats in OpenCV if you've
* used them before, but with more limited functionality.
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 4/2/2024
*/
public class Matrix {
private double[][] matrix;
public Matrix() {
matrix = new double[0][0];
}
public Matrix(int rows, int columns) {
matrix = new double[rows][columns];
}
public Matrix(double[][] setMatrix) {
setMatrix(setMatrix);
}
public Matrix(Matrix setMatrix) {
setMatrix(setMatrix);
}
public static double[][] deepCopy(double[][] copyMatrix) {
double[][] returnMatrix = new double[copyMatrix.length][copyMatrix[0].length];
for (int i = 0; i < copyMatrix.length; i++) {
returnMatrix[i] = Arrays.copyOf(copyMatrix[i], copyMatrix[i].length);
}
return returnMatrix;
}
public double[][] getMatrix() {
return deepCopy(matrix);
}
public double[] get(int row) {
return Arrays.copyOf(matrix[row], matrix[row].length);
}
public double get(int row, int column) {
return get(row)[column];
}
public int getRows() {
return matrix.length;
}
public int getColumns() {
return matrix[0].length;
}
public boolean setMatrix(Matrix setMatrix) {
return setMatrix(setMatrix.getMatrix());
}
public boolean setMatrix(double[][] setMatrix) {
int columns = setMatrix[0].length;
for (int i = 0; i < setMatrix.length; i++) {
if (setMatrix[i].length != columns) {
return false;
}
}
matrix = deepCopy(setMatrix);
return true;
}
public boolean set(int row, double[] input) {
if (input.length != getColumns()) {
return false;
}
matrix[row] = Arrays.copyOf(input, input.length);
return true;
}
public boolean set(int row, int column, double input) {
matrix[row][column] = input;
return true;
}
public boolean add(Matrix input) {
if (input.getRows() == getRows() && input.getColumns() == getColumns()) {
for (int i = 0; i < getRows(); i++) {
for (int j = 0; j < getColumns(); j++) {
set(i, j, get(i,j) + input.get(i,j));
}
}
return true;
}
return false;
}
public boolean subtract(Matrix input) {
if (input.getRows() == getRows() && input.getColumns() == getColumns()) {
for (int i = 0; i < getRows(); i++) {
for (int j = 0; j < getColumns(); j++) {
set(i, j, get(i,j) - input.get(i,j));
}
}
return true;
}
return false;
}
public boolean scalarMultiply(double scalar) {
for (int i = 0; i < getRows(); i++) {
for (int j = 0; j < getColumns(); j++) {
set(i, j, scalar * get(i,j));
}
}
return true;
}
public boolean flipSigns() {
return scalarMultiply(-1);
}
public boolean multiply(Matrix input) {
if (getColumns() == input.getRows()) {
Matrix product = new Matrix(getRows(), input.getColumns());
for (int i = 0; i < product.getRows(); i++) {
for (int j = 0; j < product.getColumns(); j++) {
double value = 0;
for (int k = 0; k < get(i).length; k++) {
value += get(i, k) * input.get(k, j);
}
product.set(i, j, value);
}
}
setMatrix(product);
return true;
}
return false;
}
public static Matrix multiply(Matrix one, Matrix two) {
Matrix returnMatrix = new Matrix(one);
if (returnMatrix.multiply(two)) {
return returnMatrix;
} else {
return new Matrix();
}
}
}

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

@ -1,6 +1,5 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
@ -9,9 +8,6 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
import java.util.ArrayList;
import java.util.List;
/**
* This is the PoseUpdater class. This class handles getting pose data from the localizer and returning
* the information in a useful way to the Follower.
@ -26,13 +22,13 @@ public class PoseUpdater {
private IMU imu;
private ThreeWheelLocalizer localizer;
private Localizer localizer;
private Pose2d startingPose = new Pose2d(0,0,0);
private Pose startingPose = new Pose(0,0,0);
private Pose2d currentPose = startingPose;
private Pose currentPose = startingPose;
private Pose2d previousPose = startingPose;
private Pose previousPose = startingPose;
private Vector currentVelocity = new Vector();
@ -59,9 +55,8 @@ public class PoseUpdater {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
List<Integer> lastTrackingEncPositions = new ArrayList<>();
List<Integer> lastTrackingEncVels = new ArrayList<>();
localizer = new ThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels);
// TODO: change this to your preferred localizer
localizer = new ThreeWheelLocalizer(hardwareMap);
}
/**
@ -85,23 +80,23 @@ public class PoseUpdater {
*
* @param set the Pose to set the starting pose to.
*/
public void setStartingPose(Pose2d set) {
public void setStartingPose(Pose set) {
startingPose = set;
previousPose = startingPose;
previousPoseTime = System.nanoTime();
currentPoseTime = System.nanoTime();
localizer.setPoseEstimate(set);
localizer.setStartPose(set);
}
/**
* This sets the current pose, using offsets so no reset time delay. This is better than the
* Road Runner reset, in general. Think of using offsets as setting trim in an aircraft. This can
* be reset as well, so beware of using the resetOffset() method.
* This sets the current pose, using offsets. Think of using offsets as setting trim in an
* aircraft. This can be reset as well, so beware of using the resetOffset() method.
*
*
* @param set The pose to set the current pose to.
*/
public void setCurrentPoseWithOffset(Pose2d set) {
Pose2d currentPose = getRawPose();
public void setCurrentPoseWithOffset(Pose set) {
Pose currentPose = getRawPose();
setXOffset(set.getX() - currentPose.getX());
setYOffset(set.getY() - currentPose.getY());
setHeadingOffset(MathFunctions.getTurnDirection(currentPose.getHeading(), set.getHeading()) * MathFunctions.getSmallestAngleDifference(currentPose.getHeading(), set.getHeading()));
@ -167,8 +162,8 @@ public class PoseUpdater {
* @param pose The pose to be offset.
* @return This returns a new Pose with the offset applied.
*/
public Pose2d applyOffset(Pose2d pose) {
return new Pose2d(pose.getX()+xOffset, pose.getY()+yOffset, pose.getHeading()+headingOffset);
public Pose applyOffset(Pose pose) {
return new Pose(pose.getX()+xOffset, pose.getY()+yOffset, pose.getHeading()+headingOffset);
}
/**
@ -189,9 +184,9 @@ public class PoseUpdater {
*
* @return returns the current pose.
*/
public Pose2d getPose() {
public Pose getPose() {
if (currentPose == null) {
currentPose = localizer.getPoseEstimate();
currentPose = localizer.getPose();
return applyOffset(currentPose);
} else {
return applyOffset(currentPose);
@ -205,9 +200,9 @@ public class PoseUpdater {
*
* @return returns the raw pose.
*/
public Pose2d getRawPose() {
public Pose getRawPose() {
if (currentPose == null) {
currentPose = localizer.getPoseEstimate();
currentPose = localizer.getPose();
return currentPose;
} else {
return currentPose;
@ -215,13 +210,13 @@ public class PoseUpdater {
}
/**
* This sets the current pose using the Road Runner pose reset. This is slow.
* This sets the current pose without using resettable offsets.
*
* @param set the pose to set the current pose to.
*/
public void setPose(Pose2d set) {
public void setPose(Pose set) {
resetOffset();
localizer.setPoseEstimate(set);
localizer.setPose(set);
}
/**
@ -229,7 +224,7 @@ public class PoseUpdater {
*
* @return returns the robot's previous pose.
*/
public Pose2d getPreviousPose() {
public Pose getPreviousPose() {
return previousPose;
}
@ -238,8 +233,10 @@ public class PoseUpdater {
*
* @return returns the robot's delta pose.
*/
public Pose2d getDeltaPose() {
return getPose().minus(previousPose);
public Pose getDeltaPose() {
Pose returnPose = getPose();
returnPose.subtract(previousPose);
return returnPose;
}
/**
@ -290,7 +287,7 @@ public class PoseUpdater {
* This resets the heading of the robot to the IMU's heading, using Road Runner's pose reset.
*/
public void resetHeadingToIMU() {
localizer.resetHeading(getNormalizedIMUHeading() + startingPose.getHeading());
localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
}
/**
@ -299,7 +296,7 @@ public class PoseUpdater {
* method.
*/
public void resetHeadingToIMUWithOffsets() {
setCurrentPoseWithOffset(new Pose2d(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
}
/**
@ -310,4 +307,22 @@ public class PoseUpdater {
public double getNormalizedIMUHeading() {
return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
}
/**
* This returns the total number of radians the robot has turned.
*
* @return the total heading.
*/
public double getTotalHeading() {
return localizer.getTotalHeading();
}
/**
* This returns the Localizer.
*
* @return the Localizer
*/
public Localizer getLocalizer() {
return localizer;
}
}

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

@ -1,125 +1,125 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization;
import com.acmerobotics.roadrunner.util.NanoClock;
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
*/
public class RoadRunnerEncoder {
private final static int CPS_STEP = 0x10000;
private static double inverseOverflow(double input, double estimate) {
// convert to uint16
int real = (int) input & 0xffff;
// initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
// because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
real += ((real % 20) / 4) * CPS_STEP;
// estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
return real;
}
public enum Direction {
FORWARD(1),
REVERSE(-1);
private int multiplier;
Direction(int multiplier) {
this.multiplier = multiplier;
}
public int getMultiplier() {
return multiplier;
}
}
private DcMotorEx motor;
private NanoClock clock;
private Direction direction;
private int lastPosition;
private int velocityEstimateIdx;
private double[] velocityEstimates;
private double lastUpdateTime;
public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) {
this.motor = motor;
this.clock = clock;
this.direction = Direction.FORWARD;
this.lastPosition = 0;
this.velocityEstimates = new double[3];
this.lastUpdateTime = clock.seconds();
}
public RoadRunnerEncoder(DcMotorEx motor) {
this(motor, NanoClock.system());
}
public Direction getDirection() {
return direction;
}
private int getMultiplier() {
return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
}
/**
* Allows you to set the direction of the counts and velocity without modifying the motor's direction state
* @param direction either reverse or forward depending on if encoder counts should be negated
*/
public void setDirection(Direction direction) {
this.direction = direction;
}
/**
* Gets the position from the underlying motor and adjusts for the set direction.
* Additionally, this method updates the velocity estimates used for compensated velocity
*
* @return encoder position
*/
public int getCurrentPosition() {
int multiplier = getMultiplier();
int currentPosition = motor.getCurrentPosition() * multiplier;
if (currentPosition != lastPosition) {
double currentTime = clock.seconds();
double dt = currentTime - lastUpdateTime;
velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
lastPosition = currentPosition;
lastUpdateTime = currentTime;
}
return currentPosition;
}
/**
* Gets the velocity directly from the underlying motor and compensates for the direction
* See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
*
* @return raw velocity
*/
public double getRawVelocity() {
int multiplier = getMultiplier();
return motor.getVelocity() * multiplier;
}
/**
* Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
* that are lost in overflow due to velocity being transmitted as 16 bits.
* CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
*
* @return corrected velocity
*/
public double getCorrectedVelocity() {
double median = velocityEstimates[0] > velocityEstimates[1]
? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
: Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
return inverseOverflow(getRawVelocity(), median);
}
}
//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
//
//import com.acmerobotics.roadrunner.util.NanoClock;
//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
// */
//public class RoadRunnerEncoder {
// private final static int CPS_STEP = 0x10000;
//
// private static double inverseOverflow(double input, double estimate) {
// // convert to uint16
// int real = (int) input & 0xffff;
// // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
// // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
// real += ((real % 20) / 4) * CPS_STEP;
// // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
// real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
// return real;
// }
//
// public enum Direction {
// FORWARD(1),
// REVERSE(-1);
//
// private int multiplier;
//
// Direction(int multiplier) {
// this.multiplier = multiplier;
// }
//
// public int getMultiplier() {
// return multiplier;
// }
// }
//
// private DcMotorEx motor;
// private NanoClock clock;
//
// private Direction direction;
//
// private int lastPosition;
// private int velocityEstimateIdx;
// private double[] velocityEstimates;
// private double lastUpdateTime;
//
// public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) {
// this.motor = motor;
// this.clock = clock;
//
// this.direction = Direction.FORWARD;
//
// this.lastPosition = 0;
// this.velocityEstimates = new double[3];
// this.lastUpdateTime = clock.seconds();
// }
//
// public RoadRunnerEncoder(DcMotorEx motor) {
// this(motor, NanoClock.system());
// }
//
// public Direction getDirection() {
// return direction;
// }
//
// private int getMultiplier() {
// return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
// }
//
// /**
// * Allows you to set the direction of the counts and velocity without modifying the motor's direction state
// * @param direction either reverse or forward depending on if encoder counts should be negated
// */
// public void setDirection(Direction direction) {
// this.direction = direction;
// }
//
// /**
// * Gets the position from the underlying motor and adjusts for the set direction.
// * Additionally, this method updates the velocity estimates used for compensated velocity
// *
// * @return encoder position
// */
// public int getCurrentPosition() {
// int multiplier = getMultiplier();
// int currentPosition = motor.getCurrentPosition() * multiplier;
// if (currentPosition != lastPosition) {
// double currentTime = clock.seconds();
// double dt = currentTime - lastUpdateTime;
// velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
// velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
// lastPosition = currentPosition;
// lastUpdateTime = currentTime;
// }
// return currentPosition;
// }
//
// /**
// * Gets the velocity directly from the underlying motor and compensates for the direction
// * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
// *
// * @return raw velocity
// */
// public double getRawVelocity() {
// int multiplier = getMultiplier();
// return motor.getVelocity() * multiplier;
// }
//
// /**
// * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
// * that are lost in overflow due to velocity being transmitted as 16 bits.
// * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
// *
// * @return corrected velocity
// */
// public double getCorrectedVelocity() {
// double median = velocityEstimates[0] > velocityEstimates[1]
// ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
// : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
// return inverseOverflow(getRawVelocity(), median);
// }
//}

117
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java Normal file
View File

@ -0,0 +1,117 @@
//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
//
//import androidx.annotation.NonNull;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
//import com.qualcomm.robotcore.hardware.DcMotorEx;
//import com.qualcomm.robotcore.hardware.HardwareMap;
//
//import java.util.Arrays;
//import java.util.List;
//
///*
// * Sample tracking wheel localizer implementation assuming the standard configuration:
// *
// * left on robot is y pos
// *
// * front of robot is x pos
// *
// * /--------------\
// * | ____ |
// * | ---- |
// * | || || |
// * | || || |
// * | |
// * | |
// * \--------------/
// *
// */
//
///**
// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will
// * be replaced with a custom localizer.
// */
//@Config
//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer {
// public static double TICKS_PER_REV = 8192;
// public static double WHEEL_RADIUS = 1.37795; // in
// public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
//
// public static double X_MULTIPLIER = 0.5008239963;
// public static double Y_MULTIPLIER = 0.5018874659;
//
// public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23;
//
// private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder;
//
// private List<Integer> lastEncPositions, lastEncVels;
//
// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) {
// super(Arrays.asList(
// new Pose2d(leftX, leftY, 0), // left
// new Pose2d(rightX, rightY, 0), // right
// new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe
// ));
//
// lastEncPositions = lastTrackingEncPositions;
// lastEncVels = lastTrackingEncVels;
//
// // TODO: redo the configs here
// leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
// rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
// strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
//
// // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
// leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
// rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
// strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD);
// }
//
// public void resetHeading(double heading) {
// setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading));
// }
//
// public static double encoderTicksToInches(double ticks) {
// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
// }
//
// @NonNull
// @Override
// public List<Double> getWheelPositions() {
// int leftPos = leftEncoder.getCurrentPosition();
// int rightPos = rightEncoder.getCurrentPosition();
// int frontPos = strafeEncoder.getCurrentPosition();
//
// lastEncPositions.clear();
// lastEncPositions.add(leftPos);
// lastEncPositions.add(rightPos);
// lastEncPositions.add(frontPos);
//
// return Arrays.asList(
// encoderTicksToInches(leftPos) * X_MULTIPLIER,
// encoderTicksToInches(rightPos) * X_MULTIPLIER,
// encoderTicksToInches(frontPos) * Y_MULTIPLIER
// );
// }
//
// @NonNull
// @Override
// public List<Double> getWheelVelocities() {
// int leftVel = (int) leftEncoder.getCorrectedVelocity();
// int rightVel = (int) rightEncoder.getCorrectedVelocity();
// int frontVel = (int) strafeEncoder.getCorrectedVelocity();
//
// lastEncVels.clear();
// lastEncVels.add(leftVel);
// lastEncVels.add(rightVel);
// lastEncVels.add(frontVel);
//
// return Arrays.asList(
// encoderTicksToInches(leftVel) * X_MULTIPLIER,
// encoderTicksToInches(rightVel) * X_MULTIPLIER,
// encoderTicksToInches(frontVel) * Y_MULTIPLIER
// );
// }
//}

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

@ -1,117 +1,193 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import java.util.Arrays;
import java.util.List;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
/*
* Sample tracking wheel localizer implementation assuming the standard configuration:
/**
* This is the ThreeWheelLocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the three wheel odometry set up. The diagram below, which is taken from
* Road Runner, shows a typical set up.
*
* The view is from the bottom of the robot looking upwards.
*
* left on robot is y pos
*
* front of robot is x pos
* front on robot is x pos
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || |
* | || || | left (y pos)
* | |
* | |
* \--------------/
* front (x pos)
*
*/
/**
* This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will
* be replaced with a custom localizer.
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 4/2/2024
*/
@Config
public class ThreeWheelLocalizer extends ThreeTrackingWheelLocalizer {
public static double TICKS_PER_REV = 8192;
public static double WHEEL_RADIUS = 1.37795; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
public class ThreeWheelLocalizer extends Localizer {
private HardwareMap hardwareMap;
private Pose startPose;
private Pose displacementPose;
private Pose currentVelocity;
private Matrix prevRotationMatrix;
private NanoTimer timer;
private long deltaTimeNano;
private Encoder leftEncoder;
private Encoder rightEncoder;
private Encoder strafeEncoder;
private Pose leftEncoderPose;
private Pose rightEncoderPose;
private Pose strafeEncoderPose;
private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
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;
public static double X_MULTIPLIER = 0.5008239963;
public static double Y_MULTIPLIER = 0.5018874659;
public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23;
private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder;
private List<Integer> lastEncPositions, lastEncVels;
public ThreeWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) {
super(Arrays.asList(
new Pose2d(leftX, leftY, 0), // left
new Pose2d(rightX, rightY, 0), // right
new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe
));
lastEncPositions = lastTrackingEncPositions;
lastEncVels = lastTrackingEncVels;
// TODO: redo the configs here
leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD);
public ThreeWheelLocalizer(HardwareMap map) {
this(map, new Pose());
}
public void resetHeading(double heading) {
setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading));
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);
rightEncoderPose = new Pose(-18.4/25.4 - 0.1, -159.6/25.4, 0);
strafeEncoderPose = new Pose(0*(-107.9/25.4+8)+-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90));
hardwareMap = map;
// TODO: replace these with your encoder ports
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(Encoder.REVERSE);
rightEncoder.setDirection(Encoder.REVERSE);
strafeEncoder.setDirection(Encoder.FORWARD);
setStartPose(setStartPose);
timer = new NanoTimer();
deltaTimeNano = 1;
displacementPose = new Pose();
currentVelocity = new Pose();
totalHeading = 0;
resetEncoders();
}
public static double encoderTicksToInches(double ticks) {
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
}
@NonNull
@Override
public List<Double> getWheelPositions() {
int leftPos = leftEncoder.getCurrentPosition();
int rightPos = rightEncoder.getCurrentPosition();
int frontPos = strafeEncoder.getCurrentPosition();
lastEncPositions.clear();
lastEncPositions.add(leftPos);
lastEncPositions.add(rightPos);
lastEncPositions.add(frontPos);
return Arrays.asList(
encoderTicksToInches(leftPos) * X_MULTIPLIER,
encoderTicksToInches(rightPos) * X_MULTIPLIER,
encoderTicksToInches(frontPos) * Y_MULTIPLIER
);
public Pose getPose() {
return MathFunctions.addPoses(startPose, displacementPose);
}
@NonNull
@Override
public List<Double> getWheelVelocities() {
int leftVel = (int) leftEncoder.getCorrectedVelocity();
int rightVel = (int) rightEncoder.getCorrectedVelocity();
int frontVel = (int) strafeEncoder.getCorrectedVelocity();
public Pose getVelocity() {
return currentVelocity.copy();
}
lastEncVels.clear();
lastEncVels.add(leftVel);
lastEncVels.add(rightVel);
lastEncVels.add(frontVel);
@Override
public Vector getVelocityVector() {
return currentVelocity.getVector();
}
return Arrays.asList(
encoderTicksToInches(leftVel) * X_MULTIPLIER,
encoderTicksToInches(rightVel) * X_MULTIPLIER,
encoderTicksToInches(frontVel) * Y_MULTIPLIER
);
@Override
public void setStartPose(Pose setStart) {
startPose = setStart;
}
public void setPrevRotationMatrix(double heading) {
prevRotationMatrix = new Matrix(3,3);
prevRotationMatrix.set(0, 0, Math.cos(heading));
prevRotationMatrix.set(0, 1, -Math.sin(heading));
prevRotationMatrix.set(1, 0, Math.sin(heading));
prevRotationMatrix.set(1, 1, Math.cos(heading));
prevRotationMatrix.set(2, 2, 1.0);
}
@Override
public void setPose(Pose setPose) {
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
resetEncoders();
}
@Override
public void update() {
deltaTimeNano = timer.getElapsedTime();
timer.resetTimer();
updateEncoders();
Matrix robotDeltas = getRobotDeltas();
Matrix globalDeltas;
setPrevRotationMatrix(getPose().getHeading());
Matrix transformation = new Matrix(3,3);
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(2, 2, 1.0);
} else {
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(2, 2, 1.0);
}
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
totalHeading += globalDeltas.get(2, 0);
}
public void updateEncoders() {
leftEncoder.update();
rightEncoder.update();
strafeEncoder.update();
}
public void resetEncoders() {
leftEncoder.reset();
rightEncoder.reset();
strafeEncoder.reset();
}
public Matrix getRobotDeltas() {
Matrix returnMatrix = new Matrix(3,1);
// x/forward movement
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
//y/strafe movement
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))));
// theta/turning
returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
return returnMatrix;
}
public double getTotalHeading() {
return totalHeading;
}
public double getForwardMultiplier() {
return FORWARD_TICKS_TO_INCHES;
}
public double getLateralMultiplier() {
return STRAFE_TICKS_TO_INCHES;
}
public double getTurningMultiplier() {
return TURN_TICKS_TO_RADIANS;
}
}

198
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java Normal file
View File

@ -0,0 +1,198 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
/**
* This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the two wheel odometry with IMU set up. The diagram below, which is taken from
* Road Runner, shows a typical set up.
*
* The view is from the bottom of the robot looking upwards.
*
* left on robot is y pos
*
* front on robot is x pos
*
* /--------------\
* | ____ |
* | ---- |
* | || |
* | || | left (y pos)
* | |
* | |
* \--------------/
* front (x pos)
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 4/2/2024
*/
@Config
public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo work
private HardwareMap hardwareMap;
private IMU imu;
private Pose startPose;
private Pose displacementPose;
private Pose currentVelocity;
private Matrix prevRotationMatrix;
private NanoTimer timer;
private long deltaTimeNano;
private Encoder forwardEncoder;
private Encoder strafeEncoder;
private Pose forwardEncoderPose;
private Pose strafeEncoderPose;
private double previousIMUOrientation;
private double deltaRadians;
private double totalHeading;
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;
public TwoWheelLocalizer(HardwareMap map) {
this(map, new Pose());
}
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);
strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90));
hardwareMap = map;
imu = hardwareMap.get(IMU.class, "imu");
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT)));
// TODO: replace these with your encoder ports
forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
// TODO: reverse any encoders necessary
forwardEncoder.setDirection(Encoder.REVERSE);
strafeEncoder.setDirection(Encoder.FORWARD);
setStartPose(setStartPose);
timer = new NanoTimer();
deltaTimeNano = 1;
displacementPose = new Pose();
currentVelocity = new Pose();
previousIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
deltaRadians = 0;
}
@Override
public Pose getPose() {
return MathFunctions.addPoses(startPose, displacementPose);
}
@Override
public Pose getVelocity() {
return currentVelocity.copy();
}
@Override
public Vector getVelocityVector() {
return currentVelocity.getVector();
}
@Override
public void setStartPose(Pose setStart) {
startPose = setStart;
}
public void setPrevRotationMatrix(double heading) {
prevRotationMatrix = new Matrix(3,3);
prevRotationMatrix.set(0, 0, Math.cos(heading));
prevRotationMatrix.set(0, 1, -Math.sin(heading));
prevRotationMatrix.set(1, 0, Math.sin(heading));
prevRotationMatrix.set(1, 1, Math.cos(heading));
prevRotationMatrix.set(2, 2, 1.0);
}
@Override
public void setPose(Pose setPose) {
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
resetEncoders();
}
@Override
public void update() {
deltaTimeNano = timer.getElapsedTime();
timer.resetTimer();
updateEncoders();
Matrix robotDeltas = getRobotDeltas();
Matrix globalDeltas;
setPrevRotationMatrix(getPose().getHeading());
Matrix transformation = new Matrix(3,3);
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(2, 2, 1.0);
} else {
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(2, 2, 1.0);
}
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
totalHeading += globalDeltas.get(2, 0);
}
public void updateEncoders() {
forwardEncoder.update();
strafeEncoder.update();
double currentIMUOrientation =MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation);
previousIMUOrientation = currentIMUOrientation;
}
public void resetEncoders() {
forwardEncoder.reset();
strafeEncoder.reset();
}
public Matrix getRobotDeltas() {
Matrix returnMatrix = new Matrix(3,1);
// x/forward movement
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians));
//y/strafe movement
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians));
// theta/turning
returnMatrix.set(2,0, deltaRadians);
return returnMatrix;
}
public double getTotalHeading() {
return totalHeading;
}
public double getForwardMultiplier() {
return FORWARD_TICKS_TO_INCHES;
}
public double getLateralMultiplier() {
return STRAFE_TICKS_TO_INCHES;
}
public double getTurningMultiplier() {
return 1;
}
}

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

@ -0,0 +1,49 @@
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.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;
/**
* This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the
* necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
* robot's current distance in ticks to the specified distance in inches. So, to use this, run the
* tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're
* at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials
* and average the results. Then, input the multiplier into the forward ticks to inches in your
* localizer of choice.
* You can adjust the target distance on FTC Dashboard: 192/168/43/1:8080/dash
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 5/6/2024
*/
@Config
@Autonomous(name = "Forward Localizer Tuner", group = "Autonomous Pathing Tuning")
public class FowardTuner extends OpMode {
private PoseUpdater poseUpdater;
private Telemetry telemetryA;
public static double DISTANCE = 30;
@Override
public void init() {
poseUpdater = new PoseUpdater(hardwareMap);
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();
}
@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()));
}
}

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

@ -0,0 +1,49 @@
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.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;
/**
* This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the
* necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
* robot's current distance in ticks to the specified distance in inches. So, to use this, run the
* tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're
* at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials
* and average the results. Then, input the multiplier into the strafe ticks to inches in your
* localizer of choice.
* You can adjust the target distance on FTC Dashboard: 192/168/43/1:8080/dash
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 5/6/2024
*/
@Config
@Autonomous(name = "Lateral Localizer Tuner", group = "Autonomous Pathing Tuning")
public class LateralTuner extends OpMode {
private PoseUpdater poseUpdater;
private Telemetry telemetryA;
public static double DISTANCE = 30;
@Override
public void init() {
poseUpdater = new PoseUpdater(hardwareMap);
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();
}
@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()));
}
}

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

@ -0,0 +1,114 @@
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;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater;
import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing;
import java.util.Arrays;
import java.util.List;
/**
* This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a
* PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot
* on FTC Dashboard (192/168/43/1:8080/dash). You should use this to check the robot's localization.
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 5/6/2024
*/
@Config
@TeleOp(group = "Pedro Pathing Tuning", name = "Localization Test")
public class LocalizationTest extends OpMode {
private PoseUpdater poseUpdater;
private Telemetry telemetryA;
private DcMotorEx leftFront;
private DcMotorEx leftRear;
private DcMotorEx rightFront;
private DcMotorEx rightRear;
private List<DcMotorEx> motors;
@Override
public void init() {
poseUpdater = new PoseUpdater(hardwareMap);
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
for (DcMotorEx motor : motors) {
MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
motor.setMotorType(motorConfigurationType);
}
for (DcMotorEx motor : motors) {
motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
}
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.addLine("This will print your robot's position to telemetry while "
+ "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());
}
@Override
public void loop() {
poseUpdater.update();
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double x = gamepad1.left_stick_x; // this is strafing
double rx = gamepad1.right_stick_x;
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double leftFrontPower = (y + x + rx) / denominator;
double leftRearPower = (y - x + rx) / denominator;
double rightFrontPower = (y - x - rx) / denominator;
double rightRearPower = (y + x - rx) / denominator;
leftFront.setPower(leftFrontPower);
leftRear.setPower(leftRearPower);
rightFront.setPower(rightFrontPower);
rightRear.setPower(rightRearPower);
telemetryA.addData("x", poseUpdater.getPose().getX());
telemetryA.addData("y", poseUpdater.getPose().getY());
telemetryA.addData("heading", poseUpdater.getPose().getHeading());
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);
}
}

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

@ -0,0 +1,49 @@
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.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;
/**
* This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the
* necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
* robot's current angle in ticks to the specified angle in radians. So, to use this, run the
* tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground.
* When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run
* multiple trials and average the results. Then, input the multiplier into the turning ticks to
* radians in your localizer of choice.
* You can adjust the target angle on FTC Dashboard: 192/168/43/1:8080/dash
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 5/6/2024
*/
@Config
@Autonomous(name = "Turn Localizer Tuner", group = "Autonomous Pathing Tuning")
public class TurnTuner extends OpMode {
private PoseUpdater poseUpdater;
private Telemetry telemetryA;
public static double ANGLE = 2 * Math.PI;
@Override
public void init() {
poseUpdater = new PoseUpdater(hardwareMap);
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();
}
@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()));
}
}

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

@ -164,7 +164,7 @@ public class BezierCurve {
public double getCurvature(double t) {
t = MathFunctions.clamp(t, 0, 1);
Vector derivative = getDerivative(t);
Vector secondDerivative = new Vector(getSecondDerivative(t).getMagnitude(), getApproxSecondDerivative(t).getTheta());
Vector secondDerivative = getSecondDerivative(t);
if (derivative.getMagnitude() == 0) return 0;
return (MathFunctions.crossProduct(derivative, secondDerivative))/Math.pow(derivative.getMagnitude(),3);

39
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java
View File

@ -1,6 +1,6 @@
package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
/**
* This is the MathFunctions class. This contains many useful math related methods that I use in
@ -105,24 +105,24 @@ public class MathFunctions {
}
/**
* This returns the distance between a Pose2d and a Point,
* This returns the distance between a Pose and a Point,
*
* @param pose this is the Pose2d.
* @param pose this is the Pose.
* @param point this is the Point.
* @return returns the distance between the two.
*/
public static double distance(Pose2d pose, Point point) {
public static double distance(Pose pose, Point point) {
return Math.sqrt(Math.pow(pose.getX()-point.getX(), 2) + Math.pow(pose.getY()-point.getY(), 2));
}
/**
* This returns the distance between a Pose2d and another Pose2d.
* This returns the distance between a Pose and another Pose.
*
* @param one this is the first Pose2d.
* @param two this is the second Pose2d.
* @param one this is the first Pose.
* @param two this is the second Pose.
* @return returns the distance between the two.
*/
public static double distance(Pose2d one, Pose2d two) {
public static double distance(Pose one, Pose two) {
return Math.sqrt(Math.pow(one.getX()-two.getX(), 2) + Math.pow(one.getY()-two.getY(), 2));
}
@ -137,6 +137,17 @@ public class MathFunctions {
return new Point(one.getX() + two.getX(), one.getY() + two.getY(), Point.CARTESIAN);
}
/**
* This returns a Pose that is the sum of the two input Pose.
*
* @param one the first Pose
* @param two the second Pose
* @return returns the sum of the two Pose.
*/
public static Pose addPoses(Pose one, Pose two) {
return new Pose(one.getX() + two.getX(), one.getY() + two.getY(), one.getHeading() + two.getHeading());
}
/**
* This subtracts the second Point from the first Point and returns the result as a Point.
* Do note that order matters here.
@ -149,6 +160,18 @@ public class MathFunctions {
return new Point(one.getX() - two.getX(), one.getY() - two.getY(), Point.CARTESIAN);
}
/**
* This subtracts the second Pose from the first Pose and returns the result as a Pose.
* Do note that order matters here.
*
* @param one the first Pose.
* @param two the second Pose.
* @return returns the difference of the two Pose.
*/
public static Pose subtractPoses(Pose one, Pose two) {
return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading());
}
/**
* This multiplies a Point by a scalar and returns the result as a Point
*

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

@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants;
import java.util.ArrayList;
@ -132,7 +131,7 @@ public class Path {
* @param searchStepLimit the binary search step limit.
* @return returns the closest Point.
*/
public Pose2d getClosestPoint(Pose2d pose, int searchStepLimit) {
public Pose getClosestPoint(Pose pose, int searchStepLimit) {
double lower = 0;
double upper = 1;
Point returnPoint;
@ -156,7 +155,7 @@ public class Path {
closestPointCurvature = curve.getCurvature(closestPointTValue);
return new Pose2d(returnPoint.getX(), returnPoint.getY(), getClosestPointHeadingGoal());
return new Pose(returnPoint.getX(), returnPoint.getY(), getClosestPointHeadingGoal());
}
/**

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

@ -1,6 +1,6 @@
package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
/**
* This is the Point class. This class handles storing information about the location of points in
@ -45,11 +45,11 @@ public class Point {
}
/**
* This creates a new Point from a Pose2d.
* This creates a new Point from a Pose.
*
* @param pose the Pose2d.
* @param pose the Pose.
*/
public Point(Pose2d pose) {
public Point(Pose pose) {
setCoordinates(pose.getX(), pose.getY(), CARTESIAN);
}

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

@ -0,0 +1,39 @@
package org.firstinspires.ftc.teamcode.pedroPathing.util;
import com.acmerobotics.dashboard.canvas.Canvas;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
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
*/
public class Drawing {
private Drawing() {}
public static void drawRobot(Canvas c, Point t) {
final double ROBOT_RADIUS = 9;
c.setStrokeWidth(1);
c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS);
Vector halfv = new Vector(0.5*ROBOT_RADIUS, t.getTheta());
Vector p1 = MathFunctions.addVectors(halfv, new Vector(t.getR(), t.getTheta()));
Vector p2 = MathFunctions.addVectors(p1, halfv);
c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent());
}
public static void drawRobot(Canvas c, Pose t) {
final double ROBOT_RADIUS = 9;
c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS);
Vector v = t.getHeadingVector();
v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS);
double x1 = t.getX() + v.getXComponent() / 2, y1 = t.getY() + v.getYComponent() / 2;
double x2 = t.getX() + v.getXComponent(), y2 = t.getY() + v.getYComponent();
c.strokeLine(x1, y1, x2, y2);
}
}