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

disabled road runner localizer adapters to reduce number of gradle dependencies

Browse Source
This commit is contained in:
brotherhobo
2024-05-14 08:39:48 -04:00
parent 168537cb28
commit 52f3cb91ae
7 changed files with 491 additions and 495 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/build.gradle
View File

@ -27,7 +27,5 @@ dependencies {
implementation project(':FtcRobotController') implementation project(':FtcRobotController')
annotationProcessor files('lib/OpModeAnnotationProcessor.jar') annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
implementation 'com.acmerobotics.roadrunner:core:0.5.6'
} }

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

@ -1,6 +1,6 @@
## Welcome! ## Welcome!
This is the Pedro Pathing path following program developed by FTC team 10158 Scott's Bots in the This is the Pedro Pathing path following program developed by FTC team 10158 Scott's Bots with Logan
2023-2024 Centerstage season. Nash in the 2023-2024 Centerstage season.
## Installation ## Installation
The quickest way to get started is with the quickstart [here](https://github.com/brotherhobo/Pedro-Pathing-Quickstart). The quickest way to get started is with the quickstart [here](https://github.com/brotherhobo/Pedro-Pathing-Quickstart).
@ -21,9 +21,7 @@ maven { url = 'https://maven.brott.dev/' }
implementation 'com.acmerobotics.dashboard:dashboard:0.4.5' implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'
``` ```
4. Find the `build.gradle` file under the `teamcode` folder. 4. Find the `build.gradle` file under the `teamcode` folder.
5. In this gradle file, add the following dependencies: 5. In this gradle file, add the following dependency:
``` ```
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'com.fasterxml/jackson.core:jacson-databind:2.12.7' implementation 'com.fasterxml/jackson.core:jacson-databind:2.12.7'
implementation 'com.acmerobotics.com.roadrunner:core:0.5.6'
``` ```

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

@ -1,94 +1,94 @@
## Prerequisites ## Prerequisites
Obviously, you have to have a robot to use Pedro Pathing. Also, Pedro Pathing is only able to work 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. 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 has both a three-wheel localizer and a You must also have a localizer of some sort. Pedro Pathing has a drive encoder, a two tracking wheel,
two-wheel localizer. You will need to have your localizer tuned before starting to tune PedroPathing. and a three tracking wheel localizer. You will need to have your localizer tuned before starting to
Check out the tuning guide under the localization tab if you're planning on using one of the 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) 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). 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. One last thing to note is that Pedro Pathing operates in inches and radians.
## Tuning ## Tuning
* To start with, we need the mass of the robot in kg. This is used for the centripetal force * To start with, we need the mass of the robot in kg. This is used for the centripetal force
correction, and the mass should be put on line `114` in the `FollowerConstants` class under the correction, and the mass should be put on line `114` in the `FollowerConstants` class under the
`tuning` package. `tuning` package.
* Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a * Next, we need to find the preferred mecanum drive vectors. The rollers on mecanum wheels point at a
45 degree angle from the forward direction, but the actual direction the force is output is actually 45 degree angle from the forward direction, but the actual direction the force is output is actually
closer to forward. To find the direction your wheels will go, you will need to run the closer to forward. To find the direction your wheels will go, you will need to run the
`Forward Velocity Tuner` and `Strafe Velocity Tuner` OpModes. These will run your robot at full `Forward Velocity Tuner` and `Strafe Velocity Tuner` OpModes. These will run your robot at full
power for 40 inches forward and to the right, respectively. The distance can be changed through FTC power for 40 inches forward and to the right, respectively. The distance can be changed through FTC
Dashboard under the dropdown for each respective class, but higher distances work better. After the Dashboard under the dropdown for each respective class, but higher distances work better. After the
distance has finished running, the end velocity will be output to telemetry. The robot may continue distance has finished running, the end velocity will be output to telemetry. The robot may continue
to drift a little bit after the robot has finished running the distance, so make sure you have to drift a little bit after the robot has finished running the distance, so make sure you have
plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `25` in plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `25` in
the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `26` in the the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `26` in the
`FollowerConstants` class. `FollowerConstants` class.
* The last set of automatic tuners you'll need to run are the zero power acceleration tuners. These * The last set of automatic tuners you'll need to run are the zero power acceleration tuners. These
find the rate at which your robot decelerates when power is cut from the drivetrain. This is used to find the rate at which your robot decelerates when power is cut from the drivetrain. This is used to
get a more accurate estimation of the drive vector. To find this, you will need to run the get a more accurate estimation of the drive vector. To find this, you will need to run the
`Forward Zero Power Acceleration Tuner` and the `Lateral Zero Power Acceleration Tuner` OpModes. `Forward Zero Power Acceleration Tuner` and the `Lateral Zero Power Acceleration Tuner` OpModes.
These will run your robot until it hits a velocity of 10 inches/second forward and to the right, These will run your robot until it hits a velocity of 10 inches/second forward and to the right,
respectively. The velocity can be changed through FTC Dashboard under the dropdown for each respectively. The velocity can be changed through FTC Dashboard under the dropdown for each
respective class, but higher velocities work better. After the velocity has been reached, power will respective class, but higher velocities work better. After the velocity has been reached, power will
be cut from the drivetrain and the robot's deceleration will be tracked until the robot stops, at be cut from the drivetrain and the robot's deceleration will be tracked until the robot stops, at
which point it will display the deceleration in telemetry. This robot will need to drift to a stop which point it will display the deceleration in telemetry. This robot will need to drift to a stop
to properly work, and the higher the velocity the greater the drift distance, so make sure you have to properly work, and the higher the velocity the greater the drift distance, so make sure you have
enough room. Once you're done, put the zero power acceleration for the enough room. Once you're done, put the zero power acceleration for the
`Forward Zero Power Acceleration Tuner` on line `122` in the `FollowerConstants` class and the zero `Forward Zero Power Acceleration Tuner` on line `122` in the `FollowerConstants` class and the zero
power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `127` in the power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `127` in the
`FollowerConstants` class. `FollowerConstants` class.
* After this, we will want to tune the translational PIDs. Go to FTC Dashboard and disable all but * After this, we will want to tune the translational PIDs. Go to FTC Dashboard and disable all but
the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make the `useTranslational` checkboxes under the `Follower` tab. Then, run `StraightBackAndForth`. Make
sure you disable the timer on autonomous OpModes. There are two different PIDs for translational sure you disable the timer on autonomous OpModes. There are two different PIDs for translational
error, the `smallTranslationalPIDF` and `largeTranslationalPIDF`. If error is larger than a certain error, the `smallTranslationalPIDF` and `largeTranslationalPIDF`. If error is larger than a certain
amount, the `largeTranslationalPIDF` will be used, and if error is smaller than that amount the amount, the `largeTranslationalPIDF` will be used, and if error is smaller than that amount the
`smallTranslationalPIDF` will be used. If you need to add a feedforward value, use the `smallTranslationalPIDF` will be used. If you need to add a feedforward value, use the
`smallTranslationalPIDFFeedForward` and `largeTranslationalPIDFFeedForward` since those will add the `smallTranslationalPIDFFeedForward` and `largeTranslationalPIDFFeedForward` since those will add the
feedforward in the direction the robot is trying to move, rather than the feedforward in the PIDFs feedforward in the direction the robot is trying to move, rather than the feedforward in the PIDFs
themselves, since those will only add the feedforward one way. You can change the amount of error themselves, since those will only add the feedforward one way. You can change the amount of error
required to switch PIDFs, as well as the PIDF constants and feedforward values, under the required to switch PIDFs, as well as the PIDF constants and feedforward values, under the
`FollowerConstants` tab in FTC Dashboard. To tune the PIDs, push the robot off the path and see how `FollowerConstants` tab in FTC Dashboard. To tune the PIDs, push the robot off the path and see how
corrects. You will want to alternate sides you push to reduce field wear and tear as well as push corrects. You will want to alternate sides you push to reduce field wear and tear as well as push
with varying power and distance. I would recommend tuning the large PID first, and tuning it so that with varying power and distance. I would recommend tuning the large PID first, and tuning it so that
the PID is capable of correcting to the point where the PIDs switch with little momentum. Then, tune the PID is capable of correcting to the point where the PIDs switch with little momentum. Then, tune
the small PID to minimize oscillations while still achieving a satisfactory level of accuracy. the small PID to minimize oscillations while still achieving a satisfactory level of accuracy.
Overall, try to tune for fewer oscillations rather than higher speeds or perfect accuracy, since Overall, try to tune for fewer oscillations rather than higher speeds or perfect accuracy, since
this will make the robot run more smoothly under actual pathing conditions. this will make the robot run more smoothly under actual pathing conditions.
* Next, we will tune the heading PIDs. The process is essentially the same as above, except you will * Next, we will tune the heading PIDs. The process is essentially the same as above, except you will
want to only enable `useHeading` under `Follower` on FTC Dashboard, as well as turn the robot from want to only enable `useHeading` under `Follower` on FTC Dashboard, as well as turn the robot from
opposing corners instead of pushing the robot. Naturally, instead of changing the stuff with opposing corners instead of pushing the robot. Naturally, instead of changing the stuff with
"translational" in the name, you will instead want to look for stuff with "heading" in the name. "translational" in the name, you will instead want to look for stuff with "heading" in the name.
Otherwise, these two PIDs are functionally very similar. The same tips from above will apply to this. Otherwise, these two PIDs are functionally very similar. The same tips from above will apply to this.
* Afterwards, we will tune the drive PIDs. Before we continue, we will need to set the * Afterwards, we will tune the drive PIDs. Before we continue, we will need to set the
`zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor `zeroPowerAccelerationMultiplier`. This determines how fast your robot will decelerate as a factor
of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but
what works best for you is most important. Higher numbers will cause a faster brake, but increase what works best for you is most important. Higher numbers will cause a faster brake, but increase
oscillations at the end. Lower numbers will do the opposite. This can be found on line `136` in oscillations at the end. Lower numbers will do the opposite. This can be found on line `136` in
`FollowerConstants`. There are once again two PIDs for the drive vector, but these PIDs are much, `FollowerConstants`. There are once again two PIDs for the drive vector, but these PIDs are much,
much more sensitive than the others. For reference, my P values were in the hundredths and much more sensitive than the others. For reference, my P values were in the hundredths and
thousandths place values, and my D values were in the hundred thousandths and millionths place thousandths place values, and my D values were in the hundred thousandths and millionths place
values. To tune this, enable `useDrive`, `useHeading`, and `useTranslational` in the `Follower` values. To tune this, enable `useDrive`, `useHeading`, and `useTranslational` in the `Follower`
dropdown in FTC Dashboard. Next, run `StraightBackAndForth`and don't forget to turn off the timer on dropdown in FTC Dashboard. Next, run `StraightBackAndForth`and don't forget to turn off the timer on
the OpMode. Then, tune the large PID and then the small PID following the tips from earlier. For the OpMode. Then, tune the large PID and then the small PID following the tips from earlier. For
this, it is very important to try to reduce oscillations. Additionally, I would absolutely not this, it is very important to try to reduce oscillations. Additionally, I would absolutely not
recommend using the I, or integral, part of the PID for this. Using integral in drivetrain PIDs is recommend using the I, or integral, part of the PID for this. Using integral in drivetrain PIDs is
already not ideal, but it will continuously build up error in this PID, causing major issues when already not ideal, but it will continuously build up error in this PID, causing major issues when
it gets too strong. So, just don't use it. P and D are enough. it gets too strong. So, just don't use it. P and D are enough.
* Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open * Finally, we will want to tune the centripetal force correction. This is a pretty simple tune. Open
up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth` up FTC Dashboard and enable everything under the `Follower` tab. Then, run `CurvedBackAndForth`
and turn off its timer. If you notice the robot is correcting towards the inside of the curve and turn off its timer. If you notice the robot is correcting towards the inside of the curve
as/after running a path, then increase `centripetalScaling`, which can be found on line `117` of as/after running a path, then increase `centripetalScaling`, which can be found on line `117` of
`FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease `FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease
`centripetalScaling`. `centripetalScaling`.
* Once you've found satisfactory tunings for everything, run the robot around in * Once you've found satisfactory tunings for everything, run the robot around in
`StraightBackAndForth`, `CurvedBackAndForth`, or some paths of your own making. There's also `StraightBackAndForth`, `CurvedBackAndForth`, or some paths of your own making. There's also
`Circle`, but that's more so for fun than anything else. If you notice something could be improved, `Circle`, but that's more so for fun than anything else. If you notice something could be improved,
feel free to mess around more with your PIDs. That should be all! If you have any more questions, feel free to mess around more with your PIDs. That should be all! If you have any more questions,
refer to the OVERVIEW readme file or the README readme file. Best of luck to your team this season! :) refer to the OVERVIEW readme file or the README readme file. Best of luck to your team this season! :)

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

