L120¶

2 file(s) copied, then 4 change(s) in 2 file(s), 19 line(s) of code to write.

Copies¶

lessons/L120CompareLocalizersOpMode.java to mytry/L120CompareLocalizersOpMode.java
lessons/L120EncoderLocalizer.java to mytry/L120EncoderLocalizer.java

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mytry/L120CompareLocalizersOpMode.java

Delete 0 lines, write 1 line.

--- before this change
+++ after it
@@ -9,6 +9,7 @@
 import org.firstinspires.ftc.teamcode.base.CorbelsMecanum;
 import org.firstinspires.ftc.teamcode.base.CorbelsTeleOp;
 import org.firstinspires.ftc.teamcode.base.Tracker;
+import org.firstinspires.ftc.teamcode.base.odometry.HardwareWheelSource;
 
 /**
  * L120: drive around while two localizers disagree. Your localizer reads the

Delete 0 lines, write 2 lines.

--- before this change
+++ after it
@@ -109,6 +109,8 @@
         // When on L120S030, see
         // https://msb-stitt.github.io/ftc31459-26-27-biobuzz/guide/tasks/l120.html#l120s030
         // for what to do here.
+        shadowLocalizers.add("driveWheelEncoders",
+                new L120EncoderLocalizer(new HardwareWheelSource(hardware)));
     }
 
     // 'public' to match the parent class. 'public' allows access from any code.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mytry/L120EncoderLocalizer.java

Delete 0 lines, write 13 lines.

--- before this change
+++ after it
@@ -51,6 +51,19 @@
         // When on L120S040, see
         // https://msb-stitt.github.io/ftc31459-26-27-biobuzz/guide/tasks/l120.html#l120s040
         // for what to do here.
+        if (lastWheels == null) {
+            lastWheels = wheels.clone();
+            lastRawHeading = rawHeading;
+            return;
+        }
+
+        double frontLeft = wheels[0] - lastWheels[0];
+        double frontRight = wheels[1] - lastWheels[1];
+        double backLeft = wheels[2] - lastWheels[2];
+        double backRight = wheels[3] - lastWheels[3];
+
+        double forward = (frontLeft + frontRight + backLeft + backRight) / 4.0;
+        double strafe = (-frontLeft + frontRight + backLeft - backRight) / 4.0;
 
         // When on L120S050, see
         // https://msb-stitt.github.io/ftc31459-26-27-biobuzz/guide/tasks/l120.html#l120s050

Delete 0 lines, write 3 lines.

--- before this change
+++ after it
@@ -68,6 +68,9 @@
         // When on L120S050, see
         // https://msb-stitt.github.io/ftc31459-26-27-biobuzz/guide/tasks/l120.html#l120s050
         // for what to do here.
+        double heading = rawHeading + headingOffset;
+        x += forward * Math.cos(heading) - strafe * Math.sin(heading);
+        y += forward * Math.sin(heading) + strafe * Math.cos(heading);
 
         lastWheels = wheels.clone();
         lastRawHeading = rawHeading;