From 79d6297a46e9202d8436672aa0a914e1ebe61753 Mon Sep 17 00:00:00 2001
From: Ryan Brott <rcbrott@gmail.com>
Date: Mon, 20 Nov 2023 22:59:36 -0800
Subject: [PATCH] Add more debug messages

---
 .../ftc/teamcode/DownsampledWriter.java       | 20 ++++++++++++
 .../ftc/teamcode/MecanumDrive.java            | 27 +++++++++++++---
 .../firstinspires/ftc/teamcode/TankDrive.java | 32 ++++++++++++++++---
 .../messages/DriveCommandMessage.java         | 24 ++++++++++++++
 .../messages/MecanumEncodersMessage.java      | 27 ++++++++++++++++
 .../ftc/teamcode/messages/PoseMessage.java    |  2 +-
 .../messages/TankEncodersMessage.java         | 29 +++++++++++++++++
 .../ThreeDeadWheelEncodersMessage.java        | 23 +++++++++++++
 .../messages/TwoDeadWheelEncodersMessage.java | 19 +++++++++++
 9 files changed, 192 insertions(+), 11 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DownsampledWriter.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/DriveCommandMessage.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankEncodersMessage.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelEncodersMessage.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelEncodersMessage.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DownsampledWriter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DownsampledWriter.java
new file mode 100644
index 0000000..608e237
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DownsampledWriter.java
@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.roadrunner.ftc.FlightRecorder;
+
+public class DownsampledWriter {
+    public final String channel;
+    public final long maxPeriod;
+    private long nextWriteTimestamp = System.nanoTime();
+    public DownsampledWriter(String channel, long maxPeriod) {
+        this.channel = channel;
+        this.maxPeriod = maxPeriod;
+    }
+    public void write(Object msg) {
+        long now = System.nanoTime();
+        if (now >= nextWriteTimestamp) {
+            nextWriteTimestamp = now + maxPeriod;
+            FlightRecorder.write(channel, msg);
+        }
+    }
+}
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 d9b93a0..a05c4f9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -38,6 +38,9 @@ import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.VoltageSensor;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage;
+import org.firstinspires.ftc.teamcode.messages.MecanumEncodersMessage;
+import org.firstinspires.ftc.teamcode.messages.PoseMessage;
 
 import java.lang.Math;
 import java.util.Arrays;
