Add initial quickstart files

.gitignore

@@ -85,3 +85,6 @@ lint/generated/
 lint/outputs/
 lint/tmp/
 # lint/reports/
+
+.DS_Store
+/.idea

TeamCode/build.gradle

@@ -19,7 +19,18 @@ android {
     namespace = 'org.firstinspires.ftc.teamcode'
 }
 
+repositories {
+    maven {
+        url = 'https://maven.brott.dev/'
+    }
+}
+
 dependencies {
     implementation project(':FtcRobotController')
     annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
+
+    implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'
+    implementation 'com.acmerobotics.roadrunner:core:1.0.0-beta0'
+
+    implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
 }

TeamCode/src/main/assets/tuning/.gitignore

@@ -0,0 +1,2 @@
+latest.json
+*.py

TeamCode/src/main/assets/tuning/common.js

@@ -0,0 +1,210 @@
+// TODO: time-interpolate data
+
+function fitLinearWithScaling(xs, ys) {
+  const xOffset = xs.reduce((a, b) => a + b, 0) / xs.length;
+  const yOffset = ys.reduce((a, b) => a + b, 0) / ys.length;
+
+  const xScale = xs.reduce((acc, x) => Math.max(acc, Math.abs(x - xOffset)), 0);
+  const yScale = ys.reduce((acc, y) => Math.max(acc, Math.abs(y - yOffset)), 0);
+
+  const data = xs.map((x, i) => [(x - xOffset) / xScale, (ys[i] - yOffset) / yScale]);
+
+  const result = regression.linear(data);
+  const [m, b] = result.equation;
+
+  return [m * yScale / xScale, b * yScale - m * xOffset * yScale / xScale + yOffset];
+}
+
+// no output for first pair
+function numDerivOnline(xs, ys) {
+  if (xs.length !== ys.length) {
+    throw new Error(`${xs.length} !== ${ys.length}`);
+  }
+
+  return ys
+    .slice(1)
+    .map((y, i) => (y - ys[i]) / (xs[i + 1] - xs[i]));
+}
+
+// no output for first or last pair
+function numDerivOffline(xs, ys) {
+  return ys
+    .slice(2)
+    .map((y, i) => (y - ys[i]) / (xs[i + 2] - xs[i]));
+}
+
+const CPS_STEP = 0x10000;
+
+function inverseOverflow(input, estimate) {
+  // convert to uint16
+  let real = input & 0xffff;
+  // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
+  // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
+  real += ((real % 20) / 4) * CPS_STEP;
+  // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
+  real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
+  return real;
+}
+
+// no output for first or last pair
+function fixVels(ts, xs, vs) {
+  if (ts.length !== xs.length || ts.length !== vs.length) {
+    throw new Error();
+  }
+
+  return numDerivOffline(ts, xs).map((est, i) => inverseOverflow(vs[i + 1], est));
+}
+
+// data comes in pairs
+function newLinearRegressionChart(container, xs, ys, options, onChange) {
+  if (xs.length !== ys.length) {
+    throw new Error(`${xs.length} !== ${ys.length}`);
+  }
+
+  // cribbed from https://plotly.com/javascript/plotlyjs-events/#select-event
+  const color = '#777';
+  const colorLight = '#bbb';
+
+  let mask = xs.map(() => true);
+
+  const [m, b] = fitLinearWithScaling(xs, ys);
+
+  if (onChange) onChange(m, b);
+
+  const minX = xs.reduce((a, b) => Math.min(a, b), 0);
+  const maxX = xs.reduce((a, b) => Math.max(a, b), 0);
+
+  const chartDiv = document.createElement('div');
+  Plotly.newPlot(chartDiv, [{
+    type: 'scatter',
+    mode: 'markers',
+    x: xs,
+    y: ys,
+    name: 'Samples',
+    // markers seem to respond to selection 
+    marker: {color: mask.map(b => b ? color : colorLight), size: 5},
+  }, {
+    type: 'scatter',
+    mode: 'lines',
+    x: [minX, maxX],
+    y: [m * minX + b, m * maxX + b],
+    name: 'Regression Line',
+    line: {color: 'red'}
+  }], {
+    title: options.title || '',
+    // sets the starting tool from the modebar
+    dragmode: 'select',
+    showlegend: false,
+    hovermode: false,
+    width: 600,
+  }, {
+    // 'select2d' left
+    modeBarButtonsToRemove: ['zoom2d', 'pan2d', 'lasso2d', 'zoomIn2d', 'zoomOut2d', 'autoScale2d', 'resetScale2d'],
+  });
+
+  const results = document.createElement('p');
+
+  function setResultText(m, b) {
+    results.innerText = `${options.slope || 'slope'}: ${m}, ${options.intercept || 'y-intercept'}: ${b}`;
+  }
+  setResultText(m, b);
+
+  function updatePlot() {
+    Plotly.restyle(chartDiv, 'marker.color', [
+      mask.map(b => b ? color : colorLight)
+    ], [0]);
+
+    const [m, b] = fitLinearWithScaling(
+      xs.filter((_, i) => mask[i]),
+      ys.filter((_, i) => mask[i]),
+    );
+    setResultText(m, b);
+    if (onChange) onChange(m, b);
+
+    const minX = xs.reduce((a, b) => Math.min(a, b));
+    const maxX = xs.reduce((a, b) => Math.max(a, b));
+
+    Plotly.restyle(chartDiv, {
+      x: [[minX, maxX]],
+      y: [[m * minX + b, m * maxX + b]],
+    }, [1]);
+  }
+
+  let pendingSelection = null;
+
+  chartDiv.on('plotly_selected', function(eventData) {
+    pendingSelection = eventData;
+  });
+
+  function applyPendingSelection(b) {
+    if (pendingSelection === null) return false;
+
+    for (const pt of pendingSelection.points) {
+      mask[pt.pointIndex] = b;
+    }
+
+    Plotly.restyle(chartDiv, 'selectedpoints', [null], [0]);
+
+    pendingSelection = null;
+
+    return true;
+  }
+
+  const includeButton = document.createElement('button');
+  includeButton.innerText = '[i]nclude';
+  includeButton.addEventListener('click', () => {
+    if (!applyPendingSelection(true)) return;
+    updatePlot();
+  });
+
+  const excludeButton = document.createElement('button');
+  excludeButton.innerText = '[e]xclude';
+  excludeButton.addEventListener('click', () => {
+    if (!applyPendingSelection(false)) return;
+    updatePlot();
+  });
+
+  document.addEventListener('keydown', e => {
+    if (e.key === 'i') {
+      if (!applyPendingSelection(true)) return;
+      updatePlot();
+    } else if (e.key === 'e') {
+      if (!applyPendingSelection(false)) return;
+      updatePlot();
+    }
+  });
+
+  while (container.firstChild) {
+    container.removeChild(container.firstChild);
+  }
+
+  const buttons = document.createElement('div');
+  buttons.appendChild(includeButton);
+  buttons.appendChild(excludeButton);
+
+  const bar = document.createElement('div');
+  bar.setAttribute('class', 'bar');
+  bar.appendChild(buttons);
+
+  bar.appendChild(results);
+
+  container.appendChild(bar);
+  container.appendChild(chartDiv);
+
+  return function(xsNew, ysNew) {
+    if (xsNew.length !== ysNew.length) {
+      throw new Error(`${xsNew.length} !== ${ysNew.length}`);
+    }
+
+    xs = xsNew;
+    ys = ysNew;
+    mask = xs.map(() => true);
+
+    Plotly.restyle(chartDiv, {
+      x: [xs],
+      y: [ys],
+    }, [0]);
+
+    updatePlot();
+  };
+}

TeamCode/src/main/assets/tuning/dead-wheel-angular-ramp.html

@@ -0,0 +1,159 @@
+<!doctype html>
+<html>
+  <head>
+    <title>RR Dead Wheel Angular Ramp Regression</title>
+
+    <style>
+body {
+  font-family: Arial, Helvetica, sans-serif;
+}
+
+.content {
+  max-width: 600px;
+  margin: auto;
+}
+
+.bar {
+  display: flex;
+  justify-content: space-between;
+  align-items: center;
+}
+
+fieldset {
+  display: flex;
+  justify-content: space-between;
+}
+
+h1 {
+  margin-bottom: 0;
+}
+
+details, a {
+  display: block;
+  margin: 1rem 0 1rem 0;
+}
+    </style>
+
+    <script src="/tuning/plotly-2.12.1.min.js"></script>
+
+    <!-- https://tom-alexander.github.io/regression-js/ -->
+    <script src="/tuning/regression-2.0.1.min.js"></script>
+
+    <!-- <script src="/tuning/common.js"></script> -->
+    <script src="common.js"></script>
+  </head>
+  <body>
+    <div class="content">
+      <h1>RR Dead Wheel Angular Ramp Regression</h1>
+      <details></details>
+
+      <div id="download"></div>
+
+      <fieldset>
+        <legend>Feedforward Parameters</legend>
+        <div>
+          kV: <input id="kv" type="text" />
+        </div>
+        <div>
+          kS: <input id="ks" type="text" />
+        </div>
+        <input id="update" type="button" value="update" />
+      </fieldset>
+
+      <div id="trackWidthChart">
+        <p>
+          <button id="latest">Latest</button>
+          <input id="browse" type="file" accept="application/json">
+        </p>
+      </div>
+    
+      <div id="deadWheelCharts"></div>
+    </div>
+
+    <script>
+function loadRegression(data) {
+  const [_, angVels] = data.angVels.reduce((acc, vsArg) => {
+    const vs = vsArg.map(v => Math.abs(v));
+    const maxV = vs.reduce((acc, v) => Math.max(acc, v), 0);
+    const [accMaxV, _] = acc;
+    if (maxV >= accMaxV) {
+      return [maxV, vs];
+    }
+    return acc;
+  }, [0, []]);
+  
+  const deadWheelCharts = document.getElementById('deadWheelCharts');
+  data.parEncVels.forEach((vs, i) => {
+    const div = document.createElement('div');
+    newLinearRegressionChart(div, 
+      angVels.slice(1, angVels.length - 1),
+      fixVels(data.encTimes, data.parEncPositions[i], vs),
+      {title: `Parallel Wheel ${i} Regression`, slope: 'y-position'});
+    deadWheelCharts.appendChild(div);
+  });
+  data.perpEncVels.forEach((vs, i) => {
+    const div = document.createElement('div');
+    newLinearRegressionChart(div, 
+      angVels.slice(1, angVels.length - 1),
+      fixVels(data.encTimes, data.perpEncPositions[i], vs),
+      {title: `Perpendicular Wheel ${i} Regression`, slope: 'x-position'});
+    deadWheelCharts.appendChild(div);
+  });
+
+  const setParams = (() => {
+    const appliedVoltages = data.voltages.map((v, i) => 
+      [...data.leftPowers, ...data.rightPowers].reduce((acc, ps) => Math.max(acc, ps[i]), 0) * v);
+
+    const setTrackWidthData = newLinearRegressionChart(
+      document.getElementById('trackWidthChart'),
+      [], [],
+      {title: 'Track Width Regression', slope: 'track width'}
+    );
+
+    return (kV, kS) => setTrackWidthData(angVels, appliedVoltages.map((v, i) => 
+      (v - kS) / kV * (data.type === 'mecanum' ? 2 : 1)));
+  })();
+
+  const kvInput = document.getElementById('kv');
+  const ksInput = document.getElementById('ks');
+  document.getElementById('update').addEventListener('click', () => {
+    setParams(parseFloat(kvInput.value), parseFloat(ksInput.value));
+  });
+}
+
+const latestButton = document.getElementById('latest');
+latestButton.addEventListener('click', function() {
+  fetch('/tuning/angular-ramp/latest.json')
+    .then(res => {
+      if (res.ok) {
+        const filename = res.headers.get('X-Filename');
+
+        const a = document.createElement('a');
+        a.innerText = 'Download';
+        a.href = `/tuning/forward-ramp/${filename}`;
+        a.download = `forward-ramp-${filename}`;
+
+        document.getElementById('download').appendChild(a);
+
+        return res.json();
+      } else {
+        document.getElementById('trackWidthChart').innerText = 'No data files found';
+        throw new Error();
+      }
+    })
+    .then(loadRegression)
+    .catch(console.log.bind(console));
+});
+
+const browseInput = document.getElementById('browse');
+browseInput.addEventListener('change', function(evt) {
+  const reader = new FileReader();
+  reader.onload = function() {
+    loadRegression(JSON.parse(reader.result.trim()));
+  };
+
+  reader.readAsText(browseInput.files[0]);
+});
+    </script>
+  </body>
+</html>

TeamCode/src/main/assets/tuning/drive-encoder-angular-ramp.html

@@ -0,0 +1,141 @@
+<!doctype html>
+<html>
+  <head>
+    <title>RR Drive Encoder Angular Ramp Regression</title>
+
+    <style>
+body {
+  font-family: Arial, Helvetica, sans-serif;
+}
+
+.content {
+  max-width: 600px;
+  margin: auto;
+}
+
+.bar {
+  display: flex;
+  justify-content: space-between;
+  align-items: center;
+}
+
+h1 {
+  margin-bottom: 0;
+}
+
+details, a {
+  display: block;
+  margin: 1rem 0 1rem 0;
+}
+    </style>
+
+    <script src="/tuning/plotly-2.12.1.min.js"></script>
+
+    <!-- https://tom-alexander.github.io/regression-js/ -->
+    <script src="/tuning/regression-2.0.1.min.js"></script>
+
+    <!-- <script src="/tuning/common.js"></script> -->
+    <script src="common.js"></script>
+  </head>
+  <body>
+    <div class="content">
+      <h1>RR Drive Encoder Angular Ramp Regression</h1>
+      <details></details>
+
+      <div id="download"></div>
+      
+      <div id="rampChart">
+        <p>
+          <button id="latest">Latest</button>
+          <input id="browse" type="file" accept="application/json">
+        </p>
+      </div>
+      <div id="trackWidthChart"></div>
+    </div>
+
+    <script>
+function loadRegression(data) {
+  const leftEncVels = data.leftEncVels.map((vs, i) =>
+        fixVels(data.encTimes, data.leftEncPositions[i], vs));
+  const rightEncVels = data.rightEncVels.map((vs, i) =>
+        fixVels(data.encTimes, data.rightEncPositions[i], vs));
+
+  newLinearRegressionChart(
+    document.getElementById('rampChart'),
+    [
+      ...leftEncVels.flatMap(vs => vs.map(v => -v)), 
+      ...rightEncVels.flatMap(vs => vs),
+    ],
+    [
+      ...data.leftPowers.flatMap(ps => {
+        const psNew = ps.map((p, i) => -p * data.voltages[i]);
+        return psNew.slice(1, psNew.length - 1);
+      }),
+      ...data.rightPowers.flatMap(ps => {
+        const psNew = ps.map((p, i) => p * data.voltages[i]);
+        return psNew.slice(1, psNew.length - 1);
+      }),
+    ],
+    {title: 'Ramp Regression', slope: 'kV', intercept: 'kS'}
+  );
+
+  const p = data.angVels.reduce((acc, vsArg) => {
+    const vs = vsArg.map(v => Math.abs(v));
+    const maxV = vs.reduce((acc, v) => Math.max(acc, v), 0);
+    const [accMaxV, _] = acc;
+    if (maxV >= accMaxV) {
+      return [maxV, vs];
+    }
+    return acc;
+  }, [0, []]);
+  const angVels = p[1].slice(1, p[1].length - 1);
+
+  newLinearRegressionChart(
+    document.getElementById('trackWidthChart'),
+    angVels,
+    angVels.map((_, i) => 
+      (leftEncVels.reduce((acc, vs) => acc - vs[i], 0)
+        + rightEncVels.reduce((acc, vs) => acc + vs[i], 0))
+        / (data.leftEncVels.length + data.rightEncVels.length)
+        * (data.type === 'mecanum' ? 1 : 2)
+    ),
+    {title: 'Track Width Regression', slope: 'track width'}
+  );
+}
+
+const latestButton = document.getElementById('latest');
+latestButton.addEventListener('click', function() {
+  fetch('/tuning/angular-ramp/latest.json')
+    .then(res => {
+      if (res.ok) {
+        const filename = res.headers.get('X-Filename');
+
+        const a = document.createElement('a');
+        a.innerText = 'Download';
+        a.href = `/tuning/forward-ramp/${filename}`;
+        a.download = `forward-ramp-${filename}`;
+
+        document.getElementById('download').appendChild(a);
+
+        return res.json();
+      } else {
+        document.getElementById('trackWidthChart').innerText = 'No data files found';
+        throw new Error();
+      }
+    })
+    .then(loadRegression)
+    .catch(console.log.bind(console));
+});
+
+const browseInput = document.getElementById('browse');
+browseInput.addEventListener('change', function(evt) {
+  const reader = new FileReader();
+  reader.onload = function() {
+    loadRegression(JSON.parse(reader.result.trim()));
+  };
+
+  reader.readAsText(browseInput.files[0]);
+});
+    </script>
+  </body>
+</html>

