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')
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.acmerobotics.roadrunner:core:0.5.6'
}

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

@ -1,6 +1,6 @@
## Welcome!
This is the Pedro Pathing path following program developed by FTC team 10158 Scott's Bots in the
2023-2024 Centerstage season.
This is the Pedro Pathing path following program developed by FTC team 10158 Scott's Bots with Logan
Nash in the 2023-2024 Centerstage season.
## Installation
The quickest way to get started is with the quickstart [here](https://github.com/brotherhobo/Pedro-Pathing-Quickstart).
@ -19,11 +19,9 @@ maven { url = 'https://maven.brott.dev/' }
3. Then, add the following code to the end of your `dependencies` block:
```
implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'
```
```
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.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
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 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
You must also have a localizer of some sort. Pedro Pathing has a drive encoder, a two tracking wheel,
and a three tracking 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
* 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
`tuning` package.
correction, and the mass should be put on line `114` in the `FollowerConstants` class under the
`tuning` package.
* 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
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
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
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
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
`FollowerConstants` class.
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
`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
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
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
the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `26` in the
`FollowerConstants` class.
* 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
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.
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
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
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
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
power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `127` in the
`FollowerConstants` class.
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
`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,
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
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
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
`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
`FollowerConstants` class.
* 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
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
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
`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
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
`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
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 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
this will make the robot run more smoothly under actual pathing conditions.
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
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
`smallTranslationalPIDF` will be used. If you need to add a feedforward value, use 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
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
`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
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 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
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
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
"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.
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
"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.
* 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
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
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,
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
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
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
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
it gets too strong. So, just don't use it. P and D are enough.
`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
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
`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
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`
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
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
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.
* 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`
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
`FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease
`centripetalScaling`.
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
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
`centripetalScaling`.
* 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
`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,
refer to the OVERVIEW readme file or the README readme file. Best of luck to your team this season! :)
`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,
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! :)

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

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;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
/**
* This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a
* custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected
* velocity counts and allow reversing independently of the corresponding slot's motor direction.
*
* I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have
* import statements, so I'm not crediting myself as an author for this.
*
* @author Road Runner dev team
* @version 1.0, 5/9/2024
*/
public class RoadRunnerEncoder {
private final static int CPS_STEP = 0x10000;
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;
//
///**
// * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a
// * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected
// * velocity counts and allow reversing independently of the corresponding slot's motor direction.
// *
// * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have
// * import statements, so I'm not crediting myself as an author for this.
// *
// * @author Road Runner dev team
// * @version 1.0, 5/9/2024
// */
//public class RoadRunnerEncoder {
// private final static int CPS_STEP = 0x10000;
//
// 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);
// }
//}

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;
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. I made some minor changes, so I'm crediting myself as an
* 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to
* Pedro Pathing to avoid having imports.
*
* @author Road Runner dev team
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 5/9/2024
*/
@Config
public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer {
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
);
}
}
//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. I made some minor changes, so I'm crediting myself as an
// * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to
// * Pedro Pathing to avoid having imports.
// *
// * @author Road Runner dev team
// * @author Anyi Lin - 10158 Scott's Bots
// * @version 1.0, 5/9/2024
// */
//@Config
//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer {
// 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
// );
// }
//}

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 PoseUpdater poseUpdater;
private long lastUpdateTime;
private final int TRACKING_LENGTH = 1000;
private final long UPDATE_TIME = 5;
private final int TRACKING_LENGTH = 2000;
private final long UPDATE_TIME = 50;
private final int TRACKING_SIZE = TRACKING_LENGTH / (int) UPDATE_TIME;
/**