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

updated readmes for the drive pid changes and the OTOS localizer

Browse Source
This commit is contained in:
brotherhobo
2024-07-21 12:54:47 -04:00
parent 09ae88b27e
commit 70cfd3be44
3 changed files with 110 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -10,7 +10,7 @@ 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
correction, and the mass should be put on line `123` 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
@ -21,8 +21,8 @@ One last thing to note is that Pedro Pathing operates in inches and radians.
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
plenty of room. Once you're done, put the velocity for the `Forward Velocity Tuner` on line `27` in
the `FollowerConstants` class, and the velocity for the `Strafe Velocity Tuner` on line `28` in the
`FollowerConstants` class.
* The last set of automatic tuners you'll need to run are the zero power acceleration tuners. These
@ -36,8 +36,8 @@ One last thing to note is that Pedro Pathing operates in inches and radians.
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
`Forward Zero Power Acceleration Tuner` on line `130` in the `FollowerConstants` class and the zero
power acceleration for the `Lateral Zero Power Acceleration Tuner` on line `134` in the
`FollowerConstants` class.
* After this, we will want to tune the translational PIDs. Go to FTC Dashboard and disable all but
@ -68,7 +68,7 @@ One last thing to note is that Pedro Pathing operates in inches and radians.
`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
oscillations at the end. Lower numbers will do the opposite. This can be found on line `143` 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
@ -78,12 +78,24 @@ One last thing to note is that Pedro Pathing operates in inches and radians.
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.
it gets too strong. So, just don't use it. P and D are enough. In the versions of Pedro Pathing
from after late July 2024, there is a Kalman filter on the drive error and the drive PIDs have a
filter as well. These smooth out the drive error and PID response so that there is not as much
oscillation during the braking portion of each path. The Kalman filter is tuned to have 6 for the
model covariance and 1 for the data covariance. These values should work, but if you feel inclined
to tune the Kalman filter yourself, a higher ratio of model covariance to data covariance means that
the filter will rely more on its previous output rather than the data, and the opposite ratio will
mean that the filter will rely more so on the data input (the raw drive error) rather than the model.
The filtered PIDs function like normal PIDs, except the derivative term is a weighted average of the
current derivative and the previous derivative. Currently, the weighting, or time constant for the
drive filtered PIDs is 0.6, so the derivative output is 0.6 times the previous derivative plus 0.4
times the current derivative. Feel free to mess around with these values and find what works best
for your robot!
* 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
as/after running a path, then increase `centripetalScaling`, which can be found on line `126` of
`FollowerConstants`. If the robot is correcting towards the outside of the curve, then decrease
`centripetalScaling`.

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

@ -3,7 +3,8 @@ This is the localization system developed for the Pedro Pathing path follower. T
the pose exponential method of localization. It's basically a way of turning movements from the
robot's coordinate frame to the global coordinate frame. If you're interested in reading more about
it, then check out pages 177 - 183 of [Controls Engineering in the FIRST Robotics Competition](https://file.tavsys.net/control/controls-engineering-in-frc.pdf)
by Tyler Veness.
by Tyler Veness. However, the OTOS localizer uses its own onboard system for calculating localization,
which I do not know about.
## Setting Your Localizer
Go to line `69` in the `PoseUpdater` class, and replace the `new ThreeWheelLocalizer(hardwareMap)`
@ -13,12 +14,13 @@ with the localizer that applies to you:
* If you're using three wheel odometry, put `new ThreeWheelLocalizer(hardwareMap)`, so basically
don't change it from the default
* If you're using three wheel odometry with the IMU, put `new ThreeWheelIMULocalizer(hardwareMap)`
* If you're using OTOS, put `new OTOSLocalizer(hardwareMap)`
## Tuning
To start, you'll want to select your localizer of choice. Below, I'll have instructions for the drive
encoder localizer, two tracking wheel localizer, the three tracking wheel localizer, and the three
wheel with IMU localizer offered in Pedro Pathing. Scroll down to the section that applies to you
and follow the directions there.
encoder localizer, two tracking wheel localizer, the three tracking wheel localizer, the three
wheel with IMU localizer, and the OTOS localizer offered in Pedro Pathing. Scroll down to the section
that applies to you and follow the directions there.
# Drive Encoders
* First, you'll need all of your drive motors to have encoders attached.
@ -39,19 +41,19 @@ to. The names of the variables is where on the robot the corresponding motor sho
this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure
you replace the number, not add on or multiply it to the previous number. The tuner takes into
account your current multiplier.
* Next, go on to `Forward Localizer Tuner`. You'll want to position a rule alongside your robot.
* Next, go on to `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot.
By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever
you set that value to, then the forward multiplier will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the multiplier you
need to have to scale your current readings to your goal of 30 inches, or the custom set angle.
need to have to scale your current readings to your goal of 30 inches, or the custom set distance.
Feel free to run a few more tests and average the results. Once you have this multiplier, then
replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current multiplier.
* Finally, go to `Lateral Localizer Tuner`. You'll want to position a rule alongside your robot.
* Finally, go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot.
By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever
you set that value to, then the lateral multiplier will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the multiplier you
need to have to scale your current readings to your goal of 30 inches, or the custom set angle.
need to have to scale your current readings to your goal of 30 inches, or the custom set distance.
Feel free to run a few more tests and average the results. Once you have this multiplier, then
replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current multiplier.
@ -81,19 +83,19 @@ to. The names of the variables is where on the robot the corresponding motor sho
world.
* You actually won't need the turning tuner for this one, since the IMU in the Control Hub will take
care of the heading readings.
* First, start with the `Forward Localizer Tuner`. You'll want to position a rule alongside your robot.
* First, start with the `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot.
By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever
you set that value to, then the forward multiplier will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the multiplier you
need to have to scale your current readings to your goal of 30 inches, or the custom set angle.
need to have to scale your current readings to your goal of 30 inches, or the custom set distance.
Feel free to run a few more tests and average the results. Once you have this multiplier, then
replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current multiplier.
* Finally, go to `Lateral Localizer Tuner`. You'll want to position a rule alongside your robot.
* Finally, go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot.
By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever
you set that value to, then the lateral multiplier will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the multiplier you
need to have to scale your current readings to your goal of 30 inches, or the custom set angle.
need to have to scale your current readings to your goal of 30 inches, or the custom set distance.
Feel free to run a few more tests and average the results. Once you have this multiplier, then
replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current multiplier.
@ -125,19 +127,19 @@ to. The names of the variables is where on the robot the corresponding motor sho
this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure
you replace the number, not add on or multiply it to the previous number. The tuner takes into
account your current multiplier.
* Next, go on to `Forward Localizer Tuner`. You'll want to position a rule alongside your robot.
* Next, go on to `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot.
By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever
you set that value to, then the forward multiplier will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the multiplier you
need to have to scale your current readings to your goal of 30 inches, or the custom set angle.
need to have to scale your current readings to your goal of 30 inches, or the custom set distance.
Feel free to run a few more tests and average the results. Once you have this multiplier, then
replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current multiplier.
* Finally, go to `Lateral Localizer Tuner`. You'll want to position a rule alongside your robot.
* Finally, go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot.
By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever
you set that value to, then the lateral multiplier will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the multiplier you
need to have to scale your current readings to your goal of 30 inches, or the custom set angle.
need to have to scale your current readings to your goal of 30 inches, or the custom set distance.
Feel free to run a few more tests and average the results. Once you have this multiplier, then
replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current multiplier.
@ -174,19 +176,19 @@ to. The names of the variables is where on the robot the corresponding motor sho
this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure
you replace the number, not add on or multiply it to the previous number. The tuner takes into
account your current multiplier.
* Next, go on to `Forward Localizer Tuner`. You should re-enable `useIMU` at this time. You'll want to position a rule alongside your robot.
* Next, go on to `Forward Localizer Tuner`. You should re-enable `useIMU` at this time. You'll want to position a ruler alongside your robot.
By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever
you set that value to, then the forward multiplier will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the multiplier you
need to have to scale your current readings to your goal of 30 inches, or the custom set angle.
need to have to scale your current readings to your goal of 30 inches, or the custom set distance.
Feel free to run a few more tests and average the results. Once you have this multiplier, then
replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current multiplier.
* Finally, go to `Lateral Localizer Tuner`. `useIMU` should be enabled for this step. You'll want to position a rule alongside your robot.
* Finally, go to `Lateral Localizer Tuner`. `useIMU` should be enabled for this step. You'll want to position a ruler alongside your robot.
By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever
you set that value to, then the lateral multiplier will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the multiplier you
need to have to scale your current readings to your goal of 30 inches, or the custom set angle.
need to have to scale your current readings to your goal of 30 inches, or the custom set distance.
Feel free to run a few more tests and average the results. Once you have this multiplier, then
replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current multiplier.
@ -197,6 +199,49 @@ to. The names of the variables is where on the robot the corresponding motor sho
robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll
want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer!
# OTOS Localizer
* First, you'll need the OTOS connected to an I2C port on a hub. Make sure the film on the sensor is removed.
* Then, go to `OTOSLocalizer.java`. First, in the constructor, go to where it tells you to replace
the current statement with your OTOS port in the constructor. Replace the `deviceName` parameter
with the name of the port that the OTOS is connected to.
* Next, enter in the position of your OTOS relative to the center of the wheels of the robot. The
positions are in inches, so convert measurements accordingly. Use the comment above the class
declaration as well as to help you with the coordinates.
* First, start with the `Turn Localizer Tuner`. You'll want to position your robot to be facing
in a direction you can easily find again, like lining up an edge of the robot against a field tile edge.
By default, you should spin the robot for one rotation going counterclockwise. Once you've spun
exactly that one rotation, or whatever you set that value to, then the angular scalar will be shown
as the second number shown. The first number is how far the robot thinks you've spun, and the second
number is the scalar you need to have to scale your current readings to your goal of one rotation,
or the custom set angle. Feel free to run a few more tests and average the results. Once you have
this scalar, then replace the angular scalar on line `78` in the localizer with your scalar.
Make sure you replace the number, not add on or multiply it to the previous number. The tuner takes into
account your current angular scalar.
* For this next step, since OTOS only has one linear scalar, you can run either the forward or lateral
localizer tuner and the result should be the same. So, you choose which one you want to run.
* Option 1: go on to `Forward Localizer Tuner`. You'll want to position a ruler alongside your robot.
By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever
you set that value to, then the linear scalar will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the scalar you
need to have to scale your current readings to your goal of 30 inches, or the custom set distance.
Feel free to run a few more tests and average the results. Once you have this scalar, then
replace the linear scalar on line `77` in the localizer with your scalar. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current scalar.
* Option 2: go to `Lateral Localizer Tuner`. You'll want to position a ruler alongside your robot.
By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever
you set that value to, then the linear scalar will be shown as the second number shown. The
first number is how far the robot thinks you've gone, and the second number is the scalar you
need to have to scale your current readings to your goal of 30 inches, or the custom set distance.
Feel free to run a few more tests and average the results. Once you have this scalar, then
replace the linear scalar on line `77` in the localizer with your scalar. Make sure you replace the number,
not add on or multiply it to the previous number. The tuner takes into account your current scalar.
* Once you're done with all this, your localizer should be tuned. To test it out, you can go to
`Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash)
and on the top right, switch the drop down from the default view to the field view. Then, on the bottom
left corner, you should see a field and the robot being drawn on the field. You can then move your
robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll
want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer!
## Using Road Runner's Localizer
Of course, many teams have experience using Road Runner in the past and so have localizers from Road
Runner that are tuned. There is an adapter for the Road Runner three wheel localizer to the Pedro

29
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java
View File

@ -12,7 +12,24 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
/**
* This is the OTOSLocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the SparkFun OTOS.
* localizer that uses the SparkFun OTOS. The diagram below, which is taken from
* Road Runner, shows a typical set up.
*
* The view is from the bottom of the robot looking upwards.
*
* left on robot is y pos
*
* front on robot is x pos
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || | left (y pos)
* | |
* | |
* \--------------/
* front (x pos)
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 7/20/2024
@ -51,10 +68,14 @@ public class OTOSLocalizer extends Localizer {
otos.setAngularUnit(AngleUnit.RADIANS);
// TODO: replace this with your OTOS offset from the center of the robot
// For the OTOS, left/right is the x axis and forward/backward is the y axis, with right being
// positive x and forward being positive y. 0 radians is facing forward, and clockwise
// For the OTOS, left/right is the y axis and forward/backward is the x axis, with left being
// positive y and forward being positive x. PI/2 radians is facing forward, and clockwise
// rotation is negative rotation.
otos.setOffset(new SparkFunOTOS.Pose2D(0,0,0));
otos.setOffset(new SparkFunOTOS.Pose2D(0,0,Math.PI / 2));
// TODO: replace these with your tuned multipliers
otos.setLinearScalar(1.0);
otos.setAngularScalar(1.0);
otos.calibrateImu();
otos.resetTracking();