From 52f3cb91ae25cf2228992e10e341f536430f5633 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Tue, 14 May 2024 08:39:48 -0400 Subject: [PATCH] disabled road runner localizer adapters to reduce number of gradle dependencies --- TeamCode/build.gradle | 2 - .../ftc/teamcode/pedroPathing/README.md | 10 +- .../ftc/teamcode/pedroPathing/TUNING.md | 142 ++++---- .../RRToPedroThreeWheelLocalizer.java | 318 +++++++++--------- .../localization/RoadRunnerEncoder.java | 264 +++++++-------- .../RoadRunnerThreeWheelLocalizer.java | 246 +++++++------- .../util/DashboardPoseTracker.java | 4 +- 7 files changed, 491 insertions(+), 495 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 442471b..c300cd0 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -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' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md index a8cf0cc..35ab001 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/README.md @@ -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' ``` \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index a41a4b2..ddd1672 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -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! :) \ No newline at end of file + `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! :) \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java index 19c6382..15275f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java @@ -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 lastTrackingEncPositions = new ArrayList<>(); - List 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 lastTrackingEncPositions = new ArrayList<>(); +// List 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; +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java index 24ea188..d743b49 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java @@ -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); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java index c2e9b94..2ee75a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java @@ -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 lastEncPositions, lastEncVels; - - public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List 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 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 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 lastEncPositions, lastEncVels; +// +// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List 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 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 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 +// ); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java index 250c7ce..cab5daa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/DashboardPoseTracker.java @@ -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; /**