@@ -47,6 +50,12 @@ import java.util.List;
 @Config
 public final class MecanumDrive {
     public static class Params {
+        // IMU orientation
+        public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
+                RevHubOrientationOnRobot.LogoFacingDirection.UP;
+        public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
+                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+
         // drive model parameters
         public double inPerTick = 0;
         public double lateralInPerTick = 1;
@@ -102,6 +111,10 @@ public final class MecanumDrive {
 
     private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
 
+    private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
+    private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
+    private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
+
     public class DriveLocalizer implements Localizer {
         public final Encoder leftFront, leftBack, rightBack, rightFront;
 
@@ -129,6 +142,9 @@ public final class MecanumDrive {
             PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
             PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
 
+            FlightRecorder.write("MECANUM_ENCODERS", new MecanumEncodersMessage(
+                    leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel));
+
             Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
             double headingDelta = heading.minus(lastHeading);
 
@@ -186,8 +202,7 @@ public final class MecanumDrive {
 
         imu = hardwareMap.get(IMU.class, "imu");
         IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
-                RevHubOrientationOnRobot.LogoFacingDirection.UP,
-                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
+                PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
         imu.initialize(parameters);
 
         voltageSensor = hardwareMap.voltageSensor.iterator().next();
@@ -261,6 +276,7 @@ public final class MecanumDrive {
                     PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
             )
                     .compute(txWorldTarget, pose, robotVelRobot);
+            driveCommandWriter.write(new DriveCommandMessage(command));
 
             MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
             double voltage = voltageSensor.getVoltage();
@@ -271,7 +287,7 @@ public final class MecanumDrive {
             rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
             rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
 
-            FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
 
             p.put("x", pose.position.x);
             p.put("y", pose.position.y);
@@ -344,6 +360,7 @@ public final class MecanumDrive {
                     PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
             )
                     .compute(txWorldTarget, pose, robotVelRobot);
+            driveCommandWriter.write(new DriveCommandMessage(command));
 
             MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
             double voltage = voltageSensor.getVoltage();
@@ -353,7 +370,7 @@ public final class MecanumDrive {
             rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
             rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
 
-            FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
 
             Canvas c = p.fieldOverlay();
             drawPoseHistory(c);
@@ -386,7 +403,7 @@ public final class MecanumDrive {
             poseHistory.removeFirst();
         }
 
-        FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose));
+        estimatedPoseWriter.write(new PoseMessage(pose));
 
         return twist.velocity().value();
     }
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 970c484..d92df77 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
@@ -43,6 +43,11 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.VoltageSensor;
 
+import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage;
+import org.firstinspires.ftc.teamcode.messages.MecanumEncodersMessage;
+import org.firstinspires.ftc.teamcode.messages.PoseMessage;
+import org.firstinspires.ftc.teamcode.messages.TankEncodersMessage;
+
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Collections;
@@ -52,6 +57,12 @@ import java.util.List;
 @Config
 public final class TankDrive {
     public static class Params {
+        // IMU orientation
+        public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
+                RevHubOrientationOnRobot.LogoFacingDirection.UP;
+        public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
+                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+
         // drive model parameters
         public double inPerTick = 0;
         public double trackWidthTicks = 0;
@@ -104,6 +115,10 @@ public final class TankDrive {
 
     private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
 
+    private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
+    private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
+    private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
+
     public class DriveLocalizer implements Localizer {
         public final List<Encoder> leftEncs, rightEncs;
 
@@ -135,11 +150,13 @@ public final class TankDrive {
 
         @Override
         public Twist2dDual<Time> update() {
+            List<PositionVelocityPair> leftReadings = new ArrayList<>(), rightReadings = new ArrayList<>();
             double meanLeftPos = 0.0, meanLeftVel = 0.0;
             for (Encoder e : leftEncs) {
                 PositionVelocityPair p = e.getPositionAndVelocity();
                 meanLeftPos += p.position;
                 meanLeftVel += p.velocity;
+                leftReadings.add(p);
             }
             meanLeftPos /= leftEncs.size();
             meanLeftVel /= leftEncs.size();
@@ -149,10 +166,14 @@ public final class TankDrive {
                 PositionVelocityPair p = e.getPositionAndVelocity();
                 meanRightPos += p.position;
                 meanRightVel += p.velocity;
+                rightReadings.add(p);
             }
             meanRightPos /= rightEncs.size();
             meanRightVel /= rightEncs.size();
 
+            FlightRecorder.write("TANK_ENCODERS",
+                    new TankEncodersMessage(leftReadings, rightReadings));
+
             TankKinematics.WheelIncrements<Time> twist = new TankKinematics.WheelIncrements<>(
                     new DualNum<Time>(new double[] {
                             meanLeftPos - lastLeftPos,
@@ -192,8 +213,7 @@ public final class TankDrive {
 
         imu = hardwareMap.get(IMU.class, "imu");
         IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
-                RevHubOrientationOnRobot.LogoFacingDirection.UP,
-                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
+                PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
         imu.initialize(parameters);
 
         voltageSensor = hardwareMap.voltageSensor.iterator().next();
@@ -270,6 +290,7 @@ public final class TankDrive {
 
             PoseVelocity2dDual<Time> command = new RamseteController(kinematics.trackWidth, PARAMS.ramseteZeta, PARAMS.ramseteBBar)
                     .compute(x, txWorldTarget, pose);
+            driveCommandWriter.write(new DriveCommandMessage(command));
 
             TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
             double voltage = voltageSensor.getVoltage();
@@ -290,7 +311,7 @@ public final class TankDrive {
             p.put("yError", error.position.y);
             p.put("headingError (deg)", Math.toDegrees(error.heading.log()));
 
-            FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
 
             // only draw when active; only one drive action should be active at a time
             Canvas c = p.fieldOverlay();
@@ -358,6 +379,7 @@ public final class TankDrive {
                             PARAMS.turnVelGain * (robotVelRobot.angVel - txWorldTarget.heading.velocity().value())
                     )
             );
+            driveCommandWriter.write(new DriveCommandMessage(command));
 
             TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
             double voltage = voltageSensor.getVoltage();
@@ -369,7 +391,7 @@ public final class TankDrive {
                 m.setPower(feedforward.compute(wheelVels.right) / voltage);
             }
 
-            FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
 
             Canvas c = p.fieldOverlay();
             drawPoseHistory(c);
@@ -402,7 +424,7 @@ public final class TankDrive {
             poseHistory.removeFirst();
         }
 
-        FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose));
+        estimatedPoseWriter.write(new PoseMessage(pose));
 
         return twist.velocity().value();
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/DriveCommandMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/DriveCommandMessage.java
new file mode 100644
index 0000000..01565c3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/DriveCommandMessage.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.messages;
+
+import com.acmerobotics.roadrunner.PoseVelocity2dDual;
+import com.acmerobotics.roadrunner.Time;
+
+public final class DriveCommandMessage {
+    public long timestamp;
+    public double forwardVelocity;
+    public double forwardAcceleration;
+    public double lateralVelocity;
+    public double lateralAcceleration;
+    public double angularVelocity;
+    public double angularAcceleration;
+
+    public DriveCommandMessage(PoseVelocity2dDual<Time> poseVelocity) {
+        this.timestamp = System.nanoTime();
+        this.forwardVelocity = poseVelocity.linearVel.x.get(0);
+        this.forwardAcceleration = poseVelocity.linearVel.x.get(1);
+        this.lateralVelocity = poseVelocity.linearVel.y.get(0);
+        this.lateralAcceleration = poseVelocity.linearVel.y.get(1);
+        this.angularVelocity = poseVelocity.angVel.get(0);
+        this.angularAcceleration = poseVelocity.angVel.get(1);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java
new file mode 100644
index 0000000..3a16239
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java
@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.messages;
+
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+
+public final class MecanumEncodersMessage {
+    public long timestamp;
+    public int rawLeftFrontPosition;
+    public int rawLeftFrontVelocity;
+    public int rawLeftBackPosition;
+    public int rawLeftBackVelocity;
+    public int rawRightBackPosition;
+    public int rawRightBackVelocity;
+    public int rawRightFrontPosition;
+    public int rawRightFrontVelocity;
+
+    public MecanumEncodersMessage(PositionVelocityPair leftFront, PositionVelocityPair leftBack, PositionVelocityPair rightBack, PositionVelocityPair rightFront) {
+        this.timestamp = System.nanoTime();
+        this.rawLeftFrontPosition = leftFront.rawPosition;
+        this.rawLeftFrontVelocity = leftFront.rawVelocity;
+        this.rawLeftBackPosition = leftBack.rawPosition;
+        this.rawLeftBackVelocity = leftBack.rawVelocity;
+        this.rawRightBackPosition = rightBack.rawPosition;
+        this.rawRightBackVelocity = rightBack.rawVelocity;
+        this.rawRightFrontPosition = rightFront.rawPosition;
+        this.rawRightFrontVelocity = rightFront.rawVelocity;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java
index b3fcf3e..8d54054 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.messages;
 
 import com.acmerobotics.roadrunner.Pose2d;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankEncodersMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankEncodersMessage.java
new file mode 100644
index 0000000..4f80ab4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankEncodersMessage.java
@@ -0,0 +1,29 @@
+package org.firstinspires.ftc.teamcode.messages;
+
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+
+import java.util.List;
+
+public final class TankEncodersMessage {
+    public long timestamp;
+    public int[] rawLeftPositions;
+    public int[] rawLeftVelocities;
+    public int[] rawRightPositions;
+    public int[] rawRightVelocities;
+
+    public TankEncodersMessage(List<PositionVelocityPair> left, List<PositionVelocityPair> right) {
+        this.timestamp = System.nanoTime();
+        this.rawLeftPositions = new int[left.size()];
+        this.rawLeftVelocities = new int[left.size()];
+        this.rawRightPositions = new int[right.size()];
+        this.rawRightVelocities = new int[right.size()];
+        for (int i = 0; i < left.size(); i++) {
+            this.rawLeftPositions[i] = left.get(i).rawPosition;
+            this.rawLeftVelocities[i] = left.get(i).rawVelocity;
+        }
+        for (int i = 0; i < right.size(); i++) {
+            this.rawRightPositions[i] = right.get(i).rawPosition;
+            this.rawRightVelocities[i] = right.get(i).rawVelocity;
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelEncodersMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelEncodersMessage.java
new file mode 100644
index 0000000..e66586d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelEncodersMessage.java
@@ -0,0 +1,23 @@
+package org.firstinspires.ftc.teamcode.messages;
+
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+
+public final class ThreeDeadWheelEncodersMessage {
+    public long timestamp;
+    public int rawPar0Position;
+    public int rawPar0Velocity;
+    public int rawPar1Position;
+    public int rawPar1Velocity;
+    public int rawPerpPosition;
+    public int rawPerpVelocity;
+
+    public ThreeDeadWheelEncodersMessage(PositionVelocityPair par0, PositionVelocityPair par1, PositionVelocityPair perp) {
+        this.timestamp = System.nanoTime();
+        this.rawPar0Position = par0.rawPosition;
+        this.rawPar0Velocity = par0.rawVelocity;
+        this.rawPar1Position = par1.rawPosition;
+        this.rawPar1Velocity = par1.rawVelocity;
+        this.rawPerpPosition = perp.rawPosition;
+        this.rawPerpVelocity = perp.rawVelocity;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelEncodersMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelEncodersMessage.java
new file mode 100644
index 0000000..aa4fb34
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelEncodersMessage.java
@@ -0,0 +1,19 @@
+package org.firstinspires.ftc.teamcode.messages;
+
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+
+public final class TwoDeadWheelEncodersMessage {
+    public long timestamp;
+    public int rawParPosition;
+    public int rawParVelocity;
+    public int rawPerpPosition;
+    public int rawPerpVelocity;
+
+    public TwoDeadWheelEncodersMessage(PositionVelocityPair par, PositionVelocityPair perp) {
+        this.timestamp = System.nanoTime();
+        this.rawParPosition = par.rawPosition;
+        this.rawParVelocity = par.rawVelocity;
+        this.rawPerpPosition = perp.rawPosition;
+        this.rawPerpVelocity = perp.rawVelocity;
+    }
+}