commit 6079d4fd2ce299b5841cb298422a35033cf2f1b1
author milind upadhyay <milind.upadhyay@gmail.com> Sun Jan 24 13:59:57 2021 -0800
committer milind upadhyay <milind.upadhyay@gmail.com> Wed Jan 27 21:06:23 2021 -0800
tree 9ef3e0945ce3fbc7a0b58e245c0a8715157ab8e9
parent 9c61d98759002ec16c17e3b12e1512c51926fbfd

Add drivetrain position velocity plots

These velocity plots are useful for tuning the robot.

Change-Id: If1df10133141e5a2fcf1bd1916e9d296a7b77af7

frc971/control_loops/drivetrain/drivetrain_plotter.ts

diff --git a/frc971/control_loops/drivetrain/drivetrain_plotter.ts b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
index f1ddd75..90c58cf 100644
--- a/frc971/control_loops/drivetrain/drivetrain_plotter.ts
+++ b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
@@ -176,16 +176,6 @@
   polyRightVelocity.setColor(CYAN);
   polyRightVelocity.setDrawLine(false);
 
-  const splineLeftVelocity = velocityPlot.addMessageLine(
-      status, ['trajectory_logging', 'left_velocity']);
-  splineLeftVelocity.setColor(RED);
-  splineLeftVelocity.setDrawLine(false);
-
-  const splineRightVelocity = velocityPlot.addMessageLine(
-      status, ['trajectory_logging', 'right_velocity']);
-  splineRightVelocity.setColor(GREEN);
-  splineRightVelocity.setDrawLine(false);
-
   const leftVelocity =
       velocityPlot.addMessageLine(status, ['estimated_left_velocity']);
   leftVelocity.setColor(RED);
@@ -193,12 +183,36 @@
       velocityPlot.addMessageLine(status, ['estimated_right_velocity']);
   rightVelocity.setColor(GREEN);
 
+  const leftSpeed = velocityPlot.addMessageLine(position, ["left_speed"]);
+  leftSpeed.setColor(BLUE);
+  const rightSpeed = velocityPlot.addMessageLine(position, ["right_speed"]);
+  rightSpeed.setColor(BROWN);
+
+  // Drivetrain trajectory and localizer velocities
+  const velocityPlot2 = aosPlotter.addPlot(element, [0, currentTop],
+                                          [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+  currentTop += DEFAULT_HEIGHT;
+  velocityPlot2.plot.getAxisLabels().setTitle(
+      "Trajectory and Localizer Velocity Plots");
+  velocityPlot2.plot.getAxisLabels().setXLabel(TIME);
+  velocityPlot2.plot.getAxisLabels().setYLabel('Wheel Velocity (m/s)');
+
+  const splineLeftVelocity = velocityPlot2.addMessageLine(
+      status, ['trajectory_logging', 'left_velocity']);
+  splineLeftVelocity.setColor(RED);
+  splineLeftVelocity.setDrawLine(false);
+
+  const splineRightVelocity = velocityPlot2.addMessageLine(
+      status, ['trajectory_logging', 'right_velocity']);
+  splineRightVelocity.setColor(GREEN);
+  splineRightVelocity.setDrawLine(false);
+
   const ekfLeftVelocity =
-      velocityPlot.addMessageLine(status, ['localizer', 'left_velocity']);
+      velocityPlot2.addMessageLine(status, ['localizer', 'left_velocity']);
   ekfLeftVelocity.setColor(RED);
   ekfLeftVelocity.setPointSize(0.0);
   const ekfRightVelocity =
-      velocityPlot.addMessageLine(status, ['localizer', 'right_velocity']);
+      velocityPlot2.addMessageLine(status, ['localizer', 'right_velocity']);
   ekfRightVelocity.setColor(GREEN);
   ekfRightVelocity.setPointSize(0.0);