73 lines
3.9 KiB
TypeScript
73 lines
3.9 KiB
TypeScript
/** Executable profile proposal. Pure calculations; no hardware or transport. */
|
|
export interface ControlProfile {
|
|
schema: 'missioncore.rover-control/v1';
|
|
revision: number;
|
|
mode: 'tank' | 'arcade';
|
|
stick: 'left' | 'right';
|
|
deadband: number;
|
|
response: 'linear' | 'squared';
|
|
outputScale: number;
|
|
}
|
|
export interface Axes { leftY: number; rightY: number; leftX: number; rightX: number }
|
|
export interface Sides { left: number; right: number }
|
|
export interface MotorBinding { uuid: string; side: 'left' | 'right'; forwardSign: 1 | -1 }
|
|
|
|
export const defaultProfile: Readonly<ControlProfile> = Object.freeze({
|
|
schema: 'missioncore.rover-control/v1', revision: 0, mode: 'tank', stick: 'right',
|
|
deadband: 0.15, response: 'linear', outputScale: 1,
|
|
});
|
|
export function bounded(value: unknown, min: number, max: number): value is number {
|
|
return typeof value === 'number' && Number.isFinite(value) && value >= min && value <= max;
|
|
}
|
|
export function parseProfile(value: unknown): ControlProfile {
|
|
if (!value || typeof value !== 'object' || Array.isArray(value)) throw Error('Invalid profile');
|
|
const p = value as ControlProfile;
|
|
if (Object.keys(p).sort().join() !== Object.keys(defaultProfile).sort().join()
|
|
|| p.schema !== defaultProfile.schema || !Number.isSafeInteger(p.revision) || p.revision < 0
|
|
|| !['tank', 'arcade'].includes(p.mode) || !['left', 'right'].includes(p.stick)
|
|
|| !['linear', 'squared'].includes(p.response) || !bounded(p.deadband, 0, 0.5)
|
|
|| !bounded(p.outputScale, 0, 1)) throw Error('Invalid profile');
|
|
return {...p};
|
|
}
|
|
export function axisKeys(profile: ControlProfile): (keyof Axes)[] {
|
|
return profile.mode === 'tank' ? ['leftY', 'rightY']
|
|
: profile.stick === 'right' ? ['rightY', 'rightX'] : ['leftY', 'leftX'];
|
|
}
|
|
export function shapeAxis(value: number, profile: ControlProfile): number {
|
|
if (!bounded(value, -1, 1)) throw Error('Invalid axis');
|
|
const magnitude = Math.max(0, (Math.abs(value) - profile.deadband) / (1 - profile.deadband));
|
|
return Math.sign(value) * (profile.response === 'squared' ? magnitude * magnitude : magnitude);
|
|
}
|
|
/** +Y = forward, +X = turn right, including while reversing (yaw convention).
|
|
* Arcade diamond desaturation follows the documented WPILib ArcadeDriveIK
|
|
* convention, with clockwise steering sign adapted here. Output is normalized
|
|
* demand, NOT amps, watts, ERPM, physical velocity, or a guaranteed turn radius.
|
|
*/
|
|
export function mix(profile: ControlProfile, axes: Axes): Sides {
|
|
parseProfile(profile);
|
|
for (const key of axisKeys(profile)) if (!bounded(axes[key], -1, 1)) throw Error('Missing or invalid axis');
|
|
let left: number, right: number;
|
|
if (profile.mode === 'tank') {
|
|
left = shapeAxis(axes.leftY, profile); right = shapeAxis(axes.rightY, profile);
|
|
} else {
|
|
const throttle = shapeAxis(profile.stick === 'right' ? axes.rightY : axes.leftY, profile);
|
|
const turn = shapeAxis(profile.stick === 'right' ? axes.rightX : axes.leftX, profile);
|
|
const peak = Math.max(Math.abs(throttle), Math.abs(turn));
|
|
const scale = peak === 0 ? 0 : peak / (Math.abs(throttle) + Math.abs(turn));
|
|
left = (throttle + turn) * scale; right = (throttle - turn) * scale;
|
|
}
|
|
return {left: left * profile.outputScale || 0, right: right * profile.outputScale || 0};
|
|
}
|
|
/** All members of both sides are required, irrespective of 1x1/2x2/6x6. */
|
|
export function motorDemands(sides: Sides, bindings: readonly MotorBinding[]): Record<string, number> {
|
|
if (!bounded(sides.left, -1, 1) || !bounded(sides.right, -1, 1)
|
|
|| !bindings.some(b => b.side === 'left') || !bindings.some(b => b.side === 'right')) throw Error('Incomplete drive');
|
|
const values: Record<string, number> = Object.create(null);
|
|
for (const b of bindings) {
|
|
if (!/^[0-9a-f]{24}$/.test(b.uuid) || b.uuid in values || !['left', 'right'].includes(b.side)
|
|
|| ![1, -1].includes(b.forwardSign)) throw Error('Invalid motor binding');
|
|
values[b.uuid] = sides[b.side] * b.forwardSign || 0;
|
|
}
|
|
return values;
|
|
}
|