7af2fc9f35
Replace constant-rate velocity rotate with ω0 spin + v_lat curl. Path samples are (x, y, theta); client time is index/SAMPLE_RATE_HZ. Ultraworked with [OhMyOpenCode](https://github.com/code-yeongyu/oh-my-opencode) Co-authored-by: Sisyphus <sisyphus@ohmyopencode>
39 lines
1.4 KiB
TypeScript
39 lines
1.4 KiB
TypeScript
import { HOG_LINE_Y, MAX_SPEED, MIN_SPEED, SAMPLE_RATE_HZ } from './protocol'
|
|
|
|
/** Path samples are (x, y, theta). Time is sample index / SAMPLE_RATE_HZ. */
|
|
export function sampleTime(index: number): number {
|
|
return index / SAMPLE_RATE_HZ
|
|
}
|
|
|
|
export function trimPathToStartAtHogLine(
|
|
path: [number, number, number][],
|
|
): [number, number, number][] {
|
|
if (path.length < 2) return path
|
|
const idx = path.findIndex(([, y]) => y >= HOG_LINE_Y)
|
|
if (idx < 0) return path
|
|
// Start just before the hog line crossing so the stone enters smoothly.
|
|
// Theta is preserved; time is rebased via sample index on the trimmed array.
|
|
const start = Math.max(0, idx - 1)
|
|
return path.slice(start)
|
|
}
|
|
|
|
/** Index at which a path should start for hog-line sync (same as trim start). */
|
|
export function hogTrimStartIndex(path: [number, number, number][]): number {
|
|
if (path.length < 2) return 0
|
|
const idx = path.findIndex(([, y]) => y >= HOG_LINE_Y)
|
|
if (idx < 0) return 0
|
|
return Math.max(0, idx - 1)
|
|
}
|
|
|
|
export function velocityToWeight(velocity: number): number {
|
|
const t = (velocity - MIN_SPEED) / (MAX_SPEED - MIN_SPEED)
|
|
const weight = 1 + Math.round(t * 9)
|
|
return Math.max(1, Math.min(10, weight))
|
|
}
|
|
|
|
export function weightToVelocity(weight: number): number {
|
|
const clamped = Math.max(1, Math.min(10, weight))
|
|
const t = (clamped - 1) / 9
|
|
return MIN_SPEED + t * (MAX_SPEED - MIN_SPEED)
|
|
}
|