Skip to content

Commit

Permalink
Merge pull request #1220 from tervay/armdiffeq
Browse files Browse the repository at this point in the history
  • Loading branch information
tervay authored Dec 17, 2023
2 parents d1409df + 3715baa commit 9692a5c
Show file tree
Hide file tree
Showing 13 changed files with 584 additions and 881 deletions.
44 changes: 22 additions & 22 deletions cypress/e2e/flywheel.cy.ts
Original file line number Diff line number Diff line change
Expand Up @@ -114,8 +114,8 @@ describe("Flywheel Calculator e2e tests", () => {
flywheelWeight: 1.5,
flywheelShooterRatio: 1,
shooterMaxSpeed: 11760,
customShooterMOI: 4.5,
customFlywheelMOI: "3.0",
customShooterMOI: "4.500",
customFlywheelMOI: "3.000",
windupTime: 2.64,
recoveryTime: 0.6129,
surfaceSpeed: 287.98,
Expand All @@ -142,8 +142,8 @@ describe("Flywheel Calculator e2e tests", () => {
flywheelWeight: 1.5,
flywheelShooterRatio: 1,
shooterMaxSpeed: 11760,
customShooterMOI: 4.5,
customFlywheelMOI: "3.0",
customShooterMOI: "4.500",
customFlywheelMOI: "3.000",
windupTime: 5.56,
recoveryTime: 1.2903,
surfaceSpeed: 287.98,
Expand All @@ -170,8 +170,8 @@ describe("Flywheel Calculator e2e tests", () => {
flywheelWeight: 1.5,
flywheelShooterRatio: 1,
shooterMaxSpeed: 11760,
customShooterMOI: 4.5,
customFlywheelMOI: "3.0",
customShooterMOI: "4.500",
customFlywheelMOI: "3.000",
windupTime: 1.27,
recoveryTime: 0.1042,
surfaceSpeed: 148.68,
Expand All @@ -198,8 +198,8 @@ describe("Flywheel Calculator e2e tests", () => {
flywheelWeight: 1.5,
flywheelShooterRatio: 1,
shooterMaxSpeed: 11760,
customShooterMOI: 4.5,
customFlywheelMOI: "3.0",
customShooterMOI: "4.500",
customFlywheelMOI: "3.000",
windupTime: "3.80",
recoveryTime: 0.8834,
surfaceSpeed: 287.98,
Expand All @@ -226,8 +226,8 @@ describe("Flywheel Calculator e2e tests", () => {
flywheelWeight: 1.5,
flywheelShooterRatio: 1,
shooterMaxSpeed: 17640,
customShooterMOI: 4.5,
customFlywheelMOI: "3.0",
customShooterMOI: "4.500",
customFlywheelMOI: "3.000",
windupTime: 2.82,
recoveryTime: 0.4087,
surfaceSpeed: 287.98,
Expand All @@ -254,8 +254,8 @@ describe("Flywheel Calculator e2e tests", () => {
flywheelWeight: 1.5,
flywheelShooterRatio: 1,
shooterMaxSpeed: 11760,
customShooterMOI: 4.5,
customFlywheelMOI: "3.0",
customShooterMOI: "4.500",
customFlywheelMOI: "3.000",
windupTime: 5.28,
recoveryTime: 1.4192,
surfaceSpeed: 287.98,
Expand All @@ -282,8 +282,8 @@ describe("Flywheel Calculator e2e tests", () => {
flywheelWeight: 1.5,
flywheelShooterRatio: 1,
shooterMaxSpeed: 11760,
customShooterMOI: "8.0",
customFlywheelMOI: "3.0",
customShooterMOI: "8.000",
customFlywheelMOI: "3.000",
windupTime: 7.74,
recoveryTime: "1.5480",
surfaceSpeed: 383.97,
Expand All @@ -310,8 +310,8 @@ describe("Flywheel Calculator e2e tests", () => {
flywheelWeight: 1.5,
flywheelShooterRatio: 1,
shooterMaxSpeed: 11760,
customShooterMOI: 22.5,
customFlywheelMOI: "3.0",
customShooterMOI: "22.500",
customFlywheelMOI: "3.000",
windupTime: 17.95,
recoveryTime: 6.8046,
surfaceSpeed: 287.98,
Expand All @@ -338,8 +338,8 @@ describe("Flywheel Calculator e2e tests", () => {
flywheelWeight: 1.5,
flywheelShooterRatio: 1,
shooterMaxSpeed: 11760,
customShooterMOI: 4.5,
customFlywheelMOI: "12.0",
customShooterMOI: "4.500",
customFlywheelMOI: "12.000",
windupTime: 11.61,
recoveryTime: 4.0272,
surfaceSpeed: 287.98,
Expand All @@ -366,8 +366,8 @@ describe("Flywheel Calculator e2e tests", () => {
flywheelRadius: 2,
flywheelShooterRatio: 1,
shooterMaxSpeed: 11760,
customShooterMOI: 4.5,
customFlywheelMOI: "4.0",
customShooterMOI: "4.500",
customFlywheelMOI: "4.000",
windupTime: 5.98,
recoveryTime: 1.5134,
surfaceSpeed: 287.98,
Expand All @@ -394,8 +394,8 @@ describe("Flywheel Calculator e2e tests", () => {
flywheelRadius: 2,
flywheelWeight: 1.5,
shooterMaxSpeed: 11760,
customShooterMOI: 4.5,
customFlywheelMOI: "3.0",
customShooterMOI: "4.500",
customFlywheelMOI: "3.000",
windupTime: 3.25,
recoveryTime: 0.4938,
surfaceSpeed: 287.98,
Expand Down
1 change: 1 addition & 0 deletions package.json
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
"katex": "0.16.9",
"lodash": "4.17.21",
"lz-string": "1.5.0",
"odex": "3.0.0-rc.4",
"marked": "11.1.0",
"query-string": "8.1.0",
"react": "18.2.0",
Expand Down
14 changes: 13 additions & 1 deletion src/common/models/Measurement.ts
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ export default class Measurement extends Model {
if (m.kind() === undefined) {
if (m.innerQty.isCompatible(Qty(1, "in^2 * lbs"))) {
// Moment of inertia
return ["in^2 lbs"];
return ["in^2 lbs", "kg m^2"];
} else if (m.innerQty.isCompatible("V*s/m")) {
// kV (linear)
return ["V*s/m", "V*s/ft", "V*s/in"];
Expand Down Expand Up @@ -342,4 +342,16 @@ export default class Measurement extends Model {
.mul(new Measurement(40, "A"))
.mul(new Measurement(10, "s"));
}

linearizeRadialPosition(inchesPerRevolution: Measurement): Measurement {
return this.mul(inchesPerRevolution)
.div(2 * Math.PI)
.removeRad();
}

radializeLinearPosition(inchesPerRevolution: Measurement): Measurement {
return this.div(inchesPerRevolution).mul(
new Measurement(2 * Math.PI, "rad"),
);
}
}
78 changes: 78 additions & 0 deletions src/common/models/Motor.ts
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@ import _rawMotorData from "common/models/data/motors.json";
import Measurement, { RawMeasurementJson } from "common/models/Measurement";
import Model from "common/models/Model";
import { MotorRules } from "common/models/Rules";
import ODESolver from "common/tooling/ODE";
import keyBy from "lodash/keyBy";

type RawMotorSpec = {
Expand Down Expand Up @@ -129,3 +130,80 @@ export type IncompleteMotorState = {
};

export type CompleteMotorState = Required<IncompleteMotorState>;

export type StoppingInfo = {
position: Measurement;
velocity: Measurement;
currentDraw: Measurement;
stepNumber: number;
};

export function solveMotorODE(
motor: Motor,
currentLimit: Measurement,
shouldStop: (info: StoppingInfo) => boolean,
J: Measurement,
antiTorque: Measurement,
efficiency: number,
) {
const B = new Measurement(0.00004, "N m s / rad");
const L = new Measurement(0.000035, "H");

const duration = 30;
const numStepsPerSec = 800;
const steps = duration * numStepsPerSec;

const solver = new ODESolver(
(t, y) => {
const prevVel = new Measurement(y[0], "rad/s");
const prevCurrent = new Measurement(y[1], "A");
const prevCurrLimit = new Measurement(y[2], "A");
const prevPosition = new Measurement(y[3], "rad");

const currToUse = prevCurrent.gte(prevCurrLimit)
? prevCurrLimit
: prevCurrent;
const limited = prevCurrent.gte(prevCurrLimit);

const newCurrentPerSec = nominalVoltage
.sub(motor.resistance.mul(prevCurrent))
.sub(motor.kV.inverse().mul(prevVel))
.div(L);

const newVelocityPerSec = Measurement.max(
new Measurement(0, "N m"),
motor.kT
.mul(motor.quantity)
.mul(efficiency / 100)
.mul(currToUse)
.sub(antiTorque)
.sub(B.mul(prevVel)),
)
.div(J)
.mul(new Measurement(1, "rad"))
.toBase();

return {
changeRates: [
newVelocityPerSec.scalar === 0
? 0
: newVelocityPerSec.to("rad/s2").scalar,
newCurrentPerSec.to("A/s").scalar,
limited ? 0 : newCurrentPerSec.to("A/s").scalar,
prevVel.to("rad/s").scalar,
],
shouldStop: shouldStop({
currentDraw: currToUse,
position: prevPosition,
stepNumber: t * numStepsPerSec,
velocity: prevVel,
}),
};
},
[0, motor.stallCurrent.scalar, currentLimit.scalar, 0],
0,
duration,
);

return solver.rk4(steps);
}
34 changes: 34 additions & 0 deletions src/common/models/tests/Measurement.test.ts
Original file line number Diff line number Diff line change
Expand Up @@ -283,4 +283,38 @@ describe("Measurement", () => {
expect(a.toBase()).toEqualMeasurement(b);
},
);

test("linearize works correctly", () => {
expect(
new Measurement(2 * Math.PI, "rad").linearizeRadialPosition(
new Measurement(1, "in"),
),
).toEqualMeasurement(new Measurement(1, "in"));

expect(
new Measurement(4 * Math.PI, "rad").linearizeRadialPosition(
new Measurement(2, "in"),
),
).toEqualMeasurement(new Measurement(4, "in"));

expect(
new Measurement(10 * Math.PI, "rad").linearizeRadialPosition(
new Measurement(0.5, "in"),
),
).toEqualMeasurement(new Measurement(2.5, "in"));
});

test("rotarize works correctly", () => {
expect(
new Measurement(1, "in").radializeLinearPosition(
new Measurement(1, "in"),
),
).toEqualMeasurement(new Measurement(2 * Math.PI, "rad"));

expect(
new Measurement(4, "in").radializeLinearPosition(
new Measurement(2, "in"),
),
).toEqualMeasurement(new Measurement(4 * Math.PI, "rad"));
});
});
114 changes: 114 additions & 0 deletions src/common/tooling/ODE.ts
Original file line number Diff line number Diff line change
@@ -0,0 +1,114 @@
export type ODEFunction = (
t: number,
y: number[],
) => {
changeRates: number[];
shouldStop: boolean;
};

export default class ODESolver {
constructor(
private readonly ode: ODEFunction,
private readonly y0: number[],
private readonly t0: number,
private readonly t1: number,
) {}

euler(resolution: number) {
const h = (this.t1 - this.t0) / resolution;
const ts = Array.from(Array(resolution + 1), (_, k) => k * h + this.t0); //time series datapoints
const ys: number[][] = Array.from(Array(resolution + 1), () =>
Array(this.y0.length).fill(0),
);
ys[0] = this.y0;

for (let i = 0; i < resolution; i++) {
console.log(i);
ys[i + 1] = this.ode(ts[i], ys[i]).changeRates.map(
(x, j) => ys[i][j] + x * h,
); //y_n+1 = y_n + dy/dx *h
}

return {
ts: ts,
ys: ys,
};
}

rk4(resolution: number) {
const h = (this.t1 - this.t0) / resolution;
const ts = Array.from(Array(resolution + 1), (_, k) => k * h + this.t0); //time series datapoints
const ys = Array.from(Array(resolution + 1), () =>
Array(this.y0.length).fill(0),
);
ys[0] = this.y0;

if (this.y0.includes(NaN))
console.warn("y0 contains invalid starting value", this.y0);

let stoppingIndex = 0;
for (let i = 0; i < resolution; i++) {
stoppingIndex = i;
let k = this.ode(ts[i], ys[i]); // f(t, y_n)
const k1 = k.changeRates;

if (k.shouldStop) {
break;
}

const s1 = ys[i].map((y, j) => y + (k1[j] * h) / 2);
k = this.ode(ts[i] + h / 2, s1); // f(t + h/2, y_n + k1*h/2)
const k2 = k.changeRates;

if (k.shouldStop) {
break;
}

const s2 = ys[i].map((y, j) => y + (k2[j] * h) / 2);
k = this.ode(ts[i] + h / 2, s2); // f(t + h/2, y_n + k2*h/2)
const k3 = k.changeRates;

if (k.shouldStop) {
break;
}

const s3 = ys[i].map((y, j) => y + k3[j] * h);
k = this.ode(ts[i] + h, s3); // f(t + h, y_n + k3*h)
const k4 = k.changeRates;

if (k.shouldStop) {
break;
}

ys[i + 1] = ys[i].map(
(x, j) => x + (k1[j] / 6 + k2[j] / 3 + k3[j] / 3 + k4[j] / 6) * h,
); //y_n+1 = y_n + (k1 +2*k2 + 2*k3 +k4)/6 *h
}

return {
ts: ts.slice(0, stoppingIndex),
ys: ys.slice(0, stoppingIndex),
};
}

midpoint(resolution: number) {
const h = (this.t1 - this.t0) / resolution;
const ts = Array.from(Array(resolution + 1), (_, k) => k * h + this.t0); //time series datapoints
const ys = Array.from(Array(resolution + 1), () =>
Array(this.y0.length).fill(0),
);
ys[0] = this.y0;

for (let i = 0; i < resolution; i++) {
const k1 = this.ode(ts[i], ys[i]).changeRates; // f(t, y_n)

const s1 = ys[i].map((y, j) => y + (k1[j] * h) / 2); // y_n + k1 * h/2
const k2 = this.ode(ts[i] + h / 2, s1).changeRates; // f(t + h/2, y_n + k1*h/2)
ys[i + 1] = ys[i].map((x, j) => x + k2[j] * h); //y_n+1 = y_n + k2 *h
}
return {
ts: ts,
ys: ys,
};
}
}
Loading

1 comment on commit 9692a5c

@tervay
Copy link
Owner Author

@tervay tervay commented on 9692a5c Dec 17, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Deploy preview for recalc ready!

✅ Preview
https://recalc-cuiqmv4op-tervay.vercel.app

Built with commit 9692a5c.
This pull request is being automatically deployed with vercel-action

Please sign in to comment.