@ -1,159 +1,159 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization; //package org.firstinspires.ftc.teamcode.pedroPathing.localization;
//
import com.acmerobotics.roadrunner.geometry.Pose2d; //import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.hardware.HardwareMap; //import com.qualcomm.robotcore.hardware.HardwareMap;
//
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; //import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; //import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
//
import java.util.ArrayList; //import java.util.ArrayList;
import java.util.List; //import java.util.List;
//
/** ///**
* This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and // * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and
* is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing // * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing
* localizer system. // * localizer system.
* // *
* @author Anyi Lin - 10158 Scott's Bots // * @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 5/9/2024 // * @version 1.0, 5/9/2024
*/ // */
public class RRToPedroThreeWheelLocalizer extends Localizer { //public class RRToPedroThreeWheelLocalizer extends Localizer {
private RoadRunnerThreeWheelLocalizer localizer; // private RoadRunnerThreeWheelLocalizer localizer;
private double totalHeading; // private double totalHeading;
private Pose startPose; // private Pose startPose;
private Pose previousPose; // private Pose previousPose;
//
/** // /**
* This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously // * This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously
* used Road Runner localization system to the new Pedro Pathing localization system. // * used Road Runner localization system to the new Pedro Pathing localization system.
* // *
* @param hardwareMap the HardwareMap // * @param hardwareMap the HardwareMap
*/ // */
public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { // public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) {
List<Integer> lastTrackingEncPositions = new ArrayList<>(); // List<Integer> lastTrackingEncPositions = new ArrayList<>();
List<Integer> lastTrackingEncVels = new ArrayList<>(); // List<Integer> lastTrackingEncVels = new ArrayList<>();
//
localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); // localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels);
//
startPose = new Pose(); // startPose = new Pose();
previousPose = new Pose(); // previousPose = new Pose();
} // }
//
/** // /**
* This returns the current pose estimate as a Pose. // * This returns the current pose estimate as a Pose.
* // *
* @return returns the current pose estimate // * @return returns the current pose estimate
*/ // */
@Override // @Override
public Pose getPose() { // public Pose getPose() {
Pose2d pose = localizer.getPoseEstimate(); // Pose2d pose = localizer.getPoseEstimate();
return new Pose(pose.getX(), pose.getY(), pose.getHeading()); // return new Pose(pose.getX(), pose.getY(), pose.getHeading());
} // }
//
/** // /**
* This returns the current velocity estimate as a Pose. // * This returns the current velocity estimate as a Pose.
* // *
* @return returns the current velocity estimate // * @return returns the current velocity estimate
*/ // */
@Override // @Override
public Pose getVelocity() { // public Pose getVelocity() {
Pose2d pose = localizer.getPoseVelocity(); // Pose2d pose = localizer.getPoseVelocity();
return new Pose(pose.getX(), pose.getY(), pose.getHeading()); // return new Pose(pose.getX(), pose.getY(), pose.getHeading());
} // }
//
/** // /**
* This returns the current velocity estimate as a Vector. // * This returns the current velocity estimate as a Vector.
* // *
* @return returns the current velocity estimate // * @return returns the current velocity estimate
*/ // */
@Override // @Override
public Vector getVelocityVector() { // public Vector getVelocityVector() {
Pose2d pose = localizer.getPoseVelocity(); // Pose2d pose = localizer.getPoseVelocity();
Vector returnVector = new Vector(); // Vector returnVector = new Vector();
returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); // returnVector.setOrthogonalComponents(pose.getX(), pose.getY());
return returnVector; // return returnVector;
} // }
//
/** // /**
* This sets the start pose. Any movement of the robot is treated as a displacement from the // * This sets the start pose. Any movement of the robot is treated as a displacement from the
* start pose, so moving the start pose will move the current pose estimate the same amount. // * start pose, so moving the start pose will move the current pose estimate the same amount.
* // *
* @param setStart the new start pose // * @param setStart the new start pose
*/ // */
@Override // @Override
public void setStartPose(Pose setStart) { // public void setStartPose(Pose setStart) {
Pose oldStart = startPose; // Pose oldStart = startPose;
startPose = setStart; // startPose = setStart;
Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); // Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart);
localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); // localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading()));
} // }
//
/** // /**
* This sets the current pose estimate. This has no effect on the start pose. // * This sets the current pose estimate. This has no effect on the start pose.
* // *
* @param setPose the new current pose estimate // * @param setPose the new current pose estimate
*/ // */
@Override // @Override
public void setPose(Pose setPose) { // public void setPose(Pose setPose) {
localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); // localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading()));
} // }
//
/** // /**
* This updates the total heading and previous pose estimate. Everything else is handled by the // * This updates the total heading and previous pose estimate. Everything else is handled by the
* Road Runner localizer on its own, but updating this tells you how far the robot has really // * Road Runner localizer on its own, but updating this tells you how far the robot has really
* turned. // * turned.
*/ // */
@Override // @Override
public void update() { // public void update() {
totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); // totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading());
previousPose = getPose(); // previousPose = getPose();
} // }
//
/** // /**
* This returns how far the robot has actually turned. // * This returns how far the robot has actually turned.
* // *
* @return returns the total angle turned, in degrees. // * @return returns the total angle turned, in degrees.
*/ // */
@Override // @Override
public double getTotalHeading() { // public double getTotalHeading() {
return totalHeading; // return totalHeading;
} // }
//
/** // /**
* This returns the forward multiplier of the Road Runner localizer, which converts from ticks // * This returns the forward multiplier of the Road Runner localizer, which converts from ticks
* to inches. You can actually use the tuners in Pedro Pathing to find the value that everything // * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything
* multiplied together should be. If you do use that, then do be aware that the value returned is // * multiplied together should be. If you do use that, then do be aware that the value returned is
* the product of the Road Runner ticks to inches and the x multiplier. // * the product of the Road Runner ticks to inches and the x multiplier.
* // *
* @return returns the forward multiplier // * @return returns the forward multiplier
*/ // */
@Override // @Override
public double getForwardMultiplier() { // public double getForwardMultiplier() {
return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; // return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER;
} // }
//
/** // /**
* This returns the lateral multiplier of the Road Runner localizer, which converts from ticks // * This returns the lateral multiplier of the Road Runner localizer, which converts from ticks
* to inches. You can actually use the tuners in Pedro Pathing to find the value that everything // * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything
* multiplied together should be. If you do use that, then do be aware that the value returned is // * multiplied together should be. If you do use that, then do be aware that the value returned is
* the product of the Road Runner ticks to inches and the y multiplier. // * the product of the Road Runner ticks to inches and the y multiplier.
* // *
* @return returns the lateral multiplier // * @return returns the lateral multiplier
*/ // */
@Override // @Override
public double getLateralMultiplier() { // public double getLateralMultiplier() {
return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; // return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER;
} // }
//
/** // /**
* This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist. // * This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist.
* There really isn't a point in tuning the turning for the Road Runner localizer. This will // * There really isn't a point in tuning the turning for the Road Runner localizer. This will
* actually just return the average of the two other multipliers. // * actually just return the average of the two other multipliers.
* // *
* @return returns the turning multiplier // * @return returns the turning multiplier
*/ // */
@Override // @Override
public double getTurningMultiplier() { // public double getTurningMultiplier() {
return (getForwardMultiplier() + getLateralMultiplier()) / 2; // return (getForwardMultiplier() + getLateralMultiplier()) / 2;
} // }
} //}

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

