updated readmes for the drive pid changes and the OTOS localizer

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

@@ -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
+  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 `26` in the
+  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
+  `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 `127` in the
+  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`.
 

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

@@ -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
+encoder localizer, two tracking wheel localizer, the three tracking wheel localizer, the three
-wheel with IMU localizer offered in Pedro Pathing. Scroll down to the section that applies to you
+wheel with IMU localizer, and the OTOS localizer offered in Pedro Pathing. Scroll down to the section
-and follow the directions there.
+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

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

@@ -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
+        // For the OTOS, left/right is the y axis and forward/backward is the x axis, with left being
-        // positive x and forward being positive y. 0 radians is facing forward, and clockwise
+        // 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();