Add initial quickstart files
This commit is contained in:
Ryan Brott
3
.gitignore
vendored
3
.gitignore
vendored
@ -85,3 +85,6 @@ lint/generated/
|
||||
lint/outputs/
|
||||
lint/tmp/
|
||||
# lint/reports/
|
||||
|
||||
.DS_Store
|
||||
/.idea
|
||||
|
||||
@ -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'
|
||||
}
|
||||
|
||||
2
TeamCode/src/main/assets/tuning/.gitignore
vendored
Normal file
2
TeamCode/src/main/assets/tuning/.gitignore
vendored
Normal file
@ -0,0 +1,2 @@
|
||||
latest.json
|
||||
*.py
|
||||
210
TeamCode/src/main/assets/tuning/common.js
Normal file
210
TeamCode/src/main/assets/tuning/common.js
Normal file
@ -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();
|
||||
};
|
||||
}
|
||||
159
TeamCode/src/main/assets/tuning/dead-wheel-angular-ramp.html
Normal file
159
TeamCode/src/main/assets/tuning/dead-wheel-angular-ramp.html
Normal file
@ -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>
|
||||
141
TeamCode/src/main/assets/tuning/drive-encoder-angular-ramp.html
Normal file
141
TeamCode/src/main/assets/tuning/drive-encoder-angular-ramp.html
Normal file
@ -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>
|
||||
108
TeamCode/src/main/assets/tuning/forward-ramp.html
Normal file
108
TeamCode/src/main/assets/tuning/forward-ramp.html
Normal file
@ -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>
|
||||
65
TeamCode/src/main/assets/tuning/plotly-2.12.1.min.js
vendored
Normal file
65
TeamCode/src/main/assets/tuning/plotly-2.12.1.min.js
vendored
Normal file
File diff suppressed because one or more lines are too long
1
TeamCode/src/main/assets/tuning/regression-2.0.1.min.js
vendored
Normal file
1
TeamCode/src/main/assets/tuning/regression-2.0.1.min.js
vendored
Normal file
File diff suppressed because one or more lines are too long
118
TeamCode/src/main/assets/tuning/test.html
Normal file
118
TeamCode/src/main/assets/tuning/test.html
Normal file
@ -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>
|
||||
219
TeamCode/src/main/assets/tuning/test/enc1.csv
Normal file
219
TeamCode/src/main/assets/tuning/test/enc1.csv
Normal file
@ -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
|
||||
|
308
TeamCode/src/main/assets/tuning/test/enc2.csv
Normal file
308
TeamCode/src/main/assets/tuning/test/enc2.csv
Normal file
@ -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
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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\"> X / ▢ - Front Left</font>");
|
||||
telemetry.addLine("<font face=\"monospace\"> Y / Δ - Front Right</font>");
|
||||
telemetry.addLine("<font face=\"monospace\"> B / O - Rear Right</font>");
|
||||
telemetry.addLine("<font face=\"monospace\"> A / X - Rear 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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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, "");
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
@ -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]));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user