@ -1,132 +1,132 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization; //package org.firstinspires.ftc.teamcode.pedroPathing.localization;
//
import com.acmerobotics.roadrunner.util.NanoClock; //import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.hardware.DcMotorEx; //import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple; //import com.qualcomm.robotcore.hardware.DcMotorSimple;
//
/** ///**
* This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a // * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a
* custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected // * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected
* velocity counts and allow reversing independently of the corresponding slot's motor direction. // * velocity counts and allow reversing independently of the corresponding slot's motor direction.
* // *
* I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have // * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have
* import statements, so I'm not crediting myself as an author for this. // * import statements, so I'm not crediting myself as an author for this.
* // *
* @author Road Runner dev team // * @author Road Runner dev team
* @version 1.0, 5/9/2024 // * @version 1.0, 5/9/2024
*/ // */
public class RoadRunnerEncoder { //public class RoadRunnerEncoder {
private final static int CPS_STEP = 0x10000; // private final static int CPS_STEP = 0x10000;
//
private static double inverseOverflow(double input, double estimate) { // private static double inverseOverflow(double input, double estimate) {
// convert to uint16 // // convert to uint16
int real = (int) input & 0xffff; // int real = (int) input & 0xffff;
// initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits // // 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 // // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
real += ((real % 20) / 4) * CPS_STEP; // real += ((real % 20) / 4) * CPS_STEP;
// estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by // // 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; // real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
return real; // return real;
} // }
//
public enum Direction { // public enum Direction {
FORWARD(1), // FORWARD(1),
REVERSE(-1); // REVERSE(-1);
//
private int multiplier; // private int multiplier;
//
Direction(int multiplier) { // Direction(int multiplier) {
this.multiplier = multiplier; // this.multiplier = multiplier;
} // }
//
public int getMultiplier() { // public int getMultiplier() {
return multiplier; // return multiplier;
} // }
} // }
//
private DcMotorEx motor; // private DcMotorEx motor;
private NanoClock clock; // private NanoClock clock;
//
private Direction direction; // private Direction direction;
//
private int lastPosition; // private int lastPosition;
private int velocityEstimateIdx; // private int velocityEstimateIdx;
private double[] velocityEstimates; // private double[] velocityEstimates;
private double lastUpdateTime; // private double lastUpdateTime;
//
public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { // public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) {
this.motor = motor; // this.motor = motor;
this.clock = clock; // this.clock = clock;
//
this.direction = Direction.FORWARD; // this.direction = Direction.FORWARD;
//
this.lastPosition = 0; // this.lastPosition = 0;
this.velocityEstimates = new double[3]; // this.velocityEstimates = new double[3];
this.lastUpdateTime = clock.seconds(); // this.lastUpdateTime = clock.seconds();
} // }
//
public RoadRunnerEncoder(DcMotorEx motor) { // public RoadRunnerEncoder(DcMotorEx motor) {
this(motor, NanoClock.system()); // this(motor, NanoClock.system());
} // }
//
public Direction getDirection() { // public Direction getDirection() {
return direction; // return direction;
} // }
//
private int getMultiplier() { // private int getMultiplier() {
return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); // 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 // * 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 // * @param direction either reverse or forward depending on if encoder counts should be negated
*/ // */
public void setDirection(Direction direction) { // public void setDirection(Direction direction) {
this.direction = direction; // this.direction = direction;
} // }
//
/** // /**
* Gets the position from the underlying motor and adjusts for the set 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 // * Additionally, this method updates the velocity estimates used for compensated velocity
* // *
* @return encoder position // * @return encoder position
*/ // */
public int getCurrentPosition() { // public int getCurrentPosition() {
int multiplier = getMultiplier(); // int multiplier = getMultiplier();
int currentPosition = motor.getCurrentPosition() * multiplier; // int currentPosition = motor.getCurrentPosition() * multiplier;
if (currentPosition != lastPosition) { // if (currentPosition != lastPosition) {
double currentTime = clock.seconds(); // double currentTime = clock.seconds();
double dt = currentTime - lastUpdateTime; // double dt = currentTime - lastUpdateTime;
velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; // velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; // velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
lastPosition = currentPosition; // lastPosition = currentPosition;
lastUpdateTime = currentTime; // lastUpdateTime = currentTime;
} // }
return currentPosition; // return currentPosition;
} // }
//
/** // /**
* Gets the velocity directly from the underlying motor and compensates for the direction // * 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) // * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
* // *
* @return raw velocity // * @return raw velocity
*/ // */
public double getRawVelocity() { // public double getRawVelocity() {
int multiplier = getMultiplier(); // int multiplier = getMultiplier();
return motor.getVelocity() * multiplier; // return motor.getVelocity() * multiplier;
} // }
//
/** // /**
* Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity // * 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. // * 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. // * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
* // *
* @return corrected velocity // * @return corrected velocity
*/ // */
public double getCorrectedVelocity() { // public double getCorrectedVelocity() {
double median = velocityEstimates[0] > velocityEstimates[1] // double median = velocityEstimates[0] > velocityEstimates[1]
? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) // ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
: Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); // : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
return inverseOverflow(getRawVelocity(), median); // return inverseOverflow(getRawVelocity(), median);
} // }
} //}

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