TeamCode/src/main/assets/tuning/forward-ramp.html

@@ -0,0 +1,108 @@
+<!doctype html>
+<html>
+  <head>
+    <title>RR Forward Ramp Regression</title>
+
+    <style>
+body {
+  font-family: Arial, Helvetica, sans-serif;
+}
+
+.content {
+  max-width: 600px;
+  margin: auto;
+}
+
+.bar {
+  display: flex;
+  justify-content: space-between;
+  align-items: center;
+}
+
+h1 {
+  margin-bottom: 0;
+}
+
+details, a {
+  display: block;
+  margin: 1rem 0 1rem 0;
+}
+    </style>
+
+    <script src="/tuning/plotly-2.12.1.min.js"></script>
+
+    <!-- https://tom-alexander.github.io/regression-js/ -->
+    <script src="/tuning/regression-2.0.1.min.js"></script>
+
+    <!-- <script src="/tuning/common.js"></script> -->
+    <script src="common.js"></script>
+  </head>
+  <body>
+    <div class="content">
+      <h1>RR Forward Ramp Regression</h1>
+      <details></details>
+
+      <div id="download"></div>
+
+      <div id="rampChart">
+        <p>
+          <button id="latest">Latest</button>
+          <input id="browse" type="file" accept="application/json">
+        </p>
+      </div>
+    </div>
+
+    <script>
+function loadRegression(data) {
+  const forwardEncVels = data.forwardEncVels.flatMap((vs, i) => 
+    fixVels(data.encTimes, data.forwardEncPositions[i], vs));
+  const appliedVoltages = data.forwardEncVels.flatMap(vs => {
+    const voltages = data.voltages.map((v, i) => 
+      data.powers.reduce((acc, ps) => Math.max(acc, ps[i]), 0) * v);
+
+    return voltages.slice(1, voltages.length - 1);
+  });
+
+  newLinearRegressionChart(
+    document.getElementById('rampChart'),
+    forwardEncVels, appliedVoltages,
+    {title: 'Ramp Regression', slope: 'kV', intercept: 'kS'}
+  );
+}
+
+const latestButton = document.getElementById('latest');
+latestButton.addEventListener('click', function() {
+  fetch('/tuning/forward-ramp/latest.json')
+    .then(res => {
+      if (res.ok) {
+        const filename = res.headers.get('X-Filename');
+
+        const a = document.createElement('a');
+        a.innerText = 'Download';
+        a.href = `/tuning/forward-ramp/${filename}`;
+        a.download = `forward-ramp-${filename}`;
+
+        document.getElementById('download').appendChild(a);
+
+        return res.json();
+      } else {
+        document.getElementById('rampChart').innerText = 'No data files found';
+        throw new Error();
+      }
+    })
+    .then(loadRegression)
+    .catch(console.log.bind(console));
+});
+
+const browseInput = document.getElementById('browse');
+browseInput.addEventListener('change', function(evt) {
+  const reader = new FileReader();
+  reader.onload = function() {
+    loadRegression(JSON.parse(reader.result.trim()));
+  };
+
+  reader.readAsText(browseInput.files[0]);
+});
+    </script>
+  </body>
+</html>

TeamCode/src/main/assets/tuning/test.html

@@ -0,0 +1,118 @@
+<!doctype html>
+<html>
+  <head>
+    <title>RR Tests</title>
+
+    <style>
+body {
+  font-family: Arial, Helvetica, sans-serif;
+}
+
+#content {
+  max-width: 600px;
+  margin: auto;
+}
+
+.bar {
+  display: flex;
+  justify-content: space-between;
+  align-items: center;
+}
+
+fieldset {
+  display: flex;
+  justify-content: space-between;
+}
+    </style>
+
+    <script src="/tuning/plotly-2.12.1.min.js"></script>
+
+    <!-- https://tom-alexander.github.io/regression-js/ -->
+    <script src="/tuning/regression-2.0.1.min.js"></script>
+
+    <!-- <script src="/tuning/common.js"></script> -->
+    <script src="common.js"></script>
+  </head>
+  <body>
+    <div id="content">
+      <h1>RR Tests</h1>
+      <p></p>
+    </div>
+
+    <script>
+function newChart(container, xs, ys, options) {
+  if (xs.length !== ys.length) {
+    throw new Error();
+  }
+
+  // cribbed from https://plotly.com/javascript/plotlyjs-events/#select-event
+  const color = '#777';
+  const colorLight = '#bbb';
+
+  const chartDiv = document.createElement('div');
+  Plotly.newPlot(chartDiv, [{
+    type: 'scatter',
+    mode: 'markers',
+    x: xs,
+    y: ys,
+    name: 'Samples',
+  }], {
+    title: options.title || '',
+    // sets the starting tool from the modebar
+    dragmode: 'zoom',
+    showlegend: false,
+    hovermode: false,
+    width: 600,
+  }, {
+    // 'zoom2d', 'select2d', 'resetScale2d' left
+    modeBarButtonsToRemove: ['pan2d', 'lasso2d', 'zoomIn2d', 'zoomOut2d', 'autoScale2d'],
+  });
+
+  container.appendChild(chartDiv);
+}
+
+function numDeriv(xs, ys) {
+  const ret = ys.slice(1).map((y, i) => (y - ys[i]) / (xs[i + 1] - xs[i]));
+  ret.unshift(0);
+  return ret;
+}
+
+const params = new URLSearchParams(window.location.search);
+const online = params.get('online') === 'true';
+
+fetch(`/tuning/test/${params.get('file')}.csv`)
+  .then(res => res.text())
+  .then(text => {
+    const lines = text.split('\n');
+    const data = lines
+      .slice(1, lines.length - 1)
+      .map(line => line.split(',').map(x => parseFloat(x)))
+    
+    const xs = data.map(xs => xs[0]);
+    const vs = data.map(xs => xs[1]);
+    const ts = data.map(xs => xs[2]);
+
+    const container = document.getElementById('content');
+    newChart(container, ts, vs, {title: 'Raw Velocity Over Time'});
+
+    const tsNew = (online ? ts.slice(1) : ts.slice(1, ts.length - 1));
+    const dxdts = (online ? numDerivOnline : numDerivOffline)(ts, xs);
+    newChart(
+      container, 
+      tsNew,
+      dxdts, 
+      {title: `${online ? 'Online' : 'Offline'} Position Derivative Over Time`}
+    );
+
+    const vsNew = dxdts.map((est, i) => inverseOverflow(vs[i + 1], est));
+    newChart(
+      container, 
+      tsNew,
+      vsNew, 
+      {title: 'Corrected Velocity Over Time'}
+    );
+  })
+  .catch(console.log.bind(console));
+    </script>
+  </body>
+</html>

TeamCode/src/main/assets/tuning/test/enc1.csv

@@ -0,0 +1,219 @@
+pos,vel,time
+248474,-23244.000000,2250.761012
+256050,-23244.000000,2250.792090
+261029,-23044.000000,2250.812276
+265827,-23104.000000,2250.832364
+270750,-23404.000000,2250.853347
+275564,-23584.000000,2250.872950
+280730,-23244.000000,2250.895051
+285076,-23464.000000,2250.912948
+289471,-23244.000000,2250.931340
+295065,-23284.000000,2250.954710
+299341,-25184.000000,2250.972536
+304300,-23244.000000,2250.993309
+308457,-23284.000000,2251.011054
+313913,-19684.000000,2251.033582
+318075,-23244.000000,2251.051005
+323228,-23244.000000,2251.072429
+327140,-23264.000000,2251.089102
+333217,-23224.000000,2251.114403
+337719,-23904.000000,2251.133205
+342702,-23244.000000,2251.154108
+347838,-25104.000000,2251.176498
+352899,-23244.000000,2251.196812
+356826,-23224.000000,2251.213216
+361581,-23704.000000,2251.233083
+368943,-24464.000000,2251.263826
+373285,-26404.000000,2251.282037
+379505,-22004.000000,2251.309094
+383474,-23244.000000,2251.324622
+387117,-23244.000000,2251.340667
+391575,-23244.000000,2251.358790
+396711,-23264.000000,2251.380079
+401699,-23264.000000,2251.400926
+406717,-23244.000000,2251.421884
+409944,-23204.000000,2251.435380
+413772,-23224.000000,2251.451349
+417264,-23204.000000,2251.466198
+421303,-26524.000000,2251.482893
+426176,-23244.000000,2251.503336
+430443,-23264.000000,2251.521113
+434626,-23244.000000,2251.538644
+438799,-23244.000000,2251.556161
+442742,-23224.000000,2251.572622
+447089,-23244.000000,2251.590881
+453249,-23244.000000,2251.617309
+458068,-23184.000000,2251.637047
+462471,-23244.000000,2251.655411
+466317,-23144.000000,2251.671379
+470697,-23264.000000,2251.689707
+474177,-23244.000000,2251.704229
+479150,-23324.000000,2251.725121
+483939,-25004.000000,2251.745112
+491062,-23164.000000,2251.774896
+494543,-21484.000000,2251.789724
+500688,-23224.000000,2251.815150
+504903,-23244.000000,2251.832885
+511649,-23004.000000,2251.861069
+515151,-23204.000000,2251.875656
+518685,-19624.000000,2251.890619
+523224,-23464.000000,2251.909380
+526924,-23044.000000,2251.924890
+530833,-23244.000000,2251.941235
+535098,-23204.000000,2251.960571
+539936,-23264.000000,2251.979646
+543748,-23224.000000,2251.995638
+548572,-23244.000000,2252.015653
+552433,-24164.000000,2252.031729
+557705,-23784.000000,2252.053808
+561934,-26824.000000,2252.071556
+567329,-23204.000000,2252.095088
+572295,-26624.000000,2252.115297
+576805,-23224.000000,2252.133881
+581977,-23224.000000,2252.155739
+586408,-26444.000000,2252.173936
+590398,-23244.000000,2252.190626
+595590,-23924.000000,2252.212343
+599207,-23244.000000,2252.227654
+603242,-23244.000000,2252.244466
+607609,-23244.000000,2252.262794
+614652,-23204.000000,2252.292170
+619146,-23244.000000,2252.311059
+622979,-22664.000000,2252.327068
+627846,-23224.000000,2252.347457
+633154,-23184.000000,2252.369717
+637078,-22184.000000,2252.386073
+640701,-25544.000000,2252.401287
+645375,-23284.000000,2252.420821
+648981,-23244.000000,2252.435949
+654161,-23244.000000,2252.457477
+658606,-23244.000000,2252.476239
+662804,-25584.000000,2252.493831
+668316,-23524.000000,2252.517067
+673628,-24444.000000,2252.539001
+678030,-21984.000000,2252.557527
+682885,-23224.000000,2252.577781
+687296,-23244.000000,2252.596224
+692505,-23224.000000,2252.618159
+697508,-23224.000000,2252.639068
+702048,-23224.000000,2252.657998
+706651,-19884.000000,2252.677227
+711625,-23224.000000,2252.698037
+715826,-22884.000000,2252.715718
+719878,-26664.000000,2252.732631
+724862,-23224.000000,2252.753627
+729844,-23244.000000,2252.774331
+736614,-23244.000000,2252.802776
+742164,-23244.000000,2252.826129
+746464,-20804.000000,2252.843850
+750788,-24924.000000,2252.862006
+756810,-23444.000000,2252.887314
+760924,-26104.000000,2252.904438
+764524,-19844.000000,2252.921106
+768979,-23304.000000,2252.938150
+773681,-19264.000000,2252.957928
+777718,-23244.000000,2252.974788
+781049,-22944.000000,2252.988631
+785994,-23244.000000,2253.009379
+789758,-23244.000000,2253.025102
+793909,-23524.000000,2253.043339
+798770,-23244.000000,2253.062814
+802715,-23244.000000,2253.079364
+807435,-23264.000000,2253.099066
+812425,-23244.000000,2253.120624
+818405,-23244.000000,2253.145323
+823488,-23224.000000,2253.166277
+829127,-23164.000000,2253.190012
+832976,-19544.000000,2253.206141
+837650,-23464.000000,2253.225559
+842266,-23304.000000,2253.244854
+846607,-24584.000000,2253.263160
+851639,-23244.000000,2253.284145
+858413,-21904.000000,2253.312487
+863782,-23084.000000,2253.335002
+868416,-22604.000000,2253.354487
+872070,-23244.000000,2253.370572
+878893,-19844.000000,2253.398231
+883737,-23244.000000,2253.418440
+888833,-23224.000000,2253.439822
+893419,-19284.000000,2253.458970
+897196,-23084.000000,2253.474870
+902037,-22944.000000,2253.495095
+907126,-23384.000000,2253.516501
+911457,-23304.000000,2253.534620
+915036,-23244.000000,2253.549473
+918703,-23464.000000,2253.564725
+922150,-23264.000000,2253.579166
+926206,-23224.000000,2253.596137
+929921,-25544.000000,2253.612078
+935607,-21524.000000,2253.635612
+939298,-23224.000000,2253.650993
+943992,-19384.000000,2253.670674
+947577,-23224.000000,2253.685643
+951071,-23244.000000,2253.700454
+954379,-23244.000000,2253.714179
+958191,-23244.000000,2253.730135
+961467,-23244.000000,2253.743823
+965640,-23244.000000,2253.761255
+971113,-23144.000000,2253.784276
+975102,-23264.000000,2253.800994
+982332,-26884.000000,2253.831418
+987134,-23204.000000,2253.851282
+991476,-23224.000000,2253.869580
+998376,-23244.000000,2253.898533
+1003029,-23264.000000,2253.917688
+1006182,-24384.000000,2253.930955
+1009529,-23264.000000,2253.944858
+1013493,-23484.000000,2253.961474
+1017676,-21784.000000,2253.978978
+1022450,-22944.000000,2253.999023
+1026989,-23244.000000,2254.018213
+1031401,-19784.000000,2254.036665
+1034652,-23504.000000,2254.050116
+1038581,-23244.000000,2254.066574
+1041878,-27044.000000,2254.080600
+1045956,-23244.000000,2254.097374
+1049433,-26864.000000,2254.111969
+1053669,-19444.000000,2254.130439
+1057749,-23244.000000,2254.146902
+1062475,-23244.000000,2254.166818
+1066081,-26944.000000,2254.181746
+1070140,-23244.000000,2254.198629
+1075407,-23204.000000,2254.220697
+1079072,-23244.000000,2254.236007
+1084601,-20144.000000,2254.259302
+1087932,-23244.000000,2254.273096
+1091886,-23244.000000,2254.289636
+1095864,-23244.000000,2254.306367
+1101299,-23124.000000,2254.329030
+1104096,-24024.000000,2254.340732
+1108368,-23244.000000,2254.358593
+1113178,-23324.000000,2254.378866
+1118365,-23264.000000,2254.400462
+1122387,-22104.000000,2254.417421
+1127728,-23224.000000,2254.439639
+1130869,-23224.000000,2254.452799
+1135472,-23224.000000,2254.472088
+1139371,-23264.000000,2254.488465
+1144081,-23364.000000,2254.508248
+1147467,-23264.000000,2254.522271
+1151382,-23224.000000,2254.538681
+1155340,-26244.000000,2254.555306
+1158704,-26624.000000,2254.569437
+1163265,-23224.000000,2254.588579
+1168164,-23664.000000,2254.609042
+1171458,-21764.000000,2254.622896
+1175749,-23184.000000,2254.640765
+1180715,-24984.000000,2254.661701
+1185782,-23504.000000,2254.682607
+1189757,-23244.000000,2254.699292
+1192891,-21484.000000,2254.712542
+1197589,-23244.000000,2254.732060
+1200873,-23204.000000,2254.745806
+1204655,-23244.000000,2254.761746
+1209806,-23244.000000,2254.783387
+1213506,-23244.000000,2254.798709
+1218559,-23244.000000,2254.819937
+1224138,-25024.000000,2254.844733
+1229075,-23264.000000,2254.863865
+1233410,-23264.000000,2254.882126

TeamCode/src/main/assets/tuning/test/enc2.csv

