From a890105110feccf443142230e419bd9a2225e4ea Mon Sep 17 00:00:00 2001
From: Ryan Brott <rcbrott@gmail.com>
Date: Sat, 23 Dec 2023 13:23:40 -0800
Subject: [PATCH] Add encoder reverse directions to dead wheel localizers

Direction doesn't matter for position regression, but it does matter
for `inPerTick`, `lateralInPerTick`. And everything makes more sense
if the directions are correct.
---
 .../java/org/firstinspires/ftc/teamcode/MecanumDrive.java    | 2 +-
 .../main/java/org/firstinspires/ftc/teamcode/TankDrive.java  | 2 +-
 .../firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java  | 4 ++++
 .../firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java    | 5 +++++
 4 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
index 01dbe5d..5140fb7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -134,7 +134,7 @@ public final class MecanumDrive {
             rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
 
             // TODO: reverse encoders if needed
-            // leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+            //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
 
             lastLeftFrontPos = leftFront.getPositionAndVelocity().position;
             lastLeftBackPos = leftBack.getPositionAndVelocity().position;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
index 9989d93..04b7d88 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
@@ -150,7 +150,7 @@ public final class TankDrive {
             }
 
             // TODO: reverse encoder directions if needed
-            // leftEncs.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
+            //   leftEncs.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
 
             for (Encoder e : leftEncs) {
                 lastLeftPos += e.getPositionAndVelocity().position;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
index 59f6063..b68efcc 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
@@ -11,6 +11,7 @@ import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
 import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
 import com.acmerobotics.roadrunner.ftc.RawEncoder;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 @Config
@@ -37,6 +38,9 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
         par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par1")));
         perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp")));
 
+        // TODO: reverse encoder directions if needed
+        //   par0.setDirection(DcMotorSimple.Direction.REVERSE);
+
         lastPar0Pos = par0.getPositionAndVelocity().position;
         lastPar1Pos = par1.getPositionAndVelocity().position;
         lastPerpPos = perp.getPositionAndVelocity().position;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
index 1ad010d..d0d7ab5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
@@ -12,6 +12,7 @@ import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
 import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
 import com.acmerobotics.roadrunner.ftc.RawEncoder;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
 
@@ -42,6 +43,10 @@ public final class TwoDeadWheelLocalizer implements Localizer {
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
         par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par")));
         perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp")));
+
+        // TODO: reverse encoder directions if needed
+        //   par.setDirection(DcMotorSimple.Direction.REVERSE);
+
         this.imu = imu;
 
         lastParPos = par.getPositionAndVelocity().position;