@ -1,123 +1,123 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization; //package org.firstinspires.ftc.teamcode.pedroPathing.localization;
//
import androidx.annotation.NonNull; //import androidx.annotation.NonNull;
//
import com.acmerobotics.dashboard.config.Config; //import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d; //import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; //import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx; //import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap; //import com.qualcomm.robotcore.hardware.HardwareMap;
//
import java.util.Arrays; //import java.util.Arrays;
import java.util.List; //import java.util.List;
//
/* ///*
* Sample tracking wheel localizer implementation assuming the standard configuration: // * Sample tracking wheel localizer implementation assuming the standard configuration:
* // *
* left on robot is y pos // * left on robot is y pos
* // *
* front of robot is x pos // * front of robot is x pos
* // *
* /--------------\ // * /--------------\
* | ____ | // * | ____ |
* | ---- | // * | ---- |
* | || || | // * | || || |
* | || || | // * | || || |
* | | // * | |
* | | // * | |
* \--------------/ // * \--------------/
* // *
*/ // */
//
/** ///**
* This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will // * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will
* be replaced with a custom localizer. I made some minor changes, so I'm crediting myself as an // * be replaced with a custom localizer. I made some minor changes, so I'm crediting myself as an
* 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to // * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to
* Pedro Pathing to avoid having imports. // * Pedro Pathing to avoid having imports.
* // *
* @author Road Runner dev team // * @author Road Runner dev team
* @author Anyi Lin - 10158 Scott's Bots // * @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 5/9/2024 // * @version 1.0, 5/9/2024
*/ // */
@Config //@Config
public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { //public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer {
public static double TICKS_PER_REV = 8192; // public static double TICKS_PER_REV = 8192;
public static double WHEEL_RADIUS = 1.37795; // in // public static double WHEEL_RADIUS = 1.37795; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed // public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
//
public static double X_MULTIPLIER = 0.5008239963; // public static double X_MULTIPLIER = 0.5008239963;
public static double Y_MULTIPLIER = 0.5018874659; // 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; // 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 RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder;
//
private List<Integer> lastEncPositions, lastEncVels; // private List<Integer> lastEncPositions, lastEncVels;
//
public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) { // public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) {
super(Arrays.asList( // super(Arrays.asList(
new Pose2d(leftX, leftY, 0), // left // new Pose2d(leftX, leftY, 0), // left
new Pose2d(rightX, rightY, 0), // right // new Pose2d(rightX, rightY, 0), // right
new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe // new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe
)); // ));
//
lastEncPositions = lastTrackingEncPositions; // lastEncPositions = lastTrackingEncPositions;
lastEncVels = lastTrackingEncVels; // lastEncVels = lastTrackingEncVels;
//
// TODO: redo the configs here // // TODO: redo the configs here
leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); // leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); // rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); // strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
//
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) // // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); // leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); // rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); // strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD);
} // }
//
public void resetHeading(double heading) { // public void resetHeading(double heading) {
setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); // setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading));
} // }
//
public static double encoderTicksToInches(double ticks) { // public static double encoderTicksToInches(double ticks) {
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; // return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
} // }
//
@NonNull // @NonNull
@Override // @Override
public List<Double> getWheelPositions() { // public List<Double> getWheelPositions() {
int leftPos = leftEncoder.getCurrentPosition(); // int leftPos = leftEncoder.getCurrentPosition();
int rightPos = rightEncoder.getCurrentPosition(); // int rightPos = rightEncoder.getCurrentPosition();
int frontPos = strafeEncoder.getCurrentPosition(); // int frontPos = strafeEncoder.getCurrentPosition();
//
lastEncPositions.clear(); // lastEncPositions.clear();
lastEncPositions.add(leftPos); // lastEncPositions.add(leftPos);
lastEncPositions.add(rightPos); // lastEncPositions.add(rightPos);
lastEncPositions.add(frontPos); // lastEncPositions.add(frontPos);
//
return Arrays.asList( // return Arrays.asList(
encoderTicksToInches(leftPos) * X_MULTIPLIER, // encoderTicksToInches(leftPos) * X_MULTIPLIER,
encoderTicksToInches(rightPos) * X_MULTIPLIER, // encoderTicksToInches(rightPos) * X_MULTIPLIER,
encoderTicksToInches(frontPos) * Y_MULTIPLIER // encoderTicksToInches(frontPos) * Y_MULTIPLIER
); // );
} // }
//
@NonNull // @NonNull
@Override // @Override
public List<Double> getWheelVelocities() { // public List<Double> getWheelVelocities() {
int leftVel = (int) leftEncoder.getCorrectedVelocity(); // int leftVel = (int) leftEncoder.getCorrectedVelocity();
int rightVel = (int) rightEncoder.getCorrectedVelocity(); // int rightVel = (int) rightEncoder.getCorrectedVelocity();
int frontVel = (int) strafeEncoder.getCorrectedVelocity(); // int frontVel = (int) strafeEncoder.getCorrectedVelocity();
//
lastEncVels.clear(); // lastEncVels.clear();
lastEncVels.add(leftVel); // lastEncVels.add(leftVel);
lastEncVels.add(rightVel); // lastEncVels.add(rightVel);
lastEncVels.add(frontVel); // lastEncVels.add(frontVel);
//
return Arrays.asList( // return Arrays.asList(
encoderTicksToInches(leftVel) * X_MULTIPLIER, // encoderTicksToInches(leftVel) * X_MULTIPLIER,
encoderTicksToInches(rightVel) * X_MULTIPLIER, // encoderTicksToInches(rightVel) * X_MULTIPLIER,
encoderTicksToInches(frontVel) * Y_MULTIPLIER // encoderTicksToInches(frontVel) * Y_MULTIPLIER
); // );
} // }
} //}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java
View File

@ -18,8 +18,8 @@ public class DashboardPoseTracker {
private double[] yPositions; private double[] yPositions;
private PoseUpdater poseUpdater; private PoseUpdater poseUpdater;
private long lastUpdateTime; private long lastUpdateTime;
private final int TRACKING_LENGTH = 1000; private final int TRACKING_LENGTH = 2000;
private final long UPDATE_TIME = 5; private final long UPDATE_TIME = 50;
private final int TRACKING_SIZE = TRACKING_LENGTH / (int) UPDATE_TIME; private final int TRACKING_SIZE = TRACKING_LENGTH / (int) UPDATE_TIME;
/** /**