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>