@@ -0,0 +1,308 @@
+pos,vel,time
+21311553,-22004.000000,2630.100978
+21314576,-21444.000000,2630.113690
+21317933,-21124.000000,2630.127594
+21322869,-20764.000000,2630.147987
+21326584,-22364.000000,2630.163649
+21330750,-21704.000000,2630.181378
+21334526,-22064.000000,2630.196542
+21337667,-20504.000000,2630.209557
+21341251,-21424.000000,2630.224387
+21345317,-22544.000000,2630.241257
+21347976,-20224.000000,2630.252348
+21351819,-21184.000000,2630.268339
+21356687,-21424.000000,2630.289332
+21361749,-21404.000000,2630.309873
+21365248,-21424.000000,2630.324209
+21369737,-21444.000000,2630.342763
+21373504,-20864.000000,2630.358452
+21377887,-21444.000000,2630.376642
+21382615,-21984.000000,2630.396245
+21386548,-22004.000000,2630.412612
+21390627,-21104.000000,2630.429547
+21393808,-21424.000000,2630.442766
+21398987,-21404.000000,2630.464427
+21404106,-21424.000000,2630.485542
+21407651,-23324.000000,2630.500307
+21410763,-21364.000000,2630.513357
+21414388,-21444.000000,2630.528793
+21418221,-19484.000000,2630.544281
+21422088,-21464.000000,2630.560155
+21425408,-21424.000000,2630.573959
+21428522,-21364.000000,2630.586916
+21434566,-21424.000000,2630.612020
+21437886,-21404.000000,2630.625799
+21441512,-21344.000000,2630.640912
+21446809,-21444.000000,2630.662977
+21453007,-21544.000000,2630.689078
+21456588,-22304.000000,2630.703880
+21460250,-21404.000000,2630.718762
+21463642,-21424.000000,2630.732934
+21468257,-20884.000000,2630.752030
+21472711,-21424.000000,2630.770521
+21475891,-21444.000000,2630.784033
+21480805,-21444.000000,2630.804094
+21484942,-21444.000000,2630.821242
+21489067,-21404.000000,2630.838375
+21493147,-21424.000000,2630.855406
+21496826,-21424.000000,2630.870663
+21500154,-21424.000000,2630.884467
+21504252,-19924.000000,2630.901564
+21507980,-21424.000000,2630.918144
+21511429,-21204.000000,2630.931418
+21514299,-20484.000000,2630.943274
+21517171,-21404.000000,2630.955257
+21521422,-22524.000000,2630.972930
+21524430,-22304.000000,2630.985401
+21527847,-21964.000000,2630.999520
+21530891,-21684.000000,2631.012158
+21534726,-21424.000000,2631.028229
+21538908,-20824.000000,2631.045573
+21542549,-21444.000000,2631.060689
+21546094,-21424.000000,2631.075328
+21549985,-21284.000000,2631.091479
+21555480,-21504.000000,2631.114398
+21558556,-21424.000000,2631.127334
+21563090,-21004.000000,2631.145955
+21567609,-21404.000000,2631.164746
+21571072,-21384.000000,2631.179143
+21575066,-21964.000000,2631.195837
+21578997,-20264.000000,2631.212074
+21582521,-21484.000000,2631.226756
+21587136,-20804.000000,2631.245971
+21590550,-21444.000000,2631.260113
+21593535,-21424.000000,2631.272443
+21597455,-21324.000000,2631.288825
+21601479,-21424.000000,2631.305501
+21605825,-20944.000000,2631.323623
+21609882,-21584.000000,2631.340317
+21613809,-21444.000000,2631.356649
+21616878,-21884.000000,2631.369369
+21619944,-22004.000000,2631.382072
+21622933,-22084.000000,2631.395674
+21626676,-21024.000000,2631.410088
+21629868,-21364.000000,2631.423533
+21633982,-21424.000000,2631.440434
+21637160,-20824.000000,2631.453688
+21640196,-21444.000000,2631.466217
+21643840,-21484.000000,2631.481344
+21647479,-21904.000000,2631.496618
+21651524,-22244.000000,2631.513514
+21654942,-21264.000000,2631.527464
+21657684,-21964.000000,2631.538900
+21660629,-20724.000000,2631.551281
+21664299,-21384.000000,2631.566610
+21667796,-21324.000000,2631.580934
+21670664,-21624.000000,2631.592743
+21674556,-22104.000000,2631.608996
+21679365,-21664.000000,2631.628943
+21682841,-21444.000000,2631.643661
+21686481,-21404.000000,2631.658446
+21691187,-21444.000000,2631.678137
+21694388,-21384.000000,2631.691603
+21697907,-21424.000000,2631.705977
+21701038,-21204.000000,2631.718976
+21704519,-21444.000000,2631.733885
+21708990,-20784.000000,2631.752013
+21713031,-21704.000000,2631.768808
+21716167,-21384.000000,2631.781820
+21720672,-22084.000000,2631.800576
+21724269,-21404.000000,2631.815498
+21727539,-21444.000000,2631.829054
+21731424,-20604.000000,2631.845186
+21735030,-21404.000000,2631.860173
+21738592,-21424.000000,2631.875038
+21742793,-21224.000000,2631.892460
+21745814,-21064.000000,2631.905028
+21748613,-21124.000000,2631.916671
+21751940,-21444.000000,2631.930456
+21755194,-20824.000000,2631.943937
+21759056,-21544.000000,2631.960035
+21762734,-21424.000000,2631.975266
+21765548,-21444.000000,2631.986996
+21768654,-21904.000000,2631.999915
+21772692,-21164.000000,2632.016768
+21777187,-21204.000000,2632.035422
+21782053,-21444.000000,2632.056841
+21785724,-21684.000000,2632.070879
+21790542,-21664.000000,2632.090936
+21794824,-21404.000000,2632.108640
+21798126,-21244.000000,2632.122369
+21802902,-21164.000000,2632.142112
+21806829,-21424.000000,2632.158435
+21810941,-21424.000000,2632.175803
+21815650,-21864.000000,2632.195132
+21818828,-21444.000000,2632.208379
+21822006,-21204.000000,2632.221548
+21825906,-20024.000000,2632.237691
+21829297,-20964.000000,2632.251842
+21833933,-21644.000000,2632.271071
+21837141,-21424.000000,2632.284474
+21840128,-21764.000000,2632.296917
+21843518,-21404.000000,2632.312295
+21849082,-21464.000000,2632.334011
+21853510,-23044.000000,2632.352408
+21856939,-21144.000000,2632.366674
+21860215,-21164.000000,2632.380234
+21864557,-20064.000000,2632.398330
+21868115,-21184.000000,2632.413174
+21872367,-21684.000000,2632.430737
+21876174,-20764.000000,2632.446589
+21880253,-21664.000000,2632.463672
+21883421,-21284.000000,2632.477140
+21886724,-21204.000000,2632.490382
+21889862,-22104.000000,2632.503570
+21894216,-21704.000000,2632.521493
+21897544,-21504.000000,2632.535374
+21902474,-21844.000000,2632.556015
+21906875,-21604.000000,2632.574155
+21911198,-21604.000000,2632.592024
+21915400,-21484.000000,2632.609479
+21919141,-21004.000000,2632.625022
+21924786,-20824.000000,2632.648480
+21928752,-21164.000000,2632.664931
+21932739,-21364.000000,2632.681565
+21935865,-22044.000000,2632.694497
+21939202,-21684.000000,2632.708501
+21943443,-21884.000000,2632.726085
+21946321,-21424.000000,2632.737980
+21949711,-20824.000000,2632.751987
+21953529,-21324.000000,2632.768031
+21958129,-21204.000000,2632.787078
+21960658,-21924.000000,2632.797537
+21964114,-21444.000000,2632.812134
+21968092,-21424.000000,2632.828727
+21971138,-21664.000000,2632.841064
+21974525,-21364.000000,2632.855055
+21977419,-21384.000000,2632.867124
+21980717,-21444.000000,2632.880869
+21983893,-22064.000000,2632.894036
+21987570,-21504.000000,2632.909275
+21991327,-21084.000000,2632.924862
+21995241,-21444.000000,2632.941161
+21998017,-20684.000000,2632.952652
+22001364,-21144.000000,2632.966680
+22004593,-21584.000000,2632.980014
+22007741,-21444.000000,2632.993139
+22011556,-21124.000000,2633.008952
+22015682,-21604.000000,2633.026147
+22019838,-21424.000000,2633.043646
+22023539,-21724.000000,2633.058772
+22028056,-21424.000000,2633.077522
+22031628,-21404.000000,2633.092404
+22035624,-21124.000000,2633.108948
+22039499,-21264.000000,2633.125452
+22043316,-21144.000000,2633.140956
+22048918,-21404.000000,2633.164197
+22052772,-21404.000000,2633.180203
+22055739,-21724.000000,2633.192616
+22059995,-21584.000000,2633.210125
+22064276,-21564.000000,2633.227979
+22067784,-21424.000000,2633.242490
+22071476,-21484.000000,2633.257862
+22074595,-21164.000000,2633.270794
+22077416,-21484.000000,2633.282452
+22080194,-22024.000000,2633.294006
+22083018,-21424.000000,2633.305723
+22087683,-21424.000000,2633.325186
+22092295,-20804.000000,2633.344353
+22096115,-21224.000000,2633.360416
+22098666,-21924.000000,2633.370918
+22102562,-21164.000000,2633.387139
+22105805,-22064.000000,2633.400455
+22109085,-21404.000000,2633.414107
+22113237,-21444.000000,2633.431376
+22117574,-20884.000000,2633.449377
+22120773,-21404.000000,2633.462648
+22123931,-21384.000000,2633.475785
+22127471,-21364.000000,2633.490530
+22130760,-21444.000000,2633.504302
+22133891,-21424.000000,2633.517210
+22137895,-21424.000000,2633.533946
+22142004,-20624.000000,2633.550858
+22146093,-21384.000000,2633.567935
+22150700,-21684.000000,2633.587200
+22154457,-22244.000000,2633.602548
+22157319,-21464.000000,2633.614509
+22161064,-21404.000000,2633.630123
+22165386,-20624.000000,2633.648038
+22170174,-20884.000000,2633.667936
+22174795,-21224.000000,2633.687150
+22178617,-22204.000000,2633.702921
+22183530,-23084.000000,2633.723570
+22187907,-21664.000000,2633.741529
+22191909,-21344.000000,2633.758210
+22196352,-21724.000000,2633.776750
+22200117,-21424.000000,2633.792437
+22204032,-21384.000000,2633.808574
+22207376,-21424.000000,2633.822495
+22211809,-22584.000000,2633.841043
+22216936,-21584.000000,2633.862221
+22220897,-21244.000000,2633.879035
+22224184,-20284.000000,2633.892385
+22227786,-20984.000000,2633.907323
+22231563,-21444.000000,2633.923051
+22236196,-22684.000000,2633.942172
+22239737,-21804.000000,2633.956956
+22244593,-21344.000000,2633.977134
+22248330,-20144.000000,2633.992633
+22252493,-21224.000000,2634.010091
+22256302,-21564.000000,2634.025699
+22260055,-21444.000000,2634.041301
+22264259,-21684.000000,2634.058748
+22269658,-21444.000000,2634.081234
+22274308,-22804.000000,2634.100588
+22278913,-21364.000000,2634.119664
+22282401,-21444.000000,2634.134131
+22285532,-20784.000000,2634.147557
+22288878,-21464.000000,2634.160989
+22294190,-21444.000000,2634.183208
+22298392,-22064.000000,2634.200538
+22301580,-21164.000000,2634.213721
+22305571,-21164.000000,2634.230373
+22310386,-20824.000000,2634.250315
+22313956,-21704.000000,2634.265111
+22317597,-21644.000000,2634.280333
+22320816,-21964.000000,2634.293633
+22324282,-21444.000000,2634.308154
+22327153,-21124.000000,2634.320154
+22330875,-21444.000000,2634.335554
+22335035,-22384.000000,2634.352784
+22339134,-21664.000000,2634.369883
+22343278,-21424.000000,2634.387139
+22347382,-21424.000000,2634.404049
+22350788,-21464.000000,2634.418131
+22355419,-21424.000000,2634.437422
+22358311,-20984.000000,2634.449550
+22361218,-21444.000000,2634.461525
+22364953,-21404.000000,2634.477044
+22367891,-21164.000000,2634.489223
+22371928,-21404.000000,2634.505974
+22375376,-21424.000000,2634.520331
+22379065,-21684.000000,2634.535653
+22382923,-20664.000000,2634.551741
+22386187,-21144.000000,2634.565202
+22389450,-21424.000000,2634.578852
+22392865,-21444.000000,2634.593248
+22396941,-21404.000000,2634.610261
+22400182,-21704.000000,2634.623624
+22404137,-21264.000000,2634.639918
+22407683,-21424.000000,2634.654504
+22410807,-21084.000000,2634.667511
+22415781,-21564.000000,2634.688281
+22419518,-21204.000000,2634.703767
+22423611,-21724.000000,2634.720774
+22426584,-21384.000000,2634.733206
+22430757,-20604.000000,2634.750427
+22435565,-21464.000000,2634.770852
+22439137,-21164.000000,2634.785177
+22442871,-22264.000000,2634.800789
+22447211,-21424.000000,2634.818859
+22450519,-21424.000000,2634.832555
+22454045,-20304.000000,2634.847252
+22458028,-21284.000000,2634.867426
+22462813,-21444.000000,2634.883903
+22467171,-22244.000000,2634.901685
+22470413,-21584.000000,2634.915088
+22473383,-21344.000000,2634.927426
+22477314,-22124.000000,2634.944091

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Action.java

@@ -0,0 +1,261 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collections;
+import java.util.List;
+
+public interface Action {
+    void init();
+    default boolean loop(TelemetryPacket p) {
+        return false;
+    }
+
+    default void draw(Canvas c) {
+
+    }
+
+    default void runBlocking() {
+        if (Thread.currentThread().isInterrupted()) return;
+
+        init();
+
+        boolean b = true;
+        while (!Thread.currentThread().isInterrupted() && b) {
+            TelemetryPacket p = new TelemetryPacket();
+
+            draw(p.fieldOverlay());
+            b = loop(p);
+
+            FtcDashboard.getInstance().sendTelemetryPacket(p);
+        }
+    }
+
+    default double clock() {
+        return 1e-9 * System.nanoTime();
+    }
+
+    // not persistent!
+    interface IBuilder<T extends IBuilder<T>> {
+        T add(Action a);
+    }
+
+    // parent-less, generic-less SeqBuilder
+    class Builder implements IBuilder<Builder> {
+        private final List<Action> as = new ArrayList<>();
+
+        @Override
+        public Builder add(Action a) {
+            as.add(a);
+            return this;
+        }
+
+        public SeqBuilder<Builder> seq() {
+            return new SeqBuilder<>(this);
+        }
+
+        public ParBuilder<Builder> par() {
+            return new ParBuilder<>(this);
+        }
+
+        public Builder sleep(double duration) {
+            return add(new SleepAction(duration));
+        }
+
+        public Builder after(double duration, Action a) {
+            return seq().sleep(duration).add(a).end();
+        }
+
+        public Action build() {
+            return new SequentialAction(as);
+        }
+    }
+
+    class SeqBuilder<T extends IBuilder<T>> implements IBuilder<SeqBuilder<T>> {
+        private final T parent;
+        private final List<Action> as = new ArrayList<>();
+
+        private SeqBuilder(T parent) {
+            this.parent = parent;
+        }
+
+        @Override
+        public SeqBuilder<T> add(Action a) {
+            as.add(a);
+            return this;
+        }
+
+        public T end() {
+            parent.add(new SequentialAction(as));
+            return parent;
+        }
+
+        public SeqBuilder<SeqBuilder<T>> seq() {
+            return new SeqBuilder<>(this);
+        }
+
+        public ParBuilder<SeqBuilder<T>> par() {
+            return new ParBuilder<>(this);
+        }
+
+        public SeqBuilder<T> sleep(double duration) {
+            return add(new SleepAction(duration));
+        }
+
+        public SeqBuilder<T> after(double duration, Action a) {
+            return seq().sleep(duration).add(a).end();
+        }
+    }
+
+    final class ParBuilder<T extends IBuilder<T>> implements IBuilder<ParBuilder<T>> {
+        private final T parent;
+        private final List<Action> as = new ArrayList<>();
+
+        private ParBuilder(T parent) {
+            this.parent = parent;
+        }
+
+        @Override
+        public ParBuilder<T> add(Action a) {
+            as.add(a);
+            return this;
+        }
+
+        public T end() {
+            parent.add(new ParallelAction(as));
+            return parent;
+        }
+
+        public SeqBuilder<ParBuilder<T>> seq() {
+            return new SeqBuilder<>(this);
+        }
+
+        public ParBuilder<ParBuilder<T>> par() {
+            return new ParBuilder<>(this);
+        }
+
+        public ParBuilder<T> sleep(double duration) {
+            return add(new SleepAction(duration));
+        }
+
+        public ParBuilder<T> after(double duration, Action a) {
+            return seq().sleep(duration).add(a).end();
+        }
+    }
+}
+
+final class SleepAction implements Action {
+    public final double duration;
+    private double endTs;
+
+    public SleepAction(double duration) {
+        this.duration = duration;
+    }
+
+    @Override
+    public void init() {
+        endTs = clock() + duration;
+    }
+
+    @Override
+    public boolean loop(TelemetryPacket p) {
+        return clock() <= endTs;
+    }
+}
+
+final class ParallelAction implements Action {
+    public final List<Action> actions;
+    private List<Action> remaining;
+
+    public ParallelAction(List<Action> actions) {
+        this.actions = Collections.unmodifiableList(actions);
+    }
+
+    public ParallelAction(Action... actions) {
+        this(Arrays.asList(actions));
+    }
+
+    @Override
+    public void init() {
+        remaining = new ArrayList<>(actions);
+        for (Action a : remaining) {
+            a.init();
+        }
+    }
+
+    @Override
+    public boolean loop(TelemetryPacket p) {
+        List<Action> newRem = new ArrayList<>();
+        for (Action a : remaining) {
+            if (a.loop(p)) {
+                newRem.add(a);
+            }
+        }
+
+        remaining = newRem;
+
+        return remaining.size() > 0;
+    }
+
+    @Override
+    public void draw(Canvas c) {
+        for (Action a : actions) {
+            draw(c);
+        }
+    }
+}
+
+final class SequentialAction implements Action {
+    public final List<Action> actions;
+    private int index;
+    private boolean needsInit;
+
+    public SequentialAction(List<Action> actions) {
+        this.actions = Collections.unmodifiableList(actions);
+    }
+
+    public SequentialAction(Action... actions) {
+        this(Arrays.asList(actions));
+    }
+
+    @Override
+    public void init() {
+        if (actions.isEmpty()) {
+            return;
+        }
+
+        actions.get(0).init();
+    }
+
+    @Override
+    public boolean loop(TelemetryPacket p) {
+        if (index >= actions.size()) {
+            return false;
+        }
+
+        Action a = actions.get(index);
+
+        if (needsInit) {
+            a.init();
+            needsInit = false;
+        }
+
+        if (!a.loop(p)) {
+            index++;
+            needsInit = true;
+        }
+
+        return index < actions.size();
+    }
+
+    @Override
+    public void draw(Canvas c) {
+        for (Action a : actions) {
+            draw(c);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java

@@ -0,0 +1,476 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.AccelConstraint;
+import com.acmerobotics.roadrunner.AngularVelConstraint;
+import com.acmerobotics.roadrunner.DualNum;
+import com.acmerobotics.roadrunner.HolonomicController;
+import com.acmerobotics.roadrunner.MecanumKinematics;
+import com.acmerobotics.roadrunner.MinVelConstraint;
+import com.acmerobotics.roadrunner.MotorFeedforward;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.Pose2dDual;
+import com.acmerobotics.roadrunner.ProfileAccelConstraint;
+import com.acmerobotics.roadrunner.Profiles;
+import com.acmerobotics.roadrunner.Rotation2d;
+import com.acmerobotics.roadrunner.Rotation2dDual;
+import com.acmerobotics.roadrunner.SafeTrajectoryBuilder;
+import com.acmerobotics.roadrunner.Time;
+import com.acmerobotics.roadrunner.TimeProfile;
+import com.acmerobotics.roadrunner.TimeTrajectory;
+import com.acmerobotics.roadrunner.Trajectory;
+import com.acmerobotics.roadrunner.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.Twist2d;
+import com.acmerobotics.roadrunner.Twist2dDual;
+import com.acmerobotics.roadrunner.Twist2dIncrDual;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.VelConstraint;
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
+import org.firstinspires.ftc.teamcode.util.Encoder;
+import org.firstinspires.ftc.teamcode.util.Localizer;
+import org.firstinspires.ftc.teamcode.util.LynxFirmwareVersion;
+import org.firstinspires.ftc.teamcode.util.OverflowEncoder;
+import org.firstinspires.ftc.teamcode.util.RawEncoder;
+
+import java.util.Arrays;
+import java.util.LinkedList;
+import java.util.List;
+
+@Config
+public final class MecanumDrive {
+    // drive model parameters
+    public static double IN_PER_TICK = 0;
+    public static double LATERAL_IN_PER_TICK = 1;
+    public static double TRACK_WIDTH_TICKS = 0;
+
+    // feedforward parameters
+    public static double kS = 0;
+    public static double kV = 0;
+    public static double kA = 0;
+
+    // path profile parameters
+    public static double MAX_WHEEL_VEL = 50;
+    public static double MIN_PROFILE_ACCEL = -30;
+    public static double MAX_PROFILE_ACCEL = 50;
+
+    // turn profile parameters
+    public static double MAX_ANG_VEL = Math.PI; // shared with path
+    public static double MAX_ANG_ACCEL = Math.PI;
+
+    // path controller gains
+    public static double AXIAL_GAIN = 0.0;
+    public static double LATERAL_GAIN = 0.0;
+    public static double HEADING_GAIN = 0.0; // shared with turn
+
+    public static double AXIAL_VEL_GAIN = 0.0;
+    public static double LATERAL_VEL_GAIN = 0.0;
+    public static double HEADING_VEL_GAIN = 0.0; // shared with turn
+
+    public final MecanumKinematics kinematics = new MecanumKinematics(
+            IN_PER_TICK * TRACK_WIDTH_TICKS,
+            IN_PER_TICK / LATERAL_IN_PER_TICK);
+
+    public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV, kA);
+
+    public final VelConstraint defaultVelConstraint =
+            new MinVelConstraint(Arrays.asList(
+                    kinematics.new WheelVelConstraint(MAX_WHEEL_VEL),
+                    new AngularVelConstraint(MAX_ANG_VEL)
+            ));
+    public final AccelConstraint defaultAccelConstraint =
+            new ProfileAccelConstraint(MIN_PROFILE_ACCEL, MAX_PROFILE_ACCEL);
+
+    public final DcMotorEx leftFront, leftBack, rightBack, rightFront;
+
+    public final VoltageSensor voltageSensor;
+
+    public final BNO055Wrapper imu;
+
+    public final Localizer localizer;
+    public Pose2d pose;
+
+    public final double inPerTick = IN_PER_TICK;
+
+    private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
+
+    public class DriveLocalizer implements Localizer {
+        public final Encoder leftFront, leftRear, rightRear, rightFront;
+
+        private int lastLeftFrontPos, lastLeftRearPos, lastRightRearPos, lastRightFrontPos;
+
+        public DriveLocalizer() {
+            leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
+            leftRear = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack));
+            rightRear = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
+            rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
+
+            lastLeftFrontPos = leftFront.getPositionAndVelocity().position;
+            lastLeftRearPos = leftRear.getPositionAndVelocity().position;
+            lastRightRearPos = rightRear.getPositionAndVelocity().position;
+            lastRightFrontPos = rightFront.getPositionAndVelocity().position;
+        }
+
+        @Override
+        public Twist2dIncrDual<Time> updateAndGetIncr() {
+            Encoder.PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
+            Encoder.PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity();
+            Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
+            Encoder.PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
+
+            MecanumKinematics.WheelIncrements<Time> incrs = new MecanumKinematics.WheelIncrements<>(
+                    new DualNum<Time>(new double[]{
+                            leftFrontPosVel.position - lastLeftFrontPos,
+                            leftFrontPosVel.velocity,
+                    }).times(inPerTick),
+                    new DualNum<Time>(new double[]{
+                            leftRearPosVel.position - lastLeftRearPos,
+                            leftRearPosVel.velocity,
+                    }).times(inPerTick),
+                    new DualNum<Time>(new double[]{
+                            rightRearPosVel.position - lastRightRearPos,
+                            rightRearPosVel.velocity,
+                    }).times(inPerTick),
+                    new DualNum<Time>(new double[]{
+                            rightFrontPosVel.position - lastRightFrontPos,
+                            rightFrontPosVel.velocity,
+                    }).times(inPerTick)
+            );
+
+            lastLeftFrontPos = leftFrontPosVel.position;
+            lastLeftRearPos = leftRearPosVel.position;
+            lastRightRearPos = rightRearPosVel.position;
+            lastRightFrontPos = rightFrontPosVel.position;
+
+            return kinematics.forward(incrs);
+        }
+    }
+
+    public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
+        this.pose = pose;
+
+        LynxFirmwareVersion.throwIfAnyModulesBelowVersion(hardwareMap,
+                new LynxFirmwareVersion(1, 8, 2));
+
+        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+            module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        }
+
+        leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
+        leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
+        rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
+        rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
+
+        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        BNO055IMU baseImu = hardwareMap.get(BNO055IMU.class, "imu");
+        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+        parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
+        baseImu.initialize(parameters);
+        imu = new BNO055Wrapper(baseImu);
+
+        voltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+        localizer = new DriveLocalizer();
+    }
+
+    public void setDrivePowers(Twist2d powers) {
+        MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(Twist2dDual.constant(powers, 1));
+        leftFront.setPower(wheelVels.leftFront.get(0));
+        leftBack.setPower(wheelVels.leftBack.get(0));
+        rightBack.setPower(wheelVels.rightBack.get(0));
+        rightFront.setPower(wheelVels.rightFront.get(0));
+    }
+
+    public final class FollowTrajectoryAction implements Action {
+        public final Trajectory trajectory;
+        private TimeTrajectory timeTrajectory;
+        private double beginTs;
+
+        private boolean active;
+        private final double[] xPoints, yPoints;
+
+        public FollowTrajectoryAction(Trajectory t) {
+            this.trajectory = t;
+            timeTrajectory = new TimeTrajectory(t);
+
+            List<Double> disps = com.acmerobotics.roadrunner.Math.range(
+                    0, t.path.length(),
+                    (int) Math.ceil(t.path.length() / 2));
+            xPoints = new double[disps.size()];
+            yPoints = new double[disps.size()];
+            for (int i = 0; i < disps.size(); i++) {
+                Pose2d p = t.path.get(disps.get(i), 1).value();
+                xPoints[i] = p.trans.x;
+                yPoints[i] = p.trans.y;
+            }
+        }
+
+        @Override
+        public void init() {
+            beginTs = clock();
+
+            active = true;
+        }
+
+        @Override
+        public boolean loop(TelemetryPacket p) {
+            double t = clock() - beginTs;
+            if (t >= timeTrajectory.duration) {
+                leftFront.setPower(0);
+                leftBack.setPower(0);
+                rightBack.setPower(0);
+                rightFront.setPower(0);
+
+                active = false;
+
+                return false;
+            }
+
+            Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
+
+            Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
+
+            Twist2dDual<Time> command = new HolonomicController(
+                    AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
+                    AXIAL_VEL_GAIN, LATERAL_VEL_GAIN, HEADING_VEL_GAIN
+            )
+                    .compute(txWorldTarget, pose, robotVelRobot);
+
+            MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
+            double voltage = voltageSensor.getVoltage();
+            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);
+
+            p.put("x", pose.trans.x);
+            p.put("y", pose.trans.y);
+            p.put("heading (deg)", Math.toDegrees(pose.rot.log()));
+
+            Pose2d error = txWorldTarget.value().minusExp(pose);
+            p.put("xError", error.trans.x);
+            p.put("yError", error.trans.y);
+            p.put("headingError (deg)", Math.toDegrees(error.rot.log()));
+
+            // only draw when active; only one drive action should be active at a time
+            Canvas c = p.fieldOverlay();
+            drawPoseHistory(c);
+
+            c.setStroke("#4CAF50");
+            drawRobot(c, txWorldTarget.value());
+
+            c.setStroke("#3F51B5");
+            drawRobot(c, pose);
+
+            return true;
+        }
+
+        public void cancel() {
+            double t = clock() - beginTs;
+            double s = timeTrajectory.profile.get(t).value();
+            beginTs += t;
+            timeTrajectory = new TimeTrajectory(trajectory.cancel(s));
+        }
+
+        @Override
+        public void draw(Canvas c) {
+            c.setStrokeWidth(1);
+            c.setStroke(active ? "#4CAF50FF" : "#4CAF507A");
+            c.strokePolyline(xPoints, yPoints);
+        }
+    }
+
+    public final class TurnAction implements Action {
+        private final Pose2d beginPose;
+
+        private final TimeProfile profile;
+        private double beginTs;
+
+        private boolean active;
+
+        public TurnAction(Pose2d beginPose, double angle) {
+            this.beginPose = beginPose;
+            profile = new TimeProfile(Profiles.constantProfile(angle, 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
+        }
+
+        @Override
+        public void init() {
+            beginTs = clock();
+
+            active = true;
+        }
+
+        @Override
+        public boolean loop(TelemetryPacket p) {
+            double t = clock() - beginTs;
+            if (t >= profile.duration) {
+                leftFront.setPower(0);
+                leftBack.setPower(0);
+                rightBack.setPower(0);
+                rightFront.setPower(0);
+
+                active = false;
+
+                return false;
+            }
+
+            DualNum<Time> x = profile.get(t);
+            Pose2dDual<Time> txWorldTarget = Rotation2dDual.exp(x).times(beginPose);
+
+            Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
+
+            Twist2dDual<Time> command = new HolonomicController(
+                    AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
+                    AXIAL_VEL_GAIN, LATERAL_VEL_GAIN, HEADING_VEL_GAIN
+            )
+                    .compute(txWorldTarget, pose, robotVelRobot);
+
+            MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
+            double voltage = voltageSensor.getVoltage();
+            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);
+
+            Canvas c = p.fieldOverlay();
+            drawPoseHistory(c);
+
+            c.setStroke("#4CAF50");
+            drawRobot(c, txWorldTarget.value());
+
+            c.setStroke("#3F51B5");
+            drawRobot(c, pose);
+
+            return true;
+        }
+
+        @Override
+        public void draw(Canvas c) {
+            c.setFill(active ? "#7C4DFFFF" : "#7C4DFF7A");
+            c.fillCircle(beginPose.trans.x, beginPose.trans.y, 2);
+        }
+    }
+
+    private Twist2d updatePoseEstimateAndGetActualVel() {
+        Twist2dIncrDual<Time> incr = localizer.updateAndGetIncr();
+        pose = pose.plus(incr.value());
+
+        poseHistory.add(pose);
+        while (poseHistory.size() > 100) {
+            poseHistory.removeFirst();
+        }
+
+        return incr.velocity().value();
+    }
+
+    private void drawPoseHistory(Canvas c) {
+        double[] xPoints = new double[poseHistory.size()];
+        double[] yPoints = new double[poseHistory.size()];
+
+        int i = 0;
+        for (Pose2d t : poseHistory) {
+            xPoints[i] = t.trans.x;
+            yPoints[i] = t.trans.y;
+
+            i++;
+        }
+
+        c.setStrokeWidth(1);
+        c.setStroke("#3F51B5");
+        c.strokePolyline(xPoints, yPoints);
+    }
+
+    private static void drawRobot(Canvas c, Pose2d t) {
+        final double ROBOT_RADIUS = 9;
+
+        c.setStrokeWidth(1);
+        c.strokeCircle(t.trans.x, t.trans.y, ROBOT_RADIUS);
+
+        Vector2d halfv = t.rot.vec().times(0.5 * ROBOT_RADIUS);
+        Vector2d p1 = t.trans.plus(halfv);
+        Vector2d p2 = p1.plus(halfv);
+        c.strokeLine(p1.x, p1.y, p2.x, p2.y);
+    }
+
+    public TurnAction turn(Pose2d beginPose, double angle) {
+        return new TurnAction(beginPose, angle);
+    }
+    public TurnAction turnTo(Pose2d beginPose, Rotation2d rot) {
+        return new TurnAction(beginPose, rot.minus(beginPose.rot));
+    }
+    public TurnAction turnTo(Pose2d beginPose, double rot) {
+        return turnTo(beginPose, Rotation2d.exp(rot));
+    }
+
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
+        return new SafeTrajectoryBuilder(
+                beginPose, beginTangent, 1e-6,
+                0.0, defaultVelConstraint, defaultAccelConstraint, 0.25, poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent), poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0), poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(beginPose, false, poseMap);
+    }
+
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent) {
+        return new SafeTrajectoryBuilder(
+                beginPose, beginTangent, 1e-6,
+                0.0, defaultVelConstraint, defaultAccelConstraint, 0.25);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent) {
+        return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent));
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed) {
+        return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0));
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose) {
+        return trajectoryBuilder(beginPose, false);
+    }
+
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(traj.path.end(1).value(), reversed, poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(traj.path.end(1).value(), poseMap);
+    }
+
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent) {
+        return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent) {
+        return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed) {
+        return trajectoryBuilder(traj.path.end(1).value(), reversed);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj) {
+        return trajectoryBuilder(traj.path.end(1).value());
+    }
+
+    public FollowTrajectoryAction followTrajectory(Trajectory trajectory) {
+        return new FollowTrajectoryAction(trajectory);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java

@@ -0,0 +1,502 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.AccelConstraint;
+import com.acmerobotics.roadrunner.AngularVelConstraint;
+import com.acmerobotics.roadrunner.Arclength;
+import com.acmerobotics.roadrunner.DualNum;
+import com.acmerobotics.roadrunner.MinVelConstraint;
+import com.acmerobotics.roadrunner.MotorFeedforward;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.Pose2dDual;
+import com.acmerobotics.roadrunner.ProfileAccelConstraint;
+import com.acmerobotics.roadrunner.Profiles;
+import com.acmerobotics.roadrunner.RamseteController;
+import com.acmerobotics.roadrunner.Rotation2d;
+import com.acmerobotics.roadrunner.Rotation2dDual;
+import com.acmerobotics.roadrunner.SafeTrajectoryBuilder;
+import com.acmerobotics.roadrunner.TankKinematics;
+import com.acmerobotics.roadrunner.Time;
+import com.acmerobotics.roadrunner.TimeProfile;
+import com.acmerobotics.roadrunner.TimeTrajectory;
+import com.acmerobotics.roadrunner.Trajectory;
+import com.acmerobotics.roadrunner.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.Twist2d;
+import com.acmerobotics.roadrunner.Twist2dDual;
+import com.acmerobotics.roadrunner.Twist2dIncrDual;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.Vector2dDual;
+import com.acmerobotics.roadrunner.VelConstraint;
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
+import org.firstinspires.ftc.teamcode.util.Encoder;
+import org.firstinspires.ftc.teamcode.util.Localizer;
+import org.firstinspires.ftc.teamcode.util.LynxFirmwareVersion;
+import org.firstinspires.ftc.teamcode.util.OverflowEncoder;
+import org.firstinspires.ftc.teamcode.util.RawEncoder;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collections;
+import java.util.LinkedList;
+import java.util.List;
+
+@Config
+public final class TankDrive {
+    // drive model parameters
+    public static double IN_PER_TICK = 0;
+    public static double TRACK_WIDTH_TICKS = 0;
+
+    // feedforward parameters
+    public static double kS = 0;
+    public static double kV = 0;
+    public static double kA = 0;
+
+    // path profile parameters
+    public static double MAX_WHEEL_VEL = 50;
+    public static double MIN_PROFILE_ACCEL = -30;
+    public static double MAX_PROFILE_ACCEL = 50;
+
+    // turn profile parameters
+    public static double MAX_ANG_VEL = Math.PI; // shared with path
+    public static double MAX_ANG_ACCEL = Math.PI;
+
+    // path controller gains
+    public static double RAMSETE_ZETA = 0.7; // in the range (0, 1)
+    public static double RAMSETE_BBAR = 2.0; // positive
+
+    // turn controller gains
+    public static double TURN_GAIN = 0.0;
+    public static double TURN_VEL_GAIN = 0.0;
+
+    public final TankKinematics kinematics = new TankKinematics(IN_PER_TICK * TRACK_WIDTH_TICKS);
+
+    public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV, kA);
+
+    public final VelConstraint defaultVelConstraint =
+            new MinVelConstraint(Arrays.asList(
+                    kinematics.new WheelVelConstraint(MAX_WHEEL_VEL),
+                    new AngularVelConstraint(MAX_ANG_VEL)
+            ));
+    public final AccelConstraint defaultAccelConstraint =
+            new ProfileAccelConstraint(MIN_PROFILE_ACCEL, MAX_PROFILE_ACCEL);
+
+    public final List<DcMotorEx> leftMotors, rightMotors;
+
+    public final BNO055Wrapper imu;
+
+    public final VoltageSensor voltageSensor;
+
+    public final Localizer localizer;
+    public Pose2d pose;
+
+    public final double inPerTick = IN_PER_TICK;
+
+    private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
+
+    public class DriveLocalizer implements Localizer {
+        public final List<Encoder> leftEncs, rightEncs;
+
+        private double lastLeftPos, lastRightPos;
+
+        public DriveLocalizer() {
+            {
+                List<Encoder> leftEncs = new ArrayList<>();
+                for (DcMotorEx m : leftMotors) {
+                    Encoder e = new OverflowEncoder(new RawEncoder(m));
+                    leftEncs.add(e);
+                    lastLeftPos += e.getPositionAndVelocity().position;
+                }
+                lastLeftPos /= leftEncs.size();
+                this.leftEncs = Collections.unmodifiableList(leftEncs);
+            }
+
+            {
+                List<Encoder> rightEncs = new ArrayList<>();
+                for (DcMotorEx m : rightMotors) {
+                    Encoder e = new OverflowEncoder(new RawEncoder(m));
+                    rightEncs.add(e);
+                    lastRightPos += e.getPositionAndVelocity().position;
+                }
+                lastRightPos /= rightEncs.size();
+                this.rightEncs = Collections.unmodifiableList(rightEncs);
+            }
+        }
+
+        @Override
+        public Twist2dIncrDual<Time> updateAndGetIncr() {
+            double meanLeftPos = 0.0, meanLeftVel = 0.0;
+            for (Encoder e : leftEncs) {
+                Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
+                meanLeftPos += p.position;
+                meanLeftVel += p.velocity;
+            }
+            meanLeftPos /= leftEncs.size();
+            meanLeftVel /= leftEncs.size();
+
+            double meanRightPos = 0.0, meanRightVel = 0.0;
+            for (Encoder e : rightEncs) {
+                Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
+                meanRightPos += p.position;
+                meanRightVel += p.velocity;
+            }
+            meanRightPos /= rightEncs.size();
+            meanRightVel /= rightEncs.size();
+
+            TankKinematics.WheelIncrements<Time> incrs = new TankKinematics.WheelIncrements<>(
+                    new DualNum<Time>(new double[] {
+                            meanLeftPos - lastLeftPos,
+                            meanLeftVel
+                    }).times(inPerTick),
+                    new DualNum<Time>(new double[] {
+                            meanRightPos - lastRightPos,
+                            meanRightVel,
+                    }).times(inPerTick)
+            );
+
+            lastLeftPos = meanLeftPos;
+            lastRightPos = meanRightPos;
+
+            return kinematics.forward(incrs);
+        }
+    }
+
+    public TankDrive(HardwareMap hardwareMap, Pose2d pose) {
+        this.pose = pose;
+        
+        LynxFirmwareVersion.throwIfAnyModulesBelowVersion(hardwareMap,
+                new LynxFirmwareVersion(1, 8, 2));
+
+        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+            module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        }
+
+        leftMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "left"));
+        rightMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "right"));
+
+        for (DcMotorEx m : leftMotors) {
+            m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        }
+        for (DcMotorEx m : rightMotors) {
+            m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        }
+
+        BNO055IMU baseImu = hardwareMap.get(BNO055IMU.class, "imu");
+        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+        parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
+        baseImu.initialize(parameters);
+        imu = new BNO055Wrapper(baseImu);
+
+        voltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+        localizer = new TankDrive.DriveLocalizer();
+    }
+
+    public void setDrivePowers(Twist2d powers) {
+        TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(Twist2dDual.constant(powers, 1));
+        for (DcMotorEx m : leftMotors) {
+            m.setPower(wheelVels.left.get(0));
+        }
+        for (DcMotorEx m : rightMotors) {
+            m.setPower(wheelVels.right.get(0));
+        }
+    }
+
+    public final class FollowTrajectoryAction implements Action {
+        public final Trajectory trajectory;
+        private TimeTrajectory timeTrajectory;
+        private double beginTs;
+
+        private boolean active;
+        private final double[] xPoints, yPoints;
+
+        public FollowTrajectoryAction(Trajectory t) {
+            this.trajectory = t;
+            timeTrajectory = new TimeTrajectory(t);
+
+            List<Double> disps = com.acmerobotics.roadrunner.Math.range(
+                    0, t.path.length(),
+                    (int) Math.ceil(t.path.length() / 2));
+            xPoints = new double[disps.size()];
+            yPoints = new double[disps.size()];
+            for (int i = 0; i < disps.size(); i++) {
+                Pose2d p = t.path.get(disps.get(i), 1).value();
+                xPoints[i] = p.trans.x;
+                yPoints[i] = p.trans.y;
+            }
+        }
+
+        @Override
+        public void init() {
+            beginTs = clock();
+
+            active = true;
+        }
+
+        @Override
+        public boolean loop(TelemetryPacket p) {
+            double t = clock() - beginTs;
+            if (t >= timeTrajectory.duration) {
+                for (DcMotorEx m : leftMotors) {
+                    m.setPower(0);
+                }
+                for (DcMotorEx m : rightMotors) {
+                    m.setPower(0);
+                }
+
+                active = false;
+
+                return false;
+            }
+
+            DualNum<Time> x = timeTrajectory.profile.get(t);
+
+            Pose2dDual<Arclength> txWorldTarget = timeTrajectory.path.get(x.value(), 3);
+
+            updatePoseEstimateAndGetActualVel();
+
+            Twist2dDual<Time> command = new RamseteController(kinematics.trackWidth, RAMSETE_ZETA, RAMSETE_BBAR)
+                    .compute(x, txWorldTarget, pose);
+
+            TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
+            double voltage = voltageSensor.getVoltage();
+            for (DcMotorEx m : leftMotors) {
+                m.setPower(feedforward.compute(wheelVels.left) / voltage);
+            }
+            for (DcMotorEx m : rightMotors) {
+                m.setPower(feedforward.compute(wheelVels.right) / voltage);
+            }
+
+            p.put("x", pose.trans.x);
+            p.put("y", pose.trans.y);
+            p.put("heading (deg)", Math.toDegrees(pose.rot.log()));
+
+            Pose2d error = txWorldTarget.value().minusExp(pose);
+            p.put("xError", error.trans.x);
+            p.put("yError", error.trans.y);
+            p.put("headingError (deg)", Math.toDegrees(error.rot.log()));
+
+            // only draw when active; only one drive action should be active at a time
+            Canvas c = p.fieldOverlay();
+            drawPoseHistory(c);
+
+            c.setStroke("#4CAF50");
+            drawRobot(c, txWorldTarget.value());
+
+            c.setStroke("#3F51B5");
+            drawRobot(c, pose);
+
+            return true;
+        }
+
+        public void cancel() {
+            double t = clock() - beginTs;
+            double s = timeTrajectory.profile.get(t).value();
+            beginTs += t;
+            timeTrajectory = new TimeTrajectory(trajectory.cancel(s));
+        }
+
+        @Override
+        public void draw(Canvas c) {
+            c.setStrokeWidth(1);
+            c.setStroke(active ? "#4CAF50FF" : "#4CAF507A");
+            c.strokePolyline(xPoints, yPoints);
+        }
+    }
+
+    public final class TurnAction implements Action {
+        private final Pose2d beginPose;
+
+        private final TimeProfile profile;
+        private double beginTs;
+
+        private boolean active;
+
+        public TurnAction(Pose2d beginPose, double angle) {
+            this.beginPose = beginPose;
+            profile = new TimeProfile(Profiles.constantProfile(angle, 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
+        }
+
+        @Override
+        public void init() {
+            beginTs = clock();
+
+            active = true;
+        }
+
+        @Override
+        public boolean loop(TelemetryPacket p) {
+            double t = clock() - beginTs;
+            if (t >= profile.duration) {
+                for (DcMotorEx m : leftMotors) {
+                    m.setPower(0);
+                }
+                for (DcMotorEx m : rightMotors) {
+                    m.setPower(0);
+                }
+
+                active = false;
+
+                return false;
+            }
+
+            DualNum<Time> x = profile.get(t);
+
+            Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
+
+            Rotation2dDual<Time> target = Rotation2dDual.<Time>constant(beginPose.rot, 3).plus(x);
+            Twist2dDual<Time> command = new Twist2dDual<>(
+                    Vector2dDual.constant(new Vector2d(0, 0), 3),
+                    target.velocity().plus(
+                            TURN_GAIN * pose.rot.minus(target.value()) +
+                            TURN_VEL_GAIN * (robotVelRobot.rotVel - target.velocity().value())
+                    )
+            );
+
+            TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
+            double voltage = voltageSensor.getVoltage();
+            for (DcMotorEx m : leftMotors) {
+                m.setPower(feedforward.compute(wheelVels.left) / voltage);
+            }
+            for (DcMotorEx m : rightMotors) {
+                m.setPower(feedforward.compute(wheelVels.right) / voltage);
+            }
+
+            Canvas c = p.fieldOverlay();
+            drawPoseHistory(c);
+
+            c.setStroke("#4CAF50");
+            drawRobot(c, new Pose2d(beginPose.trans, beginPose.rot.plus(x.value())));
+
+            c.setStroke("#3F51B5");
+            drawRobot(c, pose);
+
+            return true;
+        }
+
+        @Override
+        public void draw(Canvas c) {
+            c.setFill(active ? "#7C4DFFFF" : "#7C4DFF7A");
+            c.fillCircle(beginPose.trans.x, beginPose.trans.y, 2);
+        }
+    }
+
+    public Twist2d updatePoseEstimateAndGetActualVel() {
+        Twist2dIncrDual<Time> incr = localizer.updateAndGetIncr();
+        pose = pose.plus(incr.value());
+
+        poseHistory.add(pose);
+        while (poseHistory.size() > 100) {
+            poseHistory.removeFirst();
+        }
+
+        return incr.velocity().value();
+    }
+
+    private void drawPoseHistory(Canvas c) {
+        double[] xPoints = new double[poseHistory.size()];
+        double[] yPoints = new double[poseHistory.size()];
+
+        int i = 0;
+        for (Pose2d t : poseHistory) {
+            xPoints[i] = t.trans.x;
+            yPoints[i] = t.trans.y;
+
+            i++;
+        }
+
+        c.setStrokeWidth(1);
+        c.setStroke("#3F51B5");
+        c.strokePolyline(xPoints, yPoints);
+    }
+
+    private static void drawRobot(Canvas c, Pose2d t) {
+        final double ROBOT_RADIUS = 9;
+
+        c.setStrokeWidth(1);
+        c.strokeCircle(t.trans.x, t.trans.y, ROBOT_RADIUS);
+
+        Vector2d halfv = t.rot.vec().times(0.5 * ROBOT_RADIUS);
+        Vector2d p1 = t.trans.plus(halfv);
+        Vector2d p2 = p1.plus(halfv);
+        c.strokeLine(p1.x, p1.y, p2.x, p2.y);
+    }
+
+    public TurnAction turn(Pose2d beginPose, double angle) {
+        return new TurnAction(beginPose, angle);
+    }
+    public TurnAction turnTo(Pose2d beginPose, Rotation2d rot) {
+        return new TurnAction(beginPose, rot.minus(beginPose.rot));
+    }
+    public TurnAction turnTo(Pose2d beginPose, double rot) {
+        return turnTo(beginPose, Rotation2d.exp(rot));
+    }
+
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
+        return new SafeTrajectoryBuilder(
+                beginPose, beginTangent, 1e-6,
+                0.0, defaultVelConstraint, defaultAccelConstraint, 0.25, poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent), poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0), poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(beginPose, false, poseMap);
+    }
+
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent) {
+        return new SafeTrajectoryBuilder(
+                beginPose, beginTangent, 1e-6,
+                0.0, defaultVelConstraint, defaultAccelConstraint, 0.25);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent) {
+        return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent));
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed) {
+        return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0));
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose) {
+        return trajectoryBuilder(beginPose, false);
+    }
+
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(traj.path.end(1).value(), reversed, poseMap);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, TrajectoryBuilder.PoseMap poseMap) {
+        return trajectoryBuilder(traj.path.end(1).value(), poseMap);
+    }
+
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent) {
+        return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent) {
+        return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed) {
+        return trajectoryBuilder(traj.path.end(1).value(), reversed);
+    }
+    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj) {
+        return trajectoryBuilder(traj.path.end(1).value());
+    }
+
+    public TankDrive.FollowTrajectoryAction followTrajectory(Trajectory trajectory) {
+        return new TankDrive.FollowTrajectoryAction(trajectory);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java

@@ -0,0 +1,71 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.DualNum;
+import com.acmerobotics.roadrunner.Time;
+import com.acmerobotics.roadrunner.Twist2dIncrDual;
+import com.acmerobotics.roadrunner.Vector2dDual;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.util.Encoder;
+import org.firstinspires.ftc.teamcode.util.Localizer;
+import org.firstinspires.ftc.teamcode.util.RawEncoder;
+
+@Config
+public final class ThreeDeadWheelLocalizer implements Localizer {
+    public static double PAR0_Y_TICKS = 0.0;
+    public static double PAR1_Y_TICKS = 0.0;
+    public static double PERP_X_TICKS = 0.0;
+
+    public final Encoder par0, par1, perp;
+
+    public final double inPerTick;
+
+    private int lastPar0Pos, lastPar1Pos, lastPerpPos;
+
+    public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) {
+        par0 = new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0"));
+        par1 = new RawEncoder(hardwareMap.get(DcMotorEx.class, "par1"));
+        perp = new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp"));
+
+        lastPar0Pos = par0.getPositionAndVelocity().position;
+        lastPar1Pos = par1.getPositionAndVelocity().position;
+        lastPerpPos = perp.getPositionAndVelocity().position;
+
+        this.inPerTick = inPerTick;
+    }
+
+    public Twist2dIncrDual<Time> updateAndGetIncr() {
+        Encoder.PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
+        Encoder.PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
+        Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
+
+        int par0PosDelta = par0PosVel.position - lastPar0Pos;
+        int par1PosDelta = par1PosVel.position - lastPar1Pos;
+        int perpPosDelta = perpPosVel.position - lastPerpPos;
+
+        Twist2dIncrDual<Time> twistIncr = new Twist2dIncrDual<>(
+                new Vector2dDual<>(
+                        new DualNum<Time>(new double[] {
+                                (PAR0_Y_TICKS * par1PosDelta - PAR1_Y_TICKS * par0PosDelta) / (PAR0_Y_TICKS - PAR1_Y_TICKS),
+                                (PAR0_Y_TICKS * par1PosVel.velocity - PAR1_Y_TICKS * par0PosVel.velocity) / (PAR0_Y_TICKS - PAR1_Y_TICKS),
+                        }).times(inPerTick),
+                        new DualNum<Time>(new double[] {
+                                (PERP_X_TICKS / (PAR0_Y_TICKS - PAR1_Y_TICKS) * (par1PosDelta - par0PosDelta) + perpPosDelta),
+                                (PERP_X_TICKS / (PAR0_Y_TICKS - PAR1_Y_TICKS) * (par1PosVel.velocity - par0PosVel.velocity) + perpPosVel.velocity),
+                        }).times(inPerTick)
+                ),
+                new DualNum<>(new double[] {
+                        (par0PosDelta - par1PosDelta) / (PAR0_Y_TICKS - PAR1_Y_TICKS),
+                        (par0PosVel.velocity - par1PosVel.velocity) / (PAR0_Y_TICKS - PAR1_Y_TICKS),
+                })
+        );
+
+        lastPar0Pos = par0PosVel.position;
+        lastPar1Pos = par1PosVel.position;
+        lastPerpPos = perpPosVel.position;
+
+        return twistIncr;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java

@@ -0,0 +1,76 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.DualNum;
+import com.acmerobotics.roadrunner.Rotation2d;
+import com.acmerobotics.roadrunner.Time;
+import com.acmerobotics.roadrunner.Twist2dIncrDual;
+import com.acmerobotics.roadrunner.Vector2dDual;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
+import org.firstinspires.ftc.teamcode.util.Encoder;
+import org.firstinspires.ftc.teamcode.util.Localizer;
+import org.firstinspires.ftc.teamcode.util.RawEncoder;
+
+@Config
+public final class TwoDeadWheelLocalizer implements Localizer {
+    public static double PAR_Y_TICKS = 0.0;
+    public static double PERP_X_TICKS = 0.0;
+
+    public final Encoder par, perp;
+    public final BNO055Wrapper imu;
+
+    private int lastParPos, lastPerpPos;
+    private Rotation2d lastHeading;
+
+    private final double inPerTick;
+
+    public TwoDeadWheelLocalizer(HardwareMap hardwareMap, BNO055Wrapper imu, double inPerTick) {
+        par = new RawEncoder(hardwareMap.get(DcMotorEx.class, "par"));
+        perp = new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp"));
+        this.imu = imu;
+
+        lastParPos = par.getPositionAndVelocity().position;
+        lastPerpPos = perp.getPositionAndVelocity().position;
+        lastHeading = imu.getHeading();
+
+        this.inPerTick = inPerTick;
+    }
+
+    public Twist2dIncrDual<Time> updateAndGetIncr() {
+        Encoder.PositionVelocityPair parPosVel = par.getPositionAndVelocity();
+        Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
+        Rotation2d heading = imu.getHeading();
+
+        int parPosDelta = parPosVel.position - lastParPos;
+        int perpPosDelta = perpPosVel.position - lastPerpPos;
+        double headingDelta = heading.minus(lastHeading);
+
+        double headingVel = imu.getHeadingVelocity();
+
+        Twist2dIncrDual<Time> twistIncr = new Twist2dIncrDual<>(
+                new Vector2dDual<>(
+                        new DualNum<Time>(new double[] {
+                                parPosDelta - PAR_Y_TICKS * headingDelta,
+                                parPosVel.velocity - PAR_Y_TICKS * headingVel,
+                        }).times(inPerTick),
+                        new DualNum<Time>(new double[] {
+                                perpPosDelta - PERP_X_TICKS * headingDelta,
+                                perpPosVel.velocity - PERP_X_TICKS * headingVel,
+                        }).times(inPerTick)
+                ),
+                new DualNum<>(new double[] {
+                        headingDelta,
+                        headingVel,
+                })
+        );
+
+        lastParPos = parPosVel.position;
+        lastPerpPos = perpPosVel.position;
+        lastHeading = heading;
+
+        return twistIncr;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AngularRampLogger.java

@@ -0,0 +1,135 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.teamcode.util.Encoder;
+import org.firstinspires.ftc.teamcode.util.MidpointTimer;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public final class AngularRampLogger extends LinearOpMode {
+    private static double power(double seconds) {
+        return Math.min(0.1 * seconds, 0.9);
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        DriveView view = new DriveView(hardwareMap);
+
+        class Data {
+            final String type = view.type;
+
+            final List<List<Double>> leftPowerTimes = new ArrayList<>();
+            final List<List<Double>> leftPowers = new ArrayList<>();
+
+            final List<List<Double>> rightPowerTimes = new ArrayList<>();
+            final List<List<Double>> rightPowers = new ArrayList<>();
+
+            final List<Double> voltageTimes = new ArrayList<>();
+            final List<Double> voltages = new ArrayList<>();
+
+            final List<Double> encTimes = new ArrayList<>();
+            final List<List<Integer>> leftEncPositions = new ArrayList<>();
+            final List<List<Integer>> leftEncVels = new ArrayList<>();
+            final List<List<Integer>> rightEncPositions = new ArrayList<>();
+            final List<List<Integer>> rightEncVels = new ArrayList<>();
+            final List<List<Integer>> parEncPositions = new ArrayList<>();
+            final List<List<Integer>> parEncVels = new ArrayList<>();
+            final List<List<Integer>> perpEncPositions = new ArrayList<>();
+            final List<List<Integer>> perpEncVels = new ArrayList<>();
+
+            final List<Double> angVelTimes = new ArrayList<>();
+            final List<List<Double>> angVels = new ArrayList<>();
+        }
+
+        Data data = new Data();
+        for (DcMotorEx m : view.leftMotors) {
+            data.leftPowerTimes.add(new ArrayList<>());
+            data.leftPowers.add(new ArrayList<>());
+        }
+        for (DcMotorEx m : view.rightMotors) {
+            data.rightPowerTimes.add(new ArrayList<>());
+            data.rightPowers.add(new ArrayList<>());
+        }
+        for (Encoder e : view.leftEncs) {
+            data.leftEncPositions.add(new ArrayList<>());
+            data.leftEncVels.add(new ArrayList<>());
+        }
+        for (Encoder e : view.rightEncs) {
+            data.rightEncPositions.add(new ArrayList<>());
+            data.rightEncVels.add(new ArrayList<>());
+        }
+        for (Encoder e : view.parEncs) {
+            data.parEncPositions.add(new ArrayList<>());
+            data.parEncVels.add(new ArrayList<>());
+        }
+        for (Encoder e : view.perpEncs) {
+            data.perpEncPositions.add(new ArrayList<>());
+            data.perpEncVels.add(new ArrayList<>());
+        }
+        for (int i = 0; i < 3; i++) {
+            data.angVels.add(new ArrayList<>());
+        }
+
+        waitForStart();
+
+        MidpointTimer t = new MidpointTimer();
+        while (opModeIsActive()) {
+            for (int i = 0; i < view.leftMotors.size(); i++) {
+                double power = -power(t.seconds());
+                view.leftMotors.get(i).setPower(power);
+
+                data.leftPowers.get(i).add(power);
+                data.leftPowerTimes.get(i).add(t.addSplit());
+            }
+
+            for (int i = 0; i < view.rightMotors.size(); i++) {
+                double power = power(t.seconds());
+                view.rightMotors.get(i).setPower(power);
+
+                data.rightPowers.get(i).add(power);
+                data.rightPowerTimes.get(i).add(t.addSplit());
+            }
+
+            data.voltages.add(view.voltageSensor.getVoltage());
+            data.voltageTimes.add(t.addSplit());
+
+            for (int i = 0; i < view.leftEncs.size(); i++) {
+                Encoder.PositionVelocityPair p = view.leftEncs.get(i).getPositionAndVelocity();
+                data.leftEncPositions.get(i).add(p.position);
+                data.leftEncVels.get(i).add(p.velocity);
+            }
+            for (int i = 0; i < view.rightEncs.size(); i++) {
+                Encoder.PositionVelocityPair p = view.rightEncs.get(i).getPositionAndVelocity();
+                data.rightEncPositions.get(i).add(p.position);
+                data.rightEncVels.get(i).add(p.velocity);
+            }
+            for (int i = 0; i < view.parEncs.size(); i++) {
+                Encoder.PositionVelocityPair p = view.parEncs.get(i).getPositionAndVelocity();
+                data.parEncPositions.get(i).add(p.position);
+                data.parEncVels.get(i).add(p.velocity);
+            }
+            for (int i = 0; i < view.perpEncs.size(); i++) {
+                Encoder.PositionVelocityPair p = view.perpEncs.get(i).getPositionAndVelocity();
+                data.perpEncPositions.get(i).add(p.position);
+                data.perpEncVels.get(i).add(p.velocity);
+            }
+            data.encTimes.add(t.addSplit());
+
+            AngularVelocity av = view.imu.getAngularVelocity();
+            data.angVels.get(0).add((double) av.xRotationRate);
+            data.angVels.get(1).add((double) av.yRotationRate);
+            data.angVels.get(2).add((double) av.zRotationRate);
+            data.angVelTimes.add(t.addSplit());
+        }
+
+        for (DcMotorEx m : view.motors) {
+            m.setPower(0);
+        }
+
+        TuningFiles.save(TuningFiles.FileType.ANGULAR_RAMP, data);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java

@@ -0,0 +1,170 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.acmerobotics.roadrunner.MotorFeedforward;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.Twist2d;
+import com.qualcomm.robotcore.hardware.DcMotorController;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.teamcode.MecanumDrive;
+import org.firstinspires.ftc.teamcode.TankDrive;
+import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer;
+import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer;
+import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
+import org.firstinspires.ftc.teamcode.util.Encoder;
+import org.firstinspires.ftc.teamcode.util.Localizer;
+import org.firstinspires.ftc.teamcode.util.OverflowEncoder;
+import org.firstinspires.ftc.teamcode.util.RawEncoder;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collections;
+import java.util.List;
+
+final class DriveView {
+    public final String type;
+
+    public final double inPerTick;
+    public final double maxVel, minAccel, maxAccel;
+
+    public final List<DcMotorEx> leftMotors, rightMotors;
+    public final List<DcMotorEx> motors;
+
+    // invariant: (leftEncs.isEmpty() && rightEncs.isEmpty()) ||
+    //                  (parEncs.isEmpty() && perpEncs.isEmpty())
+    public final List<RawEncoder> leftEncs, rightEncs, parEncs, perpEncs;
+    public final List<RawEncoder> forwardEncs;
+
+    public final BNO055Wrapper imu;
+
+    public final VoltageSensor voltageSensor;
+
+    private final MecanumDrive md;
+    private final TankDrive td;
+
+    private static RawEncoder unwrap(Encoder e) {
+        if (e instanceof OverflowEncoder) {
+            return ((OverflowEncoder) e).encoder;
+        } else {
+            return (RawEncoder) e;
+        }
+    }
+
+    public DriveView(HardwareMap hardwareMap) {
+        final Localizer localizer;
+        if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
+            type = "mecanum";
+
+            inPerTick = MecanumDrive.IN_PER_TICK;
+            maxVel = MecanumDrive.MAX_WHEEL_VEL;
+            minAccel = MecanumDrive.MIN_PROFILE_ACCEL;
+            maxAccel = MecanumDrive.MAX_PROFILE_ACCEL;
+
+            md = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); td = null;
+            leftMotors = Arrays.asList(md.leftFront, md.leftBack);
+            rightMotors = Arrays.asList(md.rightFront, md.rightBack);
+            imu = md.imu;
+            voltageSensor = md.voltageSensor;
+
+            localizer = md.localizer;
+        } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
+            type = "tank";
+
+            inPerTick = TankDrive.IN_PER_TICK;
+            maxVel = TankDrive.MAX_WHEEL_VEL;
+            minAccel = TankDrive.MIN_PROFILE_ACCEL;
+            maxAccel = TankDrive.MAX_PROFILE_ACCEL;
+
+            td = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); md = null;
+            leftMotors = td.leftMotors;
+            rightMotors = td.rightMotors;
+            imu = td.imu;
+            voltageSensor = td.voltageSensor;
+
+            localizer = td.localizer;
+        } else {
+            throw new AssertionError();
+        }
+
+        if (localizer instanceof TwoDeadWheelLocalizer) {
+            TwoDeadWheelLocalizer l2 = (TwoDeadWheelLocalizer) localizer;
+            parEncs = Collections.singletonList(unwrap(l2.par));
+            perpEncs = Collections.singletonList(unwrap(l2.perp));
+            leftEncs = Collections.emptyList();
+            rightEncs = Collections.emptyList();
+        } else if (localizer instanceof ThreeDeadWheelLocalizer) {
+            ThreeDeadWheelLocalizer l3 = (ThreeDeadWheelLocalizer) localizer;
+            parEncs = Arrays.asList(unwrap(l3.par0), unwrap(l3.par1));
+            perpEncs = Collections.singletonList(unwrap(l3.perp));
+            leftEncs = Collections.emptyList();
+            rightEncs = Collections.emptyList();
+        } else if (localizer instanceof MecanumDrive.DriveLocalizer) {
+            MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) localizer;
+            parEncs = Collections.emptyList();
+            perpEncs = Collections.emptyList();
+            leftEncs = Arrays.asList(unwrap(dl.leftFront), unwrap(dl.leftRear));
+            rightEncs = Arrays.asList(unwrap(dl.rightFront), unwrap(dl.rightRear));
+        } else if (localizer instanceof TankDrive.DriveLocalizer) {
+            TankDrive.DriveLocalizer dl = (TankDrive.DriveLocalizer) localizer;
+            parEncs = Collections.emptyList();
+            perpEncs = Collections.emptyList();
+            leftEncs = new ArrayList<>();
+            for (Encoder e : dl.leftEncs) {
+                leftEncs.add(unwrap(e));
+            }
+            rightEncs = new ArrayList<>();
+            for (Encoder e : dl.rightEncs) {
+                rightEncs.add(unwrap(e));
+            }
+        } else {
+            throw new AssertionError();
+        }
+
+        motors = new ArrayList<>();
+        motors.addAll(leftMotors);
+        motors.addAll(rightMotors);
+
+        forwardEncs = new ArrayList<>();
+        forwardEncs.addAll(leftEncs);
+        forwardEncs.addAll(rightEncs);
+        forwardEncs.addAll(parEncs);
+
+        List<RawEncoder> allEncs = new ArrayList<>();
+        allEncs.addAll(forwardEncs);
+        allEncs.addAll(perpEncs);
+
+        DcMotorController c1 = allEncs.get(0).getController();
+        for (Encoder e : allEncs) {
+            DcMotorController c2 = e.getController();
+            if (c1 != c2) {
+                throw new IllegalArgumentException("all encoders must be attached to the same hub");
+            }
+        }
+    }
+
+    public MotorFeedforward feedforward() {
+        if (md != null) {
+            return new MotorFeedforward(MecanumDrive.kS, MecanumDrive.kV, MecanumDrive.kA);
+        }
+
+        if (td != null) {
+            return new MotorFeedforward(TankDrive.kS, TankDrive.kV, TankDrive.kA);
+        }
+
+        throw new AssertionError();
+    }
+
+    public void setDrivePowers(Twist2d powers) {
+        if (md != null) {
+            md.setDrivePowers(powers);
+        }
+
+        if (td != null) {
+            td.setDrivePowers(powers);
+        }
+
+        throw new AssertionError();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardPushTest.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+
+import org.firstinspires.ftc.teamcode.util.Encoder;
+
+import java.util.List;
+
+public final class ForwardPushTest extends LinearOpMode {
+    private static double avgPos(List<? extends Encoder> es) {
+        double avgPos = 0;
+        for (Encoder e : es) {
+            avgPos += e.getPositionAndVelocity().position;
+        }
+        return avgPos / es.size();
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        DriveView view = new DriveView(hardwareMap);
+
+        for (DcMotorEx m : view.motors) {
+            m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        }
+
+        waitForStart();
+
+        double initAvgPos = avgPos(view.forwardEncs);
+        while (opModeIsActive()) {
+            telemetry.addData("ticks traveled", avgPos(view.forwardEncs) - initAvgPos);
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java

@@ -0,0 +1,74 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+
+import org.firstinspires.ftc.teamcode.util.Encoder;
+import org.firstinspires.ftc.teamcode.util.MidpointTimer;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public final class ForwardRampLogger extends LinearOpMode {
+    private static double power(double seconds) {
+        return Math.min(0.1 * seconds, 0.9);
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        DriveView view = new DriveView(hardwareMap);
+
+        class Data {
+            final String type = view.type;
+
+            final List<List<Double>> powerTimes = new ArrayList<>();
+            final List<List<Double>> powers = new ArrayList<>();
+
+            final List<Double> voltageTimes = new ArrayList<>();
+            final List<Double> voltages = new ArrayList<>();
+
+            final List<Double> encTimes = new ArrayList<>();
+            final List<List<Integer>> forwardEncPositions = new ArrayList<>();
+            final List<List<Integer>> forwardEncVels = new ArrayList<>();
+        }
+
+        Data data = new Data();
+        for (DcMotorEx m : view.motors) {
+            data.powerTimes.add(new ArrayList<>());
+            data.powers.add(new ArrayList<>());
+        }
+        for (Encoder e : view.forwardEncs) {
+            data.forwardEncPositions.add(new ArrayList<>());
+            data.forwardEncVels.add(new ArrayList<>());
+        }
+
+        waitForStart();
+
+        MidpointTimer t = new MidpointTimer();
+        while (opModeIsActive()) {
+            for (int i = 0; i < view.motors.size(); i++) {
+                double power = power(t.seconds());
+                view.motors.get(i).setPower(power);
+
+                data.powers.get(i).add(power);
+                data.powerTimes.get(i).add(t.addSplit());
+            }
+
+            data.voltages.add(view.voltageSensor.getVoltage());
+            data.voltageTimes.add(t.addSplit());
+
+            for (int i = 0; i < view.forwardEncs.size(); i++) {
+                Encoder.PositionVelocityPair p = view.forwardEncs.get(i).getPositionAndVelocity();
+                data.forwardEncPositions.get(i).add(p.position);
+                data.forwardEncVels.get(i).add(p.velocity);
+            }
+            data.encTimes.add(t.addSplit());
+        }
+
+        for (DcMotorEx m : view.motors) {
+            m.setPower(0);
+        }
+
+        TuningFiles.save(TuningFiles.FileType.FORWARD_RAMP, data);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LateralPushTest.java

@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.acmerobotics.roadrunner.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.MecanumDrive;
+
+public final class LateralPushTest extends LinearOpMode {
+    private static double lateralSum(MecanumDrive.DriveLocalizer dl) {
+        return 0.25 * (
+                -dl.leftFront.getPositionAndVelocity().position
+                +dl.leftRear.getPositionAndVelocity().position
+                -dl.rightRear.getPositionAndVelocity().position
+                +dl.rightFront.getPositionAndVelocity().position);
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        if (!TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
+            throw new RuntimeException(getClass().getSimpleName() + " is for mecanum drives only.");
+        }
+
+        MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+        MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) drive.localizer;
+
+        drive.leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        drive.leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        drive.rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        drive.rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+
+        waitForStart();
+
+        double initLateralSum = lateralSum(dl);
+        while (opModeIsActive()) {
+            telemetry.addData("ticks traveled", lateralSum(dl) - initLateralSum);
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java

@@ -0,0 +1,53 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.acmerobotics.roadrunner.Trajectory;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.MecanumDrive;
+import org.firstinspires.ftc.teamcode.TankDrive;
+
+public final class ManualFeedbackTuner extends LinearOpMode {
+    public static double DISTANCE = 64;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
+            MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+            Trajectory forward = drive.trajectoryBuilder(drive.pose)
+                    .forward(DISTANCE)
+                    .build();
+
+            Trajectory backward = drive.trajectoryBuilder(forward, true)
+                    .forward(-DISTANCE)
+                    .build();
+
+            waitForStart();
+
+            while (opModeIsActive()) {
+                drive.followTrajectory(forward).runBlocking();
+                drive.followTrajectory(backward).runBlocking();
+            }
+        } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
+            TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+            Trajectory forward = drive.trajectoryBuilder(drive.pose)
+                    .forward(DISTANCE)
+                    .build();
+
+            Trajectory backward = drive.trajectoryBuilder(forward, true)
+                    .forward(-DISTANCE)
+                    .build();
+
+            waitForStart();
+
+            while (opModeIsActive()) {
+                drive.followTrajectory(forward).runBlocking();
+                drive.followTrajectory(backward).runBlocking();
+            }
+        } else {
+            throw new AssertionError();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java

@@ -0,0 +1,101 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.DualNum;
+import com.acmerobotics.roadrunner.Profiles;
+import com.acmerobotics.roadrunner.Time;
+import com.acmerobotics.roadrunner.TimeProfile;
+import com.acmerobotics.roadrunner.Twist2d;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+@Config
+public class ManualFeedforwardTuner extends LinearOpMode {
+    public static double DISTANCE = 64;
+
+    enum Mode {
+        DRIVER_MODE,
+        TUNING_MODE
+    }
+
+    @Override
+    public void runOpMode() {
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        final DriveView view = new DriveView(hardwareMap);
+
+        final TimeProfile profile = new TimeProfile(Profiles.constantProfile(
+                DISTANCE, 0, view.maxVel, view.minAccel, view.maxAccel).baseProfile);
+
+        Mode mode = Mode.TUNING_MODE;
+
+        telemetry.addLine("Ready!");
+        telemetry.update();
+        telemetry.clearAll();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        boolean movingForwards = true;
+        double startTs = System.nanoTime() / 1e9;
+
+        while (!isStopRequested()) {
+            telemetry.addData("mode", mode);
+
+            switch (mode) {
+                case TUNING_MODE: {
+                    if (gamepad1.y) {
+                        mode = Mode.DRIVER_MODE;
+                    }
+
+                    for (int i = 0; i < view.forwardEncs.size(); i++) {
+                        double v = view.forwardEncs.get(i).getPositionAndVelocity().velocity;
+                        telemetry.addData("v" + i, view.inPerTick * v);
+                    }
+
+                    double ts = System.nanoTime() / 1e9;
+                    double t = ts - startTs;
+                    if (t > profile.duration) {
+                        movingForwards = !movingForwards;
+                        startTs = ts;
+                    }
+
+                    DualNum<Time> v = profile.get(t).drop(1);
+                    if (!movingForwards) {
+                        v = v.unaryMinus();
+                    }
+                    telemetry.addData("vref", v.get(0));
+
+                    double power = view.feedforward().compute(v) / view.voltageSensor.getVoltage();
+                    view.setDrivePowers(new Twist2d(new Vector2d(power, 0), 0));
+
+                    break;
+                }
+                case DRIVER_MODE: {
+                    if (gamepad1.b) {
+                        mode = Mode.TUNING_MODE;
+                        movingForwards = true;
+                        startTs = System.nanoTime() / 1e9;
+                    }
+
+                    view.setDrivePowers(
+                            new Twist2d(
+                                    new Vector2d(
+                                            -gamepad1.left_stick_y,
+                                            -gamepad1.left_stick_x
+                                    ),
+                                    -gamepad1.right_stick_x
+                            )
+                    );
+
+                    break;
+                }
+            }
+
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MecanumMotorDirectionDebugger.java

@@ -0,0 +1,93 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.MecanumDrive;
+
+/**
+ * This is a simple teleop routine for debugging your motor configuration.
+ * Pressing each of the buttons will power its respective motor.
+ *
+ * Button Mappings:
+ *
+ * Xbox/PS4 Button - Motor
+ *   X / ▢         - Front Left
+ *   Y / Δ         - Front Right
+ *   B / O         - Rear  Right
+ *   A / X         - Rear  Left
+ *                                    The buttons are mapped to match the wheels spatially if you
+ *                                    were to rotate the gamepad 45deg°. x/square is the front left
+ *                    ________        and each button corresponds to the wheel as you go clockwise
+ *                   / ______ \
+ *     ------------.-'   _  '-..+              Front of Bot
+ *              /   _  ( Y )  _  \                  ^
+ *             |  ( X )  _  ( B ) |     Front Left   \    Front Right
+ *        ___  '.      ( A )     /|       Wheel       \      Wheel
+ *      .'    '.    '-._____.-'  .'       (x/▢)        \     (Y/Δ)
+ *     |       |                 |                      \
+ *      '.___.' '.               |          Rear Left    \   Rear Right
+ *               '.             /             Wheel       \    Wheel
+ *                \.          .'              (A/X)        \   (B/O)
+ *                  \________/
+ */
+@Config
+public class MecanumMotorDirectionDebugger extends LinearOpMode {
+    public static double MOTOR_POWER = 0.7;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        if (!TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
+            throw new RuntimeException(getClass().getSimpleName() + " is for mecanum drives only.");
+        }
+
+        MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        telemetry.addLine("Press play to begin the debugging op mode");
+        telemetry.update();
+
+        waitForStart();
+
+        telemetry.clearAll();
+        telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
+
+        while (opModeIsActive()) {
+            telemetry.addLine("Press each button to turn on its respective motor");
+            telemetry.addLine();
+            telemetry.addLine("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;X / ▢&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Left</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;Y / Δ&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Right</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;B / O&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Right</font>");
+            telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;A / X&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Left</font>");
+            telemetry.addLine();
+
+            if (gamepad1.x) {
+                drive.leftFront.setPower(MOTOR_POWER);
+                telemetry.addLine("Running Motor: Front Left");
+            } else if (gamepad1.y) {
+                drive.rightFront.setPower(MOTOR_POWER);
+                telemetry.addLine("Running Motor: Front Right");
+            } else if (gamepad1.b) {
+                drive.rightBack.setPower(MOTOR_POWER);
+                telemetry.addLine("Running Motor: Rear Right");
+            } else if (gamepad1.a) {
+                drive.leftBack.setPower(MOTOR_POWER);
+                telemetry.addLine("Running Motor: Rear Left");
+            } else {
+                drive.leftFront.setPower(0);
+                drive.rightFront.setPower(0);
+                drive.rightBack.setPower(0);
+                drive.leftBack.setPower(0);
+                telemetry.addLine("Running Motor: None");
+            }
+
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java

@@ -0,0 +1,39 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.MecanumDrive;
+import org.firstinspires.ftc.teamcode.TankDrive;
+
+public final class SplineTest extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
+            MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+            waitForStart();
+
+            drive.followTrajectory(
+                    drive.trajectoryBuilder(drive.pose)
+                            .splineTo(new Vector2d(30, 30), Math.PI / 2)
+                            .splineTo(new Vector2d(60, 0), Math.PI)
+                            .build()
+            ).runBlocking();
+        } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
+            TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+            waitForStart();
+
+            drive.followTrajectory(
+                    drive.trajectoryBuilder(drive.pose)
+                            .splineTo(new Vector2d(30, 30), Math.PI / 2)
+                            .splineTo(new Vector2d(60, 0), Math.PI)
+                            .build()
+            ).runBlocking();
+        } else {
+            throw new AssertionError();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningFiles.java

@@ -0,0 +1,174 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import android.content.Context;
+import android.content.res.AssetManager;
+
+import com.fasterxml.jackson.core.JsonFactory;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.qualcomm.robotcore.util.RobotLog;
+import com.qualcomm.robotcore.util.WebHandlerManager;
+
+import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar;
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+import org.firstinspires.ftc.robotcore.internal.webserver.WebHandler;
+import org.firstinspires.ftc.robotserver.internal.webserver.MimeTypesUtil;
+
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.IOException;
+import java.util.Objects;
+
+import fi.iki.elonen.NanoHTTPD;
+
+public final class TuningFiles {
+    private static final File ROOT =
+            new File(AppUtil.ROOT_FOLDER + "/RoadRunner/tuning/");
+
+    private static final Object IO_LOCK = new Object();
+
+    private static WebHandlerManager whm; // guarded by ioLock
+
+    public enum FileType {
+        FORWARD_RAMP("forward-ramp"),
+        ANGULAR_RAMP("angular-ramp"),
+        ACCEL("accel");
+
+        public final String name;
+
+        FileType(String s) {
+            name = s;
+        }
+    }
+
+    private TuningFiles() {
+
+    }
+
+    private static File getFileTypeDir(FileType ty) {
+        return new File(ROOT, ty.name);
+    }
+
+    public static void save(FileType ty, Object data) {
+        synchronized (IO_LOCK) {
+            File f = new File(getFileTypeDir(ty), System.currentTimeMillis() + ".json");
+
+            try {
+                new ObjectMapper(new JsonFactory())
+                        .writerWithDefaultPrettyPrinter()
+                        .writeValue(f, data);
+
+                if (whm != null) {
+                    whm.register("/tuning/" + ty.name + "/" + f.getName(),
+                            newStaticFileHandler(f));
+                }
+            } catch (IOException e) {
+                RobotLog.setGlobalErrorMsg(new RuntimeException(e),
+                        "Unable to write data to " + f.getAbsolutePath());
+            }
+        }
+    }
+
+    @WebHandlerRegistrar
+    public static void registerRoutes(Context context, WebHandlerManager manager) {
+        synchronized (IO_LOCK) {
+            AssetManager assetManager = context.getAssets();
+            registerAssetsUnderPath(manager, assetManager, "tuning");
+            for (FileType ty : FileType.values()) {
+                String base = "/tuning/" + ty.name;
+
+                WebHandler wh = newStaticAssetHandler(assetManager, ty.name + "/index.html");
+                manager.register(base, wh);
+                manager.register(base + "/", wh);
+
+                File dir = getFileTypeDir(ty);
+                //noinspection ResultOfMethodCallIgnored
+                dir.mkdirs();
+
+                manager.register(base + "/latest.json",
+                        newLatestFileHandler(dir));
+                for (File f : Objects.requireNonNull(dir.listFiles())) {
+                    manager.register(base + "/" + f.getName(),
+                            newStaticFileHandler(f));
+                }
+            }
+
+            whm = manager;
+        }
+    }
+
+    private static WebHandler newStaticAssetHandler(AssetManager assetManager, String file) {
+        return session -> {
+            if (session.getMethod() == NanoHTTPD.Method.GET) {
+                String mimeType = MimeTypesUtil.determineMimeType(file);
+                return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
+                        mimeType, assetManager.open(file));
+            } else {
+                return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
+                        NanoHTTPD.MIME_PLAINTEXT, "");
+            }
+        };
+    }
+
+    private static void registerAssetsUnderPath(WebHandlerManager webHandlerManager,
+                                                AssetManager assetManager, String path) {
+        try {
+            String[] list = assetManager.list(path);
+
+            if (list == null) return;
+
+            if (list.length > 0) {
+                for (String file : list) {
+                    registerAssetsUnderPath(webHandlerManager, assetManager, path + "/" + file);
+                }
+            } else {
+                webHandlerManager.register(path, newStaticAssetHandler(assetManager, path));
+            }
+        } catch (IOException e) {
+            RobotLog.setGlobalErrorMsg(new RuntimeException(e),
+                    "unable to register tuning web routes");
+        }
+    }
+
+
+    private static WebHandler newLatestFileHandler(File dir) {
+        return session -> {
+            File[] files = dir.listFiles();
+            if (files != null) {
+                long mostRecentLastModified = 0;
+                File mostRecentFile = null;
+                for (File f : files) {
+                    long lastModified = f.lastModified();
+                    if (lastModified > mostRecentLastModified) {
+                        mostRecentLastModified = lastModified;
+                        mostRecentFile = f;
+                    }
+                }
+
+                if (mostRecentFile != null) {
+                    String mimeType = MimeTypesUtil.determineMimeType(mostRecentFile.getName());
+                    final NanoHTTPD.Response res = NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
+                            mimeType,
+                            new FileInputStream(mostRecentFile));
+                    res.addHeader("X-Filename", mostRecentFile.getName());
+                    return res;
+                }
+            }
+
+            return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
+                    NanoHTTPD.MIME_PLAINTEXT, "");
+        };
+    }
+
+    private static WebHandler newStaticFileHandler(File f) {
+        return session -> {
+            if (session.getMethod() == NanoHTTPD.Method.GET) {
+                String mimeType = MimeTypesUtil.determineMimeType(f.getName());
+                return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
+                        mimeType, new FileInputStream(f));
+            } else {
+                return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
+                        NanoHTTPD.MIME_PLAINTEXT, "");
+            }
+        };
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java

@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar;
+
+import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta;
+import org.firstinspires.ftc.teamcode.MecanumDrive;
+
+import java.util.Arrays;
+import java.util.List;
+
+public final class TuningOpModes {
+    public static final Class<?> DRIVE_CLASS = MecanumDrive.class;
+
+    public static final String GROUP = "quickstart";
+    public static final boolean DISABLED = false;
+
+    private TuningOpModes() {
+
+    }
+
+    @OpModeRegistrar
+    public static void register(OpModeManager manager) {
+        if (DISABLED) return;
+
+        List<Class<? extends OpMode>> opModes = Arrays.asList(
+                AccelLogger.class,
+                AngularRampLogger.class,
+                ForwardPushTest.class,
+                ForwardRampLogger.class,
+                LateralPushTest.class,
+                ManualFeedbackTuner.class,
+                ManualFeedforwardTuner.class,
+                SplineTest.class,
+                MecanumMotorDirectionDebugger.class
+        );
+
+        for (Class<? extends OpMode> o : opModes) {
+            manager.register(new OpModeMeta.Builder()
+                    .setName(o.getSimpleName())
+                    .setGroup(GROUP)
+                    .setFlavor(OpModeMeta.Flavor.TELEOP)
+                    .build(), o);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/BNO055Wrapper.java

@@ -0,0 +1,148 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.acmerobotics.roadrunner.Rotation2d;
+import com.qualcomm.hardware.bosch.BNO055IMU;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+
+public final class BNO055Wrapper {
+    private final BNO055IMU bno;
+
+    public BNO055Wrapper(BNO055IMU bno) {
+        this.bno = bno;
+    }
+
+    public Rotation2d getHeading() {
+        return Rotation2d.exp(bno.getAngularOrientation()
+                .toAngleUnit(AngleUnit.RADIANS)
+                .toAxesOrder(AxesOrder.ZYX)
+                .firstAngle);
+    }
+
+    public double getHeadingVelocity() {
+        return getAngularVelocity().zRotationRate;
+    }
+
+    public AngularVelocity getAngularVelocity() {
+        return bno.getAngularVelocity().toAngleUnit(AngleUnit.RADIANS);
+    }
+
+    /**
+     * A direction for an axis to be remapped to
+     */
+    public enum AxisDirection {
+        POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z
+    }
+
+    /**
+     * IMU axes signs in the order XYZ (after remapping).
+     */
+    public enum AxesSigns {
+        PPP(0b000),
+        PPN(0b001),
+        PNP(0b010),
+        PNN(0b011),
+        NPP(0b100),
+        NPN(0b101),
+        NNP(0b110),
+        NNN(0b111);
+
+        public final int bVal;
+
+        AxesSigns(int bVal) {
+            this.bVal = bVal;
+        }
+    }
+
+    /**
+     * Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#XYZ}.
+     * Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
+     * mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
+     *
+     * Adapted from <a href="https://ftcforum.firstinspires.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587">this post</a>.
+     * Reference the <a href="https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf">BNO055 Datasheet</a> for details.
+     *
+     * NOTE: Remapping axes can be somewhat confusing. Instead, use {@link #remapZAxis}, if
+     * appropriate.
+     *
+     * @param order axes order
+     * @param signs axes signs
+     */
+    public void swapThenFlipAxes(AxesOrder order, AxesSigns signs) {
+        try {
+            // the indices correspond with the 2-bit axis encodings specified in the datasheet
+            int[] indices = order.indices();
+            // AxesSign's values align with the datasheet
+            int axisMapSigns = signs.bVal;
+
+            if (indices[0] == indices[1] || indices[0] == indices[2] || indices[1] == indices[2]) {
+                throw new RuntimeException("Same axis cannot be included in AxesOrder twice");
+            }
+
+            // ensure right-handed coordinate system
+            boolean isXSwapped = indices[0] != 0;
+            boolean isYSwapped = indices[1] != 1;
+            boolean isZSwapped = indices[2] != 2;
+            boolean areTwoAxesSwapped = (isXSwapped || isYSwapped || isZSwapped)
+                    && (!isXSwapped || !isYSwapped || !isZSwapped);
+            boolean oddNumOfFlips = (((axisMapSigns >> 2) ^ (axisMapSigns >> 1) ^ axisMapSigns) & 1) == 1;
+            // != functions as xor
+            if (areTwoAxesSwapped != oddNumOfFlips) {
+                throw new RuntimeException("Coordinate system is left-handed");
+            }
+
+            // Bit:  7  6 |  5  4  |  3  2  |  1  0  |
+            //   reserved | z axis | y axis | x axis |
+            int axisMapConfig = indices[2] << 4 | indices[1] << 2 | indices[0];
+
+            // Enter CONFIG mode
+            bno.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
+
+            Thread.sleep(100);
+
+            // Write the AXIS_MAP_CONFIG register
+            bno.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
+
+            // Write the AXIS_MAP_SIGN register
+            bno.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSigns & 0x07);
+
+            // Switch back to the previous mode
+            bno.write8(BNO055IMU.Register.OPR_MODE, bno.getParameters().mode.bVal & 0x0F);
+
+            Thread.sleep(100);
+        } catch (InterruptedException e) {
+            Thread.currentThread().interrupt();
+        }
+    }
+
+    /**
+     * Remaps the IMU coordinate system so that the remapped +Z faces the provided
+     * {@link AxisDirection}. See {@link #swapThenFlipAxes} for details about the remapping.
+     *
+     * @param direction axis direction
+     */
+    public void remapZAxis(AxisDirection direction) {
+        switch (direction) {
+            case POS_X:
+                swapThenFlipAxes(AxesOrder.ZYX, AxesSigns.NPP);
+                break;
+            case NEG_X:
+                swapThenFlipAxes(AxesOrder.ZYX, AxesSigns.PPN);
+                break;
+            case POS_Y:
+                swapThenFlipAxes(AxesOrder.XZY, AxesSigns.PNP);
+                break;
+            case NEG_Y:
+                swapThenFlipAxes(AxesOrder.XZY, AxesSigns.PPN);
+                break;
+            case POS_Z:
+                swapThenFlipAxes(AxesOrder.XYZ, AxesSigns.PPP);
+                break;
+            case NEG_Z:
+                swapThenFlipAxes(AxesOrder.XYZ, AxesSigns.PNN);
+                break;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java

@@ -0,0 +1,18 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.qualcomm.robotcore.hardware.DcMotorController;
+
+public interface Encoder {
+    class PositionVelocityPair {
+        public final int position, velocity;
+
+        public PositionVelocityPair(int position, int velocity) {
+            this.position = position;
+            this.velocity = velocity;
+        }
+    }
+
+    PositionVelocityPair getPositionAndVelocity();
+
+    DcMotorController getController();
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java

@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.acmerobotics.roadrunner.Time;
+import com.acmerobotics.roadrunner.Twist2dIncrDual;
+
+public interface Localizer {
+    Twist2dIncrDual<Time> updateAndGetIncr();
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxFirmwareVersion.java

@@ -0,0 +1,108 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Parsed representation of a Lynx module firmware version.
+ */
+public final class LynxFirmwareVersion implements Comparable<LynxFirmwareVersion> {
+
+    /**
+     * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
+     * @param hardwareMap hardware map containing Lynx modules
+     */
+    public static void throwIfAnyModulesBelowVersion(HardwareMap hardwareMap, LynxFirmwareVersion minVersion) {
+        Map<String, LynxFirmwareVersion> outdatedModules = new HashMap<>();
+
+        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+            LynxFirmwareVersion version = LynxFirmwareVersion.parse(module.getNullableFirmwareVersionString());
+            if (version == null || version.compareTo(minVersion) < 0) {
+                for (String name : hardwareMap.getNamesOf(module)) {
+                    outdatedModules.put(name, version);
+                }
+            }
+        }
+
+        if (outdatedModules.size() > 0) {
+            StringBuilder msgBuilder = new StringBuilder();
+            msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n");
+            msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n",
+                    minVersion.toString()));
+
+            for (Map.Entry<String, LynxFirmwareVersion> entry : outdatedModules.entrySet()) {
+                msgBuilder.append(Misc.formatInvariant(
+                        "\t%s: %s\n", entry.getKey(),
+                        entry.getValue() == null ? "Unknown" : entry.getValue().toString()));
+            }
+
+            throw new RuntimeException(msgBuilder.toString());
+        }
+    }
+
+    /**
+     * Parses Lynx module firmware version string into structured version object. Returns null for
+     * null argument or upon error.
+     */
+    public static LynxFirmwareVersion parse(String s) {
+        if (s == null) {
+            return null;
+        }
+
+        String[] parts = s.split("[ :,]+");
+        try {
+            // note: for now, we ignore the hardware entry
+            return new LynxFirmwareVersion(
+                    Integer.parseInt(parts[3]),
+                    Integer.parseInt(parts[5]),
+                    Integer.parseInt(parts[7])
+            );
+        } catch (NumberFormatException e) {
+            return null;
+        }
+    }
+
+    public final int major, minor, eng;
+
+    public LynxFirmwareVersion(int major, int minor, int eng) {
+        this.major = major;
+        this.minor = minor;
+        this.eng = eng;
+    }
+
+    @Override
+    public boolean equals(Object other) {
+        if (other instanceof LynxFirmwareVersion) {
+            LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other;
+            return major == otherVersion.major && minor == otherVersion.minor &&
+                    eng == otherVersion.eng;
+        } else {
+            return false;
+        }
+    }
+
+    @Override
+    public int compareTo(LynxFirmwareVersion other) {
+        int majorComp = Integer.compare(major, other.major);
+        if (majorComp == 0) {
+            int minorComp = Integer.compare(minor, other.minor);
+            if (minorComp == 0) {
+                return Integer.compare(eng, other.eng);
+            } else {
+                return minorComp;
+            }
+        } else {
+            return majorComp;
+        }
+    }
+
+    @Override
+    public String toString() {
+        return Misc.formatInvariant("%d.%d.%d", major, minor, eng);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/MidpointTimer.java

@@ -0,0 +1,17 @@
+package org.firstinspires.ftc.teamcode.util;
+
+public final class MidpointTimer {
+    private final long beginTs = System.nanoTime();
+    private long lastTime;
+
+    public double seconds() {
+        return 1e-9 * (System.nanoTime() - beginTs);
+    }
+
+    public double addSplit() {
+        long time = System.nanoTime() - beginTs;
+        double midTimeSecs = 0.5e-9 * (lastTime + time);
+        lastTime = time;
+        return midTimeSecs;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OverflowEncoder.java

@@ -0,0 +1,57 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.qualcomm.robotcore.hardware.DcMotorController;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public final class OverflowEncoder implements Encoder {
+    // encoder velocities are sent as 16-bit ints
+    // by the time they reach here, they are widened into an int and possibly negated
+    private static final int CPS_STEP = 0x10000;
+
+    private static int inverseOverflow(int input, double estimate) {
+        // convert to uint16
+        int real = input & 0xFFFF;
+        // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
+        // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
+        real += ((real % 20) / 4) * CPS_STEP;
+        // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
+        real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
+        return real;
+    }
+
+    public final RawEncoder encoder;
+
+    private int lastPosition;
+    private final ElapsedTime lastUpdate;
+
+    private final RollingThreeMedian velEstimate;
+
+    public OverflowEncoder(RawEncoder e) {
+        encoder = e;
+
+        lastPosition = e.getPositionAndVelocity().position;
+        lastUpdate = new ElapsedTime();
+
+        velEstimate = new RollingThreeMedian();
+    }
+
+    @Override
+    public PositionVelocityPair getPositionAndVelocity() {
+        PositionVelocityPair p = encoder.getPositionAndVelocity();
+        double dt = lastUpdate.seconds();
+        double v = velEstimate.update((p.position - lastPosition) / dt);
+
+        lastPosition = p.position;
+        lastUpdate.reset();
+
+        return new PositionVelocityPair(
+                p.position,
+                inverseOverflow(p.velocity, v)
+        );
+    }
+
+    @Override
+    public DcMotorController getController() {
+        return encoder.getController();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Paths.java

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.acmerobotics.roadrunner.Curves;
+import com.acmerobotics.roadrunner.PosePath;
+import com.acmerobotics.roadrunner.Vector2d;
+
+import java.util.List;
+
+public final class Paths {
+    private Paths() {
+
+    }
+
+    // at least 6 inch resolution
+    public static double project(PosePath path, Vector2d query) {
+        List<Double> xs = com.acmerobotics.roadrunner.Math.range(
+                0.0, path.length(), (int) Math.ceil(path.length() / 6.0));
+        double champDisp = Curves.project(path, query, xs.get(0));
+        final Vector2d initVec = path.get(champDisp, 1).trans.value();
+        double champSqrNorm = initVec.minus(query).sqrNorm();
+        for (int i = 1; i < xs.size(); i++) {
+            double disp = Curves.project(path, query, xs.get(i));
+            Vector2d vec = path.get(disp, 1).trans.value();
+            double sqrNorm = vec.minus(query).sqrNorm();
+
+            if (sqrNorm < champSqrNorm) {
+                champDisp = disp;
+                champSqrNorm = sqrNorm;
+            }
+        }
+
+        return champDisp;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RawEncoder.java

@@ -0,0 +1,39 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.qualcomm.robotcore.hardware.DcMotorController;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+public final class RawEncoder implements Encoder {
+    private final DcMotorEx m;
+    public DcMotorSimple.Direction direction = DcMotorSimple.Direction.FORWARD;
+
+    public RawEncoder(DcMotorEx m) {
+        this.m = m;
+    }
+
+    private int applyDirection(int x) {
+        if (m.getDirection() == DcMotorSimple.Direction.REVERSE) {
+            x = -x;
+        }
+
+        if (direction == DcMotorSimple.Direction.REVERSE) {
+            x = -x;
+        }
+
+        return x;
+    }
+
+    @Override
+    public PositionVelocityPair getPositionAndVelocity() {
+        return new PositionVelocityPair(
+                applyDirection(m.getCurrentPosition()),
+                applyDirection((int) m.getVelocity())
+        );
+    }
+
+    @Override
+    public DcMotorController getController() {
+        return m.getController();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RollingThreeMedian.java

@@ -0,0 +1,16 @@
+package org.firstinspires.ftc.teamcode.util;
+
+public final class RollingThreeMedian {
+    private final double[] history = new double[3];
+    private int i;
+
+    public double update(double x) {
+        history[i] = x;
+
+        i = (i + 1) % 3;
+
+        return history[0] + history[1] + history[2]
+                - Math.min(history[0], Math.min(history[1], history[2]))
+                - Math.max(history[0], Math.max(history[1], history[2]));
+    }
+}