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

Added new localizer that uses three tracking wheels, with the IMU for most heading tracking. Also fixed the math for the teleop centripetal force correction with help from @junkjunk123 on Discord

Browse Source
This commit is contained in:
brotherhobo
2024-07-13 23:09:55 -04:00
parent ce2009b8b4
commit 14b5f267fb
4 changed files with 361 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -789,7 +789,7 @@ public class Follower {
} 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);
curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3));
}
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()));

57
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md
View File

@ -12,11 +12,13 @@ with the localizer that applies to you:
* If you're using two wheel odometry, put `new TwoWheelLocalizer(hardwareMap)`
* If you're using three wheel odometry, put `new ThreeWheelLocalizer(hardwareMap)`, so basically
don't change it from the default
* If you're using three wheel odometry with the IMU, put `new ThreeWheelIMULocalizer(hardwareMap)`
## Tuning
To start, you'll want to select your localizer of choice. Below, I'll have instructions for the drive
encoder localizer, two tracking wheel localizer, and the three tracking wheel localizer offered in
Pedro Pathing. Scroll down to the section that applies to you and follow the directions there.
encoder localizer, two tracking wheel localizer, the three tracking wheel localizer, and the three
wheel with IMU localizer offered in Pedro Pathing. Scroll down to the section that applies to you
and follow the directions there.
# Drive Encoders
* First, you'll need all of your drive motors to have encoders attached.
@ -70,6 +72,8 @@ to. The names of the variables is where on the robot the corresponding motor sho
* Next, go to where it tells you to replace the current statements with your encoder ports in the constructor.
Replace the `deviceName` parameter with the name of the port that the encoder is connected to. The
variable names correspond to which tracking wheel should be connected.
* After that, go to the instantiation of the IMU and change the orientation of the IMU to match that
of your robot's.
* Then, reverse the direction of any encoders so that the forward encoder ticks up when the robot
is moving forward and the strafe encoder ticks up when the robot moves right.
* Now, you'll have to tune the multipliers. These convert your measurements from encoder ticks into
@ -144,6 +148,55 @@ to. The names of the variables is where on the robot the corresponding motor sho
robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll
want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer!
# Three Wheel Localizer with IMU
* First, you'll need three odometry wheels connected to motor encoder ports on a hub.
* Then, go to `ThreeWheelIMULocalizer.java`. First, in the constructor, enter in the positions of your
tracking wheels relative to the center of the wheels of the robot. The positions are in inches, so
convert measurements accordingly. Use the comment above the class declaration to help you with the
coordinates.
* Next, go to where it tells you to replace the current statements with your encoder ports in the constructor.
Replace the `deviceName` parameter with the name of the port that the encoder is connected to. The
variable names correspond to which tracking wheel should be connected.
* After that, go to the instantiation of the IMU and change the orientation of the IMU to match that
of your robot's.
* Then, reverse the direction of any encoders so that the forward encoders tick up when the robot
is moving forward and the strafe encoder ticks up when the robot moves right.
* Although heading localization is done mostly through the IMU, the tracking wheels are still used for
small angle adjustments for better stability. So, you will still need to tune your turning multiplier.
* First, start with the `Turn Localizer Tuner`. Before doing any tuning, go to FTC Dashboard and find
the `ThreeWheelIMULocalizer` dropdown and deselect `useIMU`. You'll want to position your robot to be facing
in a direction you can easily find again, like lining up an edge of the robot against a field tile edge.
By default, you should spin the robot for one rotation going counterclockwise. Once you've spun
exactly that one rotation, or whatever you set that value to, then the turn multiplier will be shown
as the second number shown. The first number is how far the robot thinks you've spun, and the second
number is the multiplier you need to have to scale your current readings to your goal of one rotation,
or the custom set angle. Feel free to run a few more tests and average the results. Once you have
this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure
you replace the number, not add on or multiply it to the previous number. The tuner takes into
account your current multiplier.
* Next, go on to `Forward Localizer Tuner`. You should re-enable `useIMU` at this time. You'll want to position a rule alongside your robot.
By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever
you set that value to, then the forward multiplier will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the multiplier you
need to have to scale your current readings to your goal of 30 inches, or the custom set angle.
Feel free to run a few more tests and average the results. Once you have this multiplier, then
replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current multiplier.
* Finally, go to `Lateral Localizer Tuner`. `useIMU` should be enabled for this step. You'll want to position a rule alongside your robot.
By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever
you set that value to, then the lateral multiplier will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the multiplier you
need to have to scale your current readings to your goal of 30 inches, or the custom set angle.
Feel free to run a few more tests and average the results. Once you have this multiplier, then
replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current multiplier.
* Once you're done with all this, your localizer should be tuned. Make sure that `useIMU` is turned back on. To test it out, you can go to
`Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash)
and on the top right, switch the drop down from the default view to the field view. Then, on the bottom
left corner, you should see a field and the robot being drawn on the field. You can then move your
robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll
want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer!
## Using Road Runner's Localizer
Of course, many teams have experience using Road Runner in the past and so have localizers from Road
Runner that are tuned. There is an adapter for the Road Runner three wheel localizer to the Pedro

304
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java Normal file
View File

@ -0,0 +1,304 @@
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.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
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 ThreeWheelIMULocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the three wheel odometry set up with the IMU to have more accurate heading
* readings. 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 Logan Nash
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 7/9/2024
*/
@Config
public class ThreeWheelIMULocalizer 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;
public final IMU imu;
private double previousIMUOrientation;
private double deltaRadians;
private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = 0.002957;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
public static double STRAFE_TICKS_TO_INCHES = -0.003127403096038503;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
public static double TURN_TICKS_TO_RADIANS = 0.002995;//8192 * 1.37795 * 2 * Math.PI * 0.5;
public static boolean useIMU = true;
/**
* This creates a new ThreeWheelIMULocalizer from a HardwareMap, with a starting Pose at (0,0)
* facing 0 heading.
*
* @param map the HardwareMap
*/
public ThreeWheelIMULocalizer(HardwareMap map) {
this(map, new Pose());
}
/**
* This creates a new ThreeWheelIMULocalizer 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 ThreeWheelIMULocalizer(HardwareMap map, Pose setStartPose) {
hardwareMap = map;
imu = hardwareMap.get(IMU.class, "imu");
// TODO: replace this with your IMU's orientation
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP)));
// TODO: replace these with your encoder positions
leftEncoderPose = new Pose(-3, 5.7, 0);
rightEncoderPose = new Pose(-3, -5.7, 0);
strafeEncoderPose = new Pose(6.9, 1, Math.toRadians(90));
// TODO: replace these with your encoder ports
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lb"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lf"));
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rf"));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(Encoder.REVERSE);
rightEncoder.setDirection(Encoder.FORWARD);
strafeEncoder.setDirection(Encoder.FORWARD);
setStartPose(setStartPose);
timer = new NanoTimer();
deltaTimeNano = 1;
displacementPose = new Pose();
currentVelocity = new Pose();
totalHeading = 0;
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));
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);
}
/**
* 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();
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);
}
/**
* This updates the Encoders.
*/
public void updateEncoders() {
leftEncoder.update();
rightEncoder.update();
strafeEncoder.update();
double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation);
previousIMUOrientation = currentIMUOrientation;
}
/**
* 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
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
if (MathFunctions.getSmallestAngleDifference(0, deltaRadians) > 0.00005 && useIMU) {
returnMatrix.set(2, 0, deltaRadians);
} else {
returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
}
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;
}
}

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

@ -80,6 +80,7 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w
hardwareMap = map;
imu = hardwareMap.get(IMU.class, "imu");
// TODO: replace this with your IMU's orientation
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT)));
// TODO: replace these with your encoder ports