From 416cab6f51dae21eaab83312eee433e945a39a3a Mon Sep 17 00:00:00 2001
From: Ryan Brott <rcbrott@gmail.com>
Date: Sun, 26 Nov 2023 13:54:10 -0800
Subject: [PATCH] Log power commands

---
 .../ftc/teamcode/DownsampledWriter.java       | 20 ----------
 .../ftc/teamcode/MecanumDrive.java            | 37 ++++++++++++++-----
 .../firstinspires/ftc/teamcode/TankDrive.java | 32 +++++++++++-----
 .../messages/MecanumCommandMessage.java       | 19 ++++++++++
 .../messages/MecanumEncodersMessage.java      | 24 ++++--------
 .../teamcode/messages/TankCommandMessage.java | 15 ++++++++
 .../messages/TankEncodersMessage.java         | 20 ++--------
 .../ThreeDeadWheelEncodersMessage.java        | 18 +++------
 .../messages/TwoDeadWheelEncodersMessage.java | 12 ++----
 9 files changed, 106 insertions(+), 91 deletions(-)
 delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DownsampledWriter.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumCommandMessage.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankCommandMessage.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
deleted file mode 100644
index 608e237..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DownsampledWriter.java
+++ /dev/null
@@ -1,20 +0,0 @@
-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 5b0215a..34dc335 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -23,6 +23,7 @@ import com.acmerobotics.roadrunner.TurnConstraints;
 import com.acmerobotics.roadrunner.Twist2dDual;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.VelConstraint;
+import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
 import com.acmerobotics.roadrunner.ftc.Encoder;
 import com.acmerobotics.roadrunner.ftc.FlightRecorder;
 import com.acmerobotics.roadrunner.ftc.LynxFirmware;
@@ -39,6 +40,7 @@ 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.MecanumCommandMessage;
 import org.firstinspires.ftc.teamcode.messages.MecanumEncodersMessage;
 import org.firstinspires.ftc.teamcode.messages.PoseMessage;
 
@@ -114,6 +116,7 @@ public final class MecanumDrive {
     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);
+    private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000);
 
     public class DriveLocalizer implements Localizer {
         public final Encoder leftFront, leftBack, rightBack, rightFront;
@@ -268,6 +271,7 @@ public final class MecanumDrive {
             }
 
             Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
 
             PoseVelocity2d robotVelRobot = updatePoseEstimate();
 
@@ -281,13 +285,20 @@ public final class MecanumDrive {
             MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
             double voltage = voltageSensor.getVoltage();
 
-            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
-            leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
-            leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage);
-            rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
-            rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
+            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
+                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
+            double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
+            double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
+            double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
+            double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
+            mecanumCommandWriter.write(new MecanumCommandMessage(
+                    voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
+            ));
 
-            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
+            leftFront.setPower(leftFrontPower);
+            leftBack.setPower(leftBackPower);
+            rightBack.setPower(rightBackPower);
+            rightFront.setPower(rightFrontPower);
 
             p.put("x", pose.position.x);
             p.put("y", pose.position.y);
@@ -352,6 +363,7 @@ public final class MecanumDrive {
             }
 
             Pose2dDual<Time> txWorldTarget = turn.get(t);
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
 
             PoseVelocity2d robotVelRobot = updatePoseEstimate();
 
@@ -364,14 +376,21 @@ public final class MecanumDrive {
 
             MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
             double voltage = voltageSensor.getVoltage();
-            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
+            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
+                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
+            double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
+            double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
+            double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
+            double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
+            mecanumCommandWriter.write(new MecanumCommandMessage(
+                    voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
+            ));
+
             leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
             leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage);
             rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
             rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
 
-            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
-
             Canvas c = p.fieldOverlay();
             drawPoseHistory(c);
 
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 5a393d7..a0da555 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
@@ -29,6 +29,7 @@ import com.acmerobotics.roadrunner.Twist2dDual;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.Vector2dDual;
 import com.acmerobotics.roadrunner.VelConstraint;
+import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
 import com.acmerobotics.roadrunner.ftc.Encoder;
 import com.acmerobotics.roadrunner.ftc.FlightRecorder;
 import com.acmerobotics.roadrunner.ftc.LynxFirmware;
@@ -46,6 +47,7 @@ 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.TankCommandMessage;
 import org.firstinspires.ftc.teamcode.messages.TankEncodersMessage;
 
 import java.util.ArrayList;
@@ -119,6 +121,8 @@ public final class TankDrive {
     private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
     private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
 
+    private final DownsampledWriter tankCommandWriter = new DownsampledWriter("TANK_COMMAND", 50_000_000);
+
     public class DriveLocalizer implements Localizer {
         public final List<Encoder> leftEncs, rightEncs;
 
@@ -285,6 +289,7 @@ public final class TankDrive {
             DualNum<Time> x = timeTrajectory.profile.get(t);
 
             Pose2dDual<Arclength> txWorldTarget = timeTrajectory.path.get(x.value(), 3);
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
 
             updatePoseEstimate();
 
@@ -294,12 +299,17 @@ public final class TankDrive {
 
             TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
             double voltage = voltageSensor.getVoltage();
-            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
+            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
+                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
+            double leftPower = feedforward.compute(wheelVels.left) / voltage;
+            double rightPower = feedforward.compute(wheelVels.right) / voltage;
+            tankCommandWriter.write(new TankCommandMessage(voltage, leftPower, rightPower));
+
             for (DcMotorEx m : leftMotors) {
-                m.setPower(feedforward.compute(wheelVels.left) / voltage);
+                m.setPower(leftPower);
             }
             for (DcMotorEx m : rightMotors) {
-                m.setPower(feedforward.compute(wheelVels.right) / voltage);
+                m.setPower(rightPower);
             }
 
             p.put("x", pose.position.x);
@@ -311,8 +321,6 @@ public final class TankDrive {
             p.put("yError", error.position.y);
             p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));
 
-            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
-
             // only draw when active; only one drive action should be active at a time
             Canvas c = p.fieldOverlay();
             drawPoseHistory(c);
@@ -369,6 +377,7 @@ public final class TankDrive {
             }
 
             Pose2dDual<Time> txWorldTarget = turn.get(t);
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
 
             PoseVelocity2d robotVelRobot = updatePoseEstimate();
 
@@ -383,16 +392,19 @@ public final class TankDrive {
 
             TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
             double voltage = voltageSensor.getVoltage();
-            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
+            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
+                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
+            double leftPower = feedforward.compute(wheelVels.left) / voltage;
+            double rightPower = feedforward.compute(wheelVels.right) / voltage;
+            tankCommandWriter.write(new TankCommandMessage(voltage, leftPower, rightPower));
+
             for (DcMotorEx m : leftMotors) {
-                m.setPower(feedforward.compute(wheelVels.left) / voltage);
+                m.setPower(leftPower);
             }
             for (DcMotorEx m : rightMotors) {
-                m.setPower(feedforward.compute(wheelVels.right) / voltage);
+                m.setPower(rightPower);
             }
 
-            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
-
             Canvas c = p.fieldOverlay();
             drawPoseHistory(c);
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumCommandMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumCommandMessage.java
new file mode 100644
index 0000000..bfc3c58
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumCommandMessage.java
@@ -0,0 +1,19 @@
+package org.firstinspires.ftc.teamcode.messages;
+
+public final class MecanumCommandMessage {
+    public long timestamp;
+    public double voltage;
+    public double leftFrontPower;
+    public double leftBackPower;
+    public double rightBackPower;
+    public double rightFrontPower;
+
+    public MecanumCommandMessage(double voltage, double leftFrontPower, double leftBackPower, double rightBackPower, double rightFrontPower) {
+        this.timestamp = System.nanoTime();
+        this.voltage = voltage;
+        this.leftFrontPower = leftFrontPower;
+        this.leftBackPower = leftBackPower;
+        this.rightBackPower = rightBackPower;
+        this.rightFrontPower = rightFrontPower;
+    }
+}
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
index 3a16239..8fa961e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java
@@ -4,24 +4,16 @@ 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 PositionVelocityPair leftFront;
+    public PositionVelocityPair leftBack;
+    public PositionVelocityPair rightBack;
+    public PositionVelocityPair rightFront;
 
     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;
+        this.leftFront = leftFront;
+        this.leftBack = leftBack;
+        this.rightBack = rightBack;
+        this.rightFront = rightFront;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankCommandMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankCommandMessage.java
new file mode 100644
index 0000000..c9ea1df
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankCommandMessage.java
@@ -0,0 +1,15 @@
+package org.firstinspires.ftc.teamcode.messages;
+
+public final class TankCommandMessage {
+    public long timestamp;
+    public double voltage;
+    public double leftPower;
+    public double rightPower;
+
+    public TankCommandMessage(double voltage, double leftPower, double rightPower) {
+        this.timestamp = System.nanoTime();
+        this.voltage = voltage;
+        this.leftPower = leftPower;
+        this.rightPower = rightPower;
+    }
+}
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
index 4f80ab4..cc6c545 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankEncodersMessage.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankEncodersMessage.java
@@ -6,24 +6,12 @@ import java.util.List;
 
 public final class TankEncodersMessage {
     public long timestamp;
-    public int[] rawLeftPositions;
-    public int[] rawLeftVelocities;
-    public int[] rawRightPositions;
-    public int[] rawRightVelocities;
+    public PositionVelocityPair[] left;
+    public PositionVelocityPair[] right;
 
     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;
-        }
+        this.left = left.toArray(new PositionVelocityPair[0]);
+        this.right = right.toArray(new PositionVelocityPair[0]);
     }
 }
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
index e66586d..7d8a63c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelEncodersMessage.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelEncodersMessage.java
@@ -4,20 +4,14 @@ 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 PositionVelocityPair par0;
+    public PositionVelocityPair par1;
+    public PositionVelocityPair perp;
 
     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;
+        this.par0 = par0;
+        this.par1 = par1;
+        this.perp = perp;
     }
 }
\ 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
index aa4fb34..dde7f44 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelEncodersMessage.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelEncodersMessage.java
@@ -4,16 +4,12 @@ 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 PositionVelocityPair par;
+    public PositionVelocityPair perp;
 
     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;
+        this.par = par;
+        this.perp = perp;
     }
 }