diff --git a/cypress/e2e/flywheel.cy.ts b/cypress/e2e/flywheel.cy.ts index 96d317070..27777b4d1 100644 --- a/cypress/e2e/flywheel.cy.ts +++ b/cypress/e2e/flywheel.cy.ts @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, diff --git a/package.json b/package.json index 68c5ac95c..613c350a4 100644 --- a/package.json +++ b/package.json @@ -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", diff --git a/src/common/models/Measurement.ts b/src/common/models/Measurement.ts index c71e5233f..eefd00996 100644 --- a/src/common/models/Measurement.ts +++ b/src/common/models/Measurement.ts @@ -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"]; @@ -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"), + ); + } } diff --git a/src/common/models/Motor.ts b/src/common/models/Motor.ts index 0c99ec21b..772b861ca 100644 --- a/src/common/models/Motor.ts +++ b/src/common/models/Motor.ts @@ -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 = { @@ -129,3 +130,80 @@ export type IncompleteMotorState = { }; export type CompleteMotorState = Required; + +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); +} diff --git a/src/common/models/tests/Measurement.test.ts b/src/common/models/tests/Measurement.test.ts index 52fde5b09..4d53ca1b2 100644 --- a/src/common/models/tests/Measurement.test.ts +++ b/src/common/models/tests/Measurement.test.ts @@ -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")); + }); }); diff --git a/src/common/tooling/ODE.ts b/src/common/tooling/ODE.ts new file mode 100644 index 000000000..0039602c1 --- /dev/null +++ b/src/common/tooling/ODE.ts @@ -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, + }; + } +} diff --git a/src/web/calculators/arm/armMath.ts b/src/web/calculators/arm/armMath.ts index 91dc2e81e..66c71917e 100644 --- a/src/web/calculators/arm/armMath.ts +++ b/src/web/calculators/arm/armMath.ts @@ -7,6 +7,7 @@ import Motor, { import Ratio, { RatioDict } from "common/models/Ratio"; import { MotorRules } from "common/models/Rules"; import { expose } from "common/tooling/promise-worker"; +import { Derivative, Solver } from "odex"; function calculateArmTorque( comLength: Measurement, @@ -45,6 +46,131 @@ export function calculateMaximumStringArmMountingDistance( return springLength.div(denom); } +export function deriv( + kcos: Measurement, + komega: Measurement, + k: Measurement, + max_motor_brake: Measurement, + k_min_motor_brake: Measurement, + direction: number, +): Derivative { + return function (t, y) { + const phi_d = new Measurement(y[1], "rad/s"); + + let motor_brake_decel = komega.mul(phi_d).add(k.mul(direction)); + + if (motor_brake_decel.abs().gt(max_motor_brake)) { + motor_brake_decel = max_motor_brake.mul(direction).negate(); + } + + if (motor_brake_decel.abs().lt(k_min_motor_brake.mul(phi_d))) { + motor_brake_decel = k_min_motor_brake.mul(phi_d).mul(direction).negate(); + } + + if (motor_brake_decel.abs().gt(max_motor_brake)) { + motor_brake_decel = max_motor_brake.mul(direction).negate(); + } + + const phi_dd = kcos + .mul(Math.cos(y[0])) + .add(motor_brake_decel) + .mul(new Measurement(1, "rad")); + + return [phi_d.to("rad/s").scalar, phi_dd.to("rad/s2").scalar]; + }; +} + +export function getDecelerationTime( + start_angle: Measurement, + start_velocity: Measurement, + ratio: Ratio, + comLength: Measurement, + motor: Motor, + mass: Measurement, + currentLimit: Measurement, + inertia: Measurement, +) { + const k_cos = comLength + .mul(mass) + .mul(Measurement.GRAVITY.negate()) + .div(inertia) + .negate(); + + const k_omega = motor.kT + .mul(ratio.asNumber()) + .mul(ratio.asNumber()) + .div(motor.resistance.mul(motor.kV).mul(inertia)); + + const k = motor.kT + .mul(ratio.asNumber()) + .negate() + .mul(motor.stallCurrent.add(motor.freeCurrent)) + .div(inertia); + + const max_motor_brake_decel = currentLimit + .mul(ratio.asNumber()) + .mul(motor.kT) + .div(inertia); + + const min_motor_brake = motor.kT + .negate() + .mul(ratio.asNumber()) + .mul(ratio.asNumber()) + .div(motor.resistance.mul(motor.kV).mul(inertia)); + + const solver = new Solver( + deriv( + k_cos, + k_omega, + k, + max_motor_brake_decel, + min_motor_brake, + start_velocity.sign(), + ), + 2, + ); + + let sentinel = false; + const slowdownStates: MomentaryArmState[] = []; + + solver.solve( + 0, + [start_angle.to("rad").scalar, start_velocity.to("rad/s").scalar], + 0.15, + solver.grid(0.002, (t, y) => { + if (!sentinel) { + const ms = new MotorRules(motor, currentLimit, { + voltage: nominalVoltage, + rpm: new Measurement(y[1], "rad/s").mul(ratio.asNumber()), + }).solve(); + slowdownStates.push({ + position: new Measurement(y[0], "rad").toDict(), + time: new Measurement(t, "s").toDict(), + motorState: { + current: ms.current.toDict(), + power: ms.power.toDict(), + rpm: ms.rpm.to("rpm").toDict(), + torque: ms.torque.toDict(), + voltage: ms.voltage.toDict(), + }, + }); + } + + if ( + (start_velocity.sign() > 0 && y[1] < 0) || + (start_velocity.sign() < 0 && y[1] > 0) + ) { + if (!sentinel) { + sentinel = true; + } + + return false; + } + }), + ); + console.log(slowdownStates); +} + export type MomentaryArmState = { time: MeasurementDict; position: MeasurementDict; diff --git a/src/web/calculators/flywheel/components/FlywheelCalculator.tsx b/src/web/calculators/flywheel/components/FlywheelCalculator.tsx index 22e624951..149b31499 100644 --- a/src/web/calculators/flywheel/components/FlywheelCalculator.tsx +++ b/src/web/calculators/flywheel/components/FlywheelCalculator.tsx @@ -377,7 +377,7 @@ export default function FlywheelCalculator(): JSX.Element { set.setShooterMomentOfInertia, ]} numberDisabledIf={() => !get.useCustomShooterMoi} - numberRoundTo={1} + numberRoundTo={3} /> @@ -437,7 +437,7 @@ export default function FlywheelCalculator(): JSX.Element { set.setFlywheelMomentOfInertia, ]} numberDisabledIf={() => !get.useCustomFlywheelMoi} - numberRoundTo={1} + numberRoundTo={3} /> diff --git a/src/web/calculators/linear/components/LinearCalculator.tsx b/src/web/calculators/linear/components/LinearCalculator.tsx index 464e0a9ed..7dc63382b 100644 --- a/src/web/calculators/linear/components/LinearCalculator.tsx +++ b/src/web/calculators/linear/components/LinearCalculator.tsx @@ -1,23 +1,20 @@ import Graph from "common/components/graphing/Graph"; -import { - GraphConfig, - GraphDataPoint, -} from "common/components/graphing/graphConfig"; +import { GraphConfig } from "common/components/graphing/graphConfig"; import SimpleHeading from "common/components/heading/SimpleHeading"; import SingleInputLine from "common/components/io/inputs/SingleInputLine"; import { - BooleanInput, MeasurementInput, MotorInput, NumberInput, RatioInput, } from "common/components/io/new/inputs"; import MeasurementOutput from "common/components/io/outputs/MeasurementOutput"; -import { Column, Columns } from "common/components/styling/Building"; +import { Column, Columns, Message } from "common/components/styling/Building"; import { useAsyncMemo } from "common/hooks/useAsyncMemo"; import Measurement from "common/models/Measurement"; import { useGettersSetters } from "common/tooling/conversion"; import { wrap } from "common/tooling/promise-worker"; +import { maxBy } from "lodash"; import { useMemo } from "react"; import { LinearParamsV1, @@ -27,7 +24,6 @@ import { import { LinearState } from "web/calculators/linear/converter"; import { LinearWorkerFunctions, - calculateProfiledTimeToGoal, calculateStallLoad, } from "web/calculators/linear/linearMath"; import rawWorker from "web/calculators/linear/linearMath?worker"; @@ -43,70 +39,45 @@ const worker = await wrap(new rawWorker()); export default function LinearCalculator(): JSX.Element { const [get, set] = useGettersSetters(LinearState.getState() as LinearStateV1); - const profiledTimeToGoal = useMemo( + const moi = useMemo( () => - calculateProfiledTimeToGoal( - get.motor, - get.currentLimit, - get.ratio, - get.spoolDiameter, - get.load, - get.travelDistance, - get.angle, - get.efficiency, - get.limitAcceleration ? get.limitedAcceleration : undefined, - get.limitDeceleration ? get.limitedDeceleration : undefined, - get.limitVelocity ? get.limitedVelocity : undefined, - ), - [ - get.motor, - get.currentLimit, - get.ratio, - get.spoolDiameter, - get.load, - get.travelDistance, - get.efficiency, - get.angle, - get.limitAcceleration, - get.limitedAcceleration, - get.limitDeceleration, - get.limitedDeceleration, - get.limitVelocity, - get.limitedVelocity, - ], + get.ratio.asNumber() === 0 + ? new Measurement(0, "kg m2") + : get.load + .mul(get.spoolDiameter.div(2)) + .mul(get.spoolDiameter.div(2)) + .div(get.ratio.asNumber()) + .div(get.ratio.asNumber()), + [get.load, get.angle, get.ratio, get.spoolDiameter], ); - const chartData = useAsyncMemo( - { position: [] as GraphDataPoint[], velocity: [] as GraphDataPoint[] }, + const odeChartData = useAsyncMemo( + { + position: [], + velocity: [], + currentDraw: [], + }, () => - worker.generateTimeToGoalChartData( + worker.generateODEData( get.motor.toDict(), get.currentLimit.toDict(), + get.travelDistance.toDict(), get.ratio.toDict(), get.spoolDiameter.toDict(), get.load.toDict(), - get.travelDistance.toDict(), - get.angle.toDict(), + moi.toDict(), get.efficiency, - get.limitAcceleration ? get.limitedAcceleration.toDict() : undefined, - get.limitDeceleration ? get.limitedDeceleration.toDict() : undefined, - get.limitVelocity ? get.limitedVelocity.toDict() : undefined, + get.angle.toDict(), ), [ get.motor, get.currentLimit, + get.travelDistance, get.ratio, get.spoolDiameter, - get.load, - get.travelDistance, - get.angle, + moi, get.efficiency, - get.limitAcceleration, - get.limitedAcceleration, - get.limitDeceleration, - get.limitedDeceleration, - get.limitVelocity, - get.limitedVelocity, + get.angle, ], ); @@ -159,6 +130,23 @@ export default function LinearCalculator(): JSX.Element { ], ); + const maxVelocity = useMemo( + () => + new Measurement(maxBy(odeChartData.velocity, (v) => v.y)?.y ?? 0, "in/s"), + [odeChartData], + ); + + const timeToGoal = useMemo( + () => + new Measurement( + odeChartData.position.length === 0 + ? 0 + : odeChartData.position[odeChartData.position.length - 1].x, + "s", + ), + [odeChartData], + ); + const stallLoad = useMemo( () => calculateStallLoad( @@ -186,21 +174,30 @@ export default function LinearCalculator(): JSX.Element { id="motor" tooltip="The motors powering the system." > - + - + - + - + get.currentLimit .mul(get.currentLimit) - .mul(profiledTimeToGoal.smartTimeToGoal) + .mul(timeToGoal) .gte(Measurement.STANDARD_BREAKER_ESTIMATE_I2T()) } + numberDelay={500} /> - + - - - - - - - - - !get.limitVelocity} - /> - - - - - - - - - - - - - - - - - - {(get.limitAcceleration || get.limitDeceleration) && ( - - - {get.limitAcceleration && ( - - !get.limitAcceleration} - defaultUnit="in/s2" - /> - - )} - - - - {get.limitDeceleration && ( - - !get.limitDeceleration} - defaultUnit="in/s2" - /> - - )} - - - )} - - - - - undefined, - ]} - numberRoundTo={2} - defaultUnit="s" - /> - - - - - undefined, - ]} - numberRoundTo={2} - defaultUnit="s" - /> - - - - - - - undefined]} - numberRoundTo={2} - defaultUnit="in/s2" - /> - - - - - undefined]} - numberRoundTo={2} - defaultUnit="in/s2" - /> - - - - - - - undefined, - ]} - numberRoundTo={2} - defaultUnit="in" - /> - - - - - undefined, - ]} - numberRoundTo={2} - defaultUnit="in" - /> - - - undefined]} + stateHook={[timeToGoal, () => undefined]} numberRoundTo={2} /> @@ -465,100 +272,63 @@ export default function LinearCalculator(): JSX.Element { tooltip="The highest velocity the system reaches during the motion profile." > undefined]} + stateHook={[maxVelocity, () => undefined]} numberRoundTo={2} defaultUnit="in/s" /> - - - - - undefined, - ]} - numberRoundTo={2} - defaultUnit="s" - /> - - - - - undefined, - ]} - numberRoundTo={2} - defaultUnit="in" - /> - - - - - - undefined, - ]} - numberRoundTo={2} - defaultUnit="in/s2" - /> - - undefined]} numberRoundTo={2} - defaultUnit="lbs" + defaultUnit="lb" /> + {/* + undefined]} + numberRoundTo={4} + defaultUnit="kg m^2" + /> + */} + + There is a small delay in output updates. The longer it takes to + reach the goal, the slower the graph & outputs will be to update. + This does not model deceleration. + s.y > 0), + odeChartData.position, 0, "y-position", ), GraphConfig.dataset( "Velocity (in/s)", - chartData.velocity, + odeChartData.velocity, 1, "y-velocity", ), + GraphConfig.dataset( + "Current Draw (A)", + odeChartData.currentDraw, + 2, + "y-current", + ), ]} title="Motion Profile over Time" id="linearGraph" diff --git a/src/web/calculators/linear/index.ts b/src/web/calculators/linear/index.ts index 6c4b77807..a6fd016bd 100644 --- a/src/web/calculators/linear/index.ts +++ b/src/web/calculators/linear/index.ts @@ -64,6 +64,16 @@ export const linearGraphConfig = GraphConfig.options( }, min: 0, }, + "y-current": { + type: "linear", + position: "right", + beginAtZero: true, + title: { + display: true, + text: "Current Draw (A)", + }, + min: 0, + }, x: { type: "linear", beginAtZero: true, diff --git a/src/web/calculators/linear/linearMath.ts b/src/web/calculators/linear/linearMath.ts index 8a3a529b8..449c3a3c2 100644 --- a/src/web/calculators/linear/linearMath.ts +++ b/src/web/calculators/linear/linearMath.ts @@ -1,470 +1,120 @@ import { GraphDataPoint } from "common/components/graphing/graphConfig"; import Measurement, { MeasurementDict } from "common/models/Measurement"; -import Motor, { MotorDict, nominalVoltage } from "common/models/Motor"; +import Motor, { + MotorDict, + nominalVoltage, + solveMotorODE, +} from "common/models/Motor"; import Ratio, { RatioDict } from "common/models/Ratio"; import { MotorRules } from "common/models/Rules"; import { expose } from "common/tooling/promise-worker"; -interface MotionProfile { - accelerationPhaseDuration: Measurement; - constantVelocityPhaseDuration: Measurement; - decelerationPhaseDuration: Measurement; - maxVelocity: Measurement; - acceleration: Measurement; - deceleration: Measurement; - systemAcceleration: Measurement; - accelDistance: Measurement; - decelDistance: Measurement; - cruiseDistance: Measurement; -} - -interface TrapezoidalProfileParams { - distance: Measurement; - motorTorque: Measurement; - maxVelocity: Measurement; - systemMass: Measurement; - spoolDiameter: Measurement; - angle: Measurement; - - limitedAcceleration?: Measurement; - limitedDeceleration?: Measurement; - limitedVelocity?: Measurement; -} - -export function calculateStallLoad( - motor: Motor, - currentLimit: Measurement, - spoolDiameter: Measurement, - ratio: Ratio, - efficiency: number, -): Measurement { - if ([spoolDiameter.scalar].includes(0)) { - return new Measurement(0, "lb"); - } - - return new MotorRules(motor, currentLimit, { - current: currentLimit, - voltage: nominalVoltage, - }) - .solve() - .torque.mul(motor.quantity) - .mul(ratio.asNumber()) - .mul(efficiency / 100) - .div(spoolDiameter.div(2)) - .div(Measurement.GRAVITY); -} - -function canDecelerateInGivenDistance( - currentSpeed: Measurement, - distance: Measurement, - deceleration: Measurement, -): boolean { - const finalSpeed = new Measurement(0, "m/s"); - const stoppingDistance = finalSpeed - .mul(finalSpeed) - .sub(currentSpeed.mul(currentSpeed)) - .div(deceleration.mul(2)); - - return stoppingDistance.lte(distance); -} - -export function planTrapezoidalMotionProfile( - params: TrapezoidalProfileParams, -): MotionProfile { - const { - spoolDiameter, - systemMass, - angle, - distance, - maxVelocity, - motorTorque, - limitedAcceleration, - limitedDeceleration, - limitedVelocity, - } = params; - - if (Measurement.anyAreZero(spoolDiameter, systemMass)) { - return { - accelerationPhaseDuration: new Measurement(0, "s"), - constantVelocityPhaseDuration: new Measurement(0, "s"), - decelerationPhaseDuration: new Measurement(0, "s"), - maxVelocity: new Measurement(0, "m/s"), - acceleration: new Measurement(0, "m/s2"), - deceleration: new Measurement(0, "m/s2"), - systemAcceleration: new Measurement(0, "m/s2"), - accelDistance: new Measurement(0, "m"), - decelDistance: new Measurement(0, "m"), - cruiseDistance: new Measurement(0, "m"), - }; - } - - const observedMotorForce = motorTorque.div(spoolDiameter.div(2)); - const gravityForce = Measurement.GRAVITY.mul(systemMass).mul( - Math.sin(angle.to("rad").scalar), - ); - - let carriageMaxVelocity = maxVelocity - .mul(spoolDiameter.mul(Math.PI)) - .div(new Measurement(360, "deg")); - if (limitedVelocity !== undefined) { - carriageMaxVelocity = Measurement.min(limitedVelocity, carriageMaxVelocity); - } - - let observedMotorAcceleration = observedMotorForce.div(systemMass); - if (limitedAcceleration !== undefined) { - observedMotorAcceleration = Measurement.min( - observedMotorAcceleration, - limitedAcceleration, - ); - } - - const observedGravityAcceleration = gravityForce.div(systemMass); - - const effectiveAcceleration = observedMotorAcceleration.add( - observedGravityAcceleration, - ); - - let observedMotorDeceleration = observedMotorForce.div(systemMass); - if (limitedDeceleration !== undefined) { - observedMotorDeceleration = Measurement.min( - observedMotorDeceleration, - limitedDeceleration, - ); - } - - const effectiveDeceleration = observedMotorDeceleration - .negate() - .add(observedGravityAcceleration) - .negate(); - - const timeToMaxVelocity = carriageMaxVelocity.div(effectiveAcceleration); - - let distanceDuringAcceleration = effectiveAcceleration - .mul(0.5) - .mul(timeToMaxVelocity) - .mul(timeToMaxVelocity); - - if ( - !canDecelerateInGivenDistance( - effectiveAcceleration.mul(timeToMaxVelocity), - distance.sub(distanceDuringAcceleration), - effectiveDeceleration, - ) - ) { - const t_1_numerator = new Measurement( - Math.sqrt( - distance.mul(effectiveDeceleration.abs()).mul(2).to("m2/s2").scalar, - ), - "m/s", - ); - const t1_denominator = new Measurement( - Math.sqrt( - effectiveAcceleration - .mul(effectiveAcceleration.add(effectiveDeceleration.abs())) - .to("m2/s4").scalar, - ), - "m/s2", - ); - - const t_1 = t_1_numerator.div(t1_denominator); - const maxVelo = t_1.mul(effectiveAcceleration); - const t_2 = maxVelo.div(effectiveDeceleration); - - return { - acceleration: effectiveAcceleration, - accelerationPhaseDuration: t_1, - constantVelocityPhaseDuration: new Measurement(0, "s"), - cruiseDistance: new Measurement(0, "m"), - deceleration: effectiveDeceleration, - decelerationPhaseDuration: t_2, - maxVelocity: maxVelo, - systemAcceleration: observedGravityAcceleration, - accelDistance: effectiveAcceleration.mul(t_1).mul(t_1).div(2), - decelDistance: effectiveDeceleration.mul(t_2).mul(t_2).div(2), - }; - } - - distanceDuringAcceleration = effectiveAcceleration - .mul(0.5) - .mul(timeToMaxVelocity) - .mul(timeToMaxVelocity); - - const timeToSlowFromMaxVelocity = carriageMaxVelocity.div( - effectiveDeceleration, - ); - - const distanceDuringDeceleration = effectiveDeceleration - .mul(0.5) - .mul(timeToSlowFromMaxVelocity) - .mul(timeToSlowFromMaxVelocity); - - if ( - distanceDuringAcceleration.add(distanceDuringDeceleration).gte(distance) - ) { - const accelTimeSq = distanceDuringAcceleration - .mul(2) - .div(effectiveAcceleration); - const accelTime = new Measurement( - Math.sqrt(accelTimeSq.to("s2").scalar), - "s", - ); - - const decelTimeSq = distanceDuringDeceleration - .mul(2) - .div(effectiveDeceleration); - const decelTime = new Measurement( - Math.sqrt(decelTimeSq.to("s2").scalar), - "s", - ); - - return { - accelerationPhaseDuration: accelTime, - constantVelocityPhaseDuration: new Measurement(0, "s"), - decelerationPhaseDuration: decelTime, - maxVelocity: accelTime.mul(effectiveAcceleration), - acceleration: effectiveAcceleration, - deceleration: effectiveDeceleration, - systemAcceleration: observedGravityAcceleration, - accelDistance: distanceDuringAcceleration, - decelDistance: distanceDuringDeceleration, - cruiseDistance: new Measurement(0, "m"), - }; - } - - // Calculate the distance covered during the constant velocity phase - const distanceDuringConstantVelocity = distance - .sub(distanceDuringAcceleration) - .sub(distanceDuringDeceleration); - - // Calculate the time for the constant velocity phase - const timeForConstantVelocity = - distanceDuringConstantVelocity.div(carriageMaxVelocity); - - // Calculate the durations of each phase - const accelerationPhaseDuration = timeToMaxVelocity; - const constantVelocityPhaseDuration = timeForConstantVelocity; - const decelerationPhaseDuration = timeToSlowFromMaxVelocity; - - return { - accelerationPhaseDuration: accelerationPhaseDuration.to("s"), - constantVelocityPhaseDuration: constantVelocityPhaseDuration.to("s"), - decelerationPhaseDuration: decelerationPhaseDuration.to("s"), - maxVelocity: carriageMaxVelocity, - acceleration: effectiveAcceleration, - deceleration: effectiveDeceleration, - systemAcceleration: observedGravityAcceleration, - accelDistance: distanceDuringAcceleration, - decelDistance: distanceDuringDeceleration, - cruiseDistance: distanceDuringConstantVelocity, - }; -} - -export function calculateProfiledTimeToGoal( - motor: Motor, - currentLimit: Measurement, - ratio: Ratio, - spoolDiameter: Measurement, - load: Measurement, - travelDistance: Measurement, - angle: Measurement, - efficiency: number, - limitedAcceleration?: Measurement, - limitedDeceleration?: Measurement, - limitedVelocity?: Measurement, -): MotionProfile & { - smartTimeToGoal: Measurement; - maxVelocity: Measurement; -} { - if (ratio.asNumber() == 0) { - return { - accelerationPhaseDuration: new Measurement(0, "s"), - constantVelocityPhaseDuration: new Measurement(0, "s"), - decelerationPhaseDuration: new Measurement(0, "s"), - maxVelocity: new Measurement(0, "m/s"), - acceleration: new Measurement(0, "m/s2"), - deceleration: new Measurement(0, "m/s2"), - smartTimeToGoal: new Measurement(0, "s"), - systemAcceleration: new Measurement(0, "m/s2"), - accelDistance: new Measurement(0, "m"), - decelDistance: new Measurement(0, "m"), - cruiseDistance: new Measurement(0, "m"), - }; - } - - const profile = planTrapezoidalMotionProfile({ - distance: travelDistance, - motorTorque: motor.kT - .mul(Measurement.min(currentLimit, motor.stallCurrent)) - .mul(motor.quantity) - .mul(ratio.asNumber()) - .mul(efficiency / 100), - maxVelocity: motor.freeSpeed.div(ratio.asNumber()), - systemMass: load, - spoolDiameter, - angle, - limitedAcceleration, - limitedDeceleration, - limitedVelocity, - }); - - let smartTimeToGoal; - if ( - profile.accelerationPhaseDuration.lte(new Measurement(0, "s")) || - profile.decelerationPhaseDuration.lte(new Measurement(0, "s")) - ) { - smartTimeToGoal = new Measurement(0, "s"); - profile.accelerationPhaseDuration = new Measurement(0, "s"); - profile.decelerationPhaseDuration = new Measurement(0, "s"); - profile.constantVelocityPhaseDuration = new Measurement(0, "s"); - } else if ( - profile.constantVelocityPhaseDuration.lte(new Measurement(0, "s")) - ) { - profile.constantVelocityPhaseDuration = new Measurement(0, "s"); - smartTimeToGoal = profile.accelerationPhaseDuration.add( - profile.decelerationPhaseDuration, - ); - } else { - smartTimeToGoal = profile.accelerationPhaseDuration - .add(profile.constantVelocityPhaseDuration) - .add(profile.decelerationPhaseDuration); - } - - return { ...profile, smartTimeToGoal }; -} - -export function generateTimeToGoalChartData( +function generateODEData( motor_: MotorDict, currentLimit_: MeasurementDict, + travelDistance_: MeasurementDict, ratio_: RatioDict, spoolDiameter_: MeasurementDict, load_: MeasurementDict, - travelDistance_: MeasurementDict, - angle_: MeasurementDict, + J_: MeasurementDict, efficiency: number, - limitedAcceleration_?: MeasurementDict, - limitedDeceleration_?: MeasurementDict, - limitedVelocity_?: MeasurementDict, + angle_: MeasurementDict, ): { position: GraphDataPoint[]; velocity: GraphDataPoint[]; + currentDraw: GraphDataPoint[]; } { const motor = Motor.fromDict(motor_); const currentLimit = Measurement.fromDict(currentLimit_); - const load = Measurement.fromDict(load_); const travelDistance = Measurement.fromDict(travelDistance_); const spoolDiameter = Measurement.fromDict(spoolDiameter_); - const angle = Measurement.fromDict(angle_); const ratio = Ratio.fromDict(ratio_); - const limitedAcceleration = - limitedAcceleration_ === undefined - ? undefined - : Measurement.fromDict(limitedAcceleration_); - const limitedDeceleration = - limitedDeceleration_ === undefined - ? undefined - : Measurement.fromDict(limitedDeceleration_); - const limitedVelocity = - limitedVelocity_ === undefined - ? undefined - : Measurement.fromDict(limitedVelocity_); - - const profile = calculateProfiledTimeToGoal( - motor, - currentLimit, - ratio, - spoolDiameter, - load, - travelDistance, - angle, - efficiency, - limitedAcceleration, - limitedDeceleration, - limitedVelocity, - ); - - const timestep = new Measurement(0.005, "s"); - let t = new Measurement(0, "s"); - let velocity = new Measurement(0, "m/s"); - let position = new Measurement(0, "m"); - - const timestamps: Measurement[] = [t]; - const positions: Measurement[] = [position]; - const velocities: Measurement[] = [velocity]; - - while (t.lte(profile.accelerationPhaseDuration)) { - t = t.add(timestep); - position = position.add( - velocity.mul(timestep).add( - profile.acceleration - .mul(timestep) - .mul(timestep) - .mul(1 / 2), - ), - ); - velocity = Measurement.min( - velocity.add(profile.acceleration.mul(timestep)), - profile.maxVelocity, - ); - - positions.push(position); - velocities.push(velocity); - timestamps.push(t); - } + const load = Measurement.fromDict(load_); + const J = Measurement.fromDict(J_); + const angle = Measurement.fromDict(angle_); - while ( - t.lte( - profile.accelerationPhaseDuration.add( - profile.constantVelocityPhaseDuration, - ), - ) + if ( + [ + motor.quantity, + ratio.magnitude, + spoolDiameter.baseScalar, + currentLimit.baseScalar, + ].includes(0) ) { - t = t.add(timestep); - position = position.add(velocity.mul(timestep)); - velocity = velocity; - - positions.push(position); - velocities.push(velocity); - timestamps.push(t); + return { position: [], velocity: [], currentDraw: [] }; } - while ( - t.lte( - profile.accelerationPhaseDuration - .add(profile.constantVelocityPhaseDuration) - .add(profile.decelerationPhaseDuration), - ) - ) { - t = t.add(timestep); - position = position.add( - velocity.mul(timestep).add( - profile.deceleration - .negate() - .mul(timestep) - .mul(timestep) - .mul(1 / 2), - ), - ); - velocity = velocity.add(profile.deceleration.negate().mul(timestep)); + const gravitationalForce = load.mul(Measurement.GRAVITY.negate()); + const gravitationalTorque = gravitationalForce + .mul(spoolDiameter.div(2)) + .div(ratio.asNumber()) + .mul(Math.sin(angle.to("rad").scalar)); - positions.push(position); - velocities.push(velocity); - timestamps.push(t); - } + const data = solveMotorODE( + motor, + currentLimit, + (info) => + info.position + .linearizeRadialPosition( + spoolDiameter.mul(Math.PI).div(ratio.asNumber()), + ) + .gte(travelDistance) || + (info.velocity.lte(new Measurement(2, "rad/s")) && + info.stepNumber >= 1000), + J, + gravitationalTorque, + efficiency, + ); return { - position: positions.map((p, i) => ({ - y: p.to("in").scalar, - x: timestamps[i].to("s").scalar, + position: data.ys.map((y, i) => ({ + x: data.ts[i], + y: new Measurement(y[3], "rad") + .linearizeRadialPosition( + spoolDiameter.mul(Math.PI).div(ratio.asNumber()), + ) + .to("in").scalar, + })), + velocity: data.ys.map((y, i) => ({ + x: data.ts[i], + y: new Measurement(y[0], "rad/s") + .linearizeRadialPosition( + spoolDiameter.mul(Math.PI).div(ratio.asNumber()), + ) + .to("in/s").scalar, })), - velocity: velocities.map((p, i) => ({ - y: p.to("in/s").scalar, - x: timestamps[i].to("s").scalar, + currentDraw: data.ys.map((y, i) => ({ + x: data.ts[i], + y: y[2], })), }; } +export function calculateStallLoad( + motor: Motor, + currentLimit: Measurement, + spoolDiameter: Measurement, + ratio: Ratio, + efficiency: number, +): Measurement { + if ([spoolDiameter.scalar].includes(0)) { + return new Measurement(0, "lb"); + } + + return new MotorRules(motor, currentLimit, { + current: currentLimit, + voltage: nominalVoltage, + }) + .solve() + .torque.mul(motor.quantity) + .mul(ratio.asNumber()) + .mul(efficiency / 100) + .div(spoolDiameter.div(2)) + .div(Measurement.GRAVITY); +} + const workerFunctions = { - generateTimeToGoalChartData, + generateODEData, }; expose(workerFunctions); diff --git a/src/web/calculators/linear/tests/linearMath.test.ts b/src/web/calculators/linear/tests/linearMath.test.ts index 4c8604faf..758f56466 100644 --- a/src/web/calculators/linear/tests/linearMath.test.ts +++ b/src/web/calculators/linear/tests/linearMath.test.ts @@ -1,102 +1,5 @@ -import { - Nm, - deg, - kg, - m, - m_s, - m_s2, - mm, - rpm, - s, -} from "common/models/ExtraTypes"; -import { describe, expect, test } from "vitest"; -import { planTrapezoidalMotionProfile } from "web/calculators/linear/linearMath"; +import { describe, test } from "vitest"; describe("linearMath", () => { - test.each([ - { - angle: deg(90), - distance: m(1), - maxVelocity: rpm(3000), - motorTorque: Nm(3), - spoolDiameter: mm(25), - systemMass: kg(5), - expected: { - acceleration: m_s2(38.19), - accelerationPhaseDuration: s(0.1028), - constantVelocityPhaseDuration: s(0.16927), - deceleration: m_s2(57.81), - decelerationPhaseDuration: s(0.068), - maxVelocity: m_s(3.927), - }, - }, - { - angle: deg(45), - distance: m(1), - maxVelocity: rpm(3000), - motorTorque: Nm(3), - spoolDiameter: mm(25), - systemMass: kg(5), - expected: { - acceleration: m_s2(41.0633), - accelerationPhaseDuration: s(0.0956), - constantVelocityPhaseDuration: s(0.1711), - deceleration: m_s2(54.9367), - decelerationPhaseDuration: s(0.0715), - maxVelocity: m_s(3.927), - }, - }, - { - angle: deg(90), - distance: m(0.1), - maxVelocity: rpm(5000), - motorTorque: Nm(5), - spoolDiameter: mm(25), - systemMass: kg(1), - expected: { - acceleration: m_s2(390.19), - accelerationPhaseDuration: s(0.016), - constantVelocityPhaseDuration: s(0), - deceleration: m_s2(409.81), - decelerationPhaseDuration: s(0.016), - maxVelocity: m_s(6.545), - }, - }, - ])( - "%p planTrapezoidalMotionProfile", - ({ - angle, - distance, - maxVelocity, - motorTorque, - spoolDiameter, - systemMass, - expected, - }) => { - const profile = planTrapezoidalMotionProfile({ - angle, - distance, - maxVelocity, - motorTorque, - spoolDiameter, - systemMass, - }); - expect(profile.acceleration).toBeCloseToMeasurement( - expected.acceleration, - ); - expect(profile.accelerationPhaseDuration).toBeCloseToMeasurement( - expected.accelerationPhaseDuration, - ); - expect(profile.constantVelocityPhaseDuration).toBeCloseToMeasurement( - expected.constantVelocityPhaseDuration, - ); - expect(profile.deceleration).toBeCloseToMeasurement( - expected.deceleration, - ); - expect(profile.decelerationPhaseDuration).toBeCloseToMeasurement( - expected.decelerationPhaseDuration, - ); - expect(profile.maxVelocity).toBeCloseToMeasurement(expected.maxVelocity); - }, - ); + test("", () => {}); }); diff --git a/yarn.lock b/yarn.lock index 2bcb4f7e1..763b4dc24 100644 --- a/yarn.lock +++ b/yarn.lock @@ -4548,6 +4548,11 @@ object.values@^1.1.6: define-properties "^1.2.0" es-abstract "^1.22.1" +odex@3.0.0-rc.4: + version "3.0.0-rc.4" + resolved "https://registry.yarnpkg.com/odex/-/odex-3.0.0-rc.4.tgz#083c201e3a5e6504bc71a8d9bd023002f5d78ca7" + integrity sha512-NPpR3eFHUJWQWYT9VParetnYF2bVe1p9iel1PHk2PLGgOdfXd7tr67XJi6x6fVzDmoKnUv1e9AtRtdwLxg2Z2Q== + once@^1.3.0, once@^1.3.1, once@^1.4.0: version "1.4.0" resolved "https://registry.yarnpkg.com/once/-/once-1.4.0.tgz#583b1aa775961d4b113ac17d9c50baef9dd76bd1"