diff --git a/client/src/lib/Dashboard/Visualization/Controls.svelte b/client/src/lib/Dashboard/Visualization/Controls.svelte index 50b237c..f78fd68 100644 --- a/client/src/lib/Dashboard/Visualization/Controls.svelte +++ b/client/src/lib/Dashboard/Visualization/Controls.svelte @@ -1,10 +1,19 @@ - - diff --git a/client/src/lib/Dashboard/Visualization/Scene.svelte b/client/src/lib/Dashboard/Visualization/Scene.svelte index b530293..d276160 100644 --- a/client/src/lib/Dashboard/Visualization/Scene.svelte +++ b/client/src/lib/Dashboard/Visualization/Scene.svelte @@ -2,22 +2,37 @@ import { T, useTask } from '@threlte/core' import { ContactShadows, Float, Grid, OrbitControls } from '@threlte/extras' import Hornet from './models/Hornet.svelte' - import { onMount } from 'svelte' import Controls from './Controls.svelte' - import type { Camera, Group, Object3D, Object3DEventMap } from 'three' + import { + Vector3, + type Camera, + type Group, + type Object3D, + type Object3DEventMap, + } from 'three' import { telemetryReadonlyStore, telemetryStore, } from '../../stores/telemetryStore' import { get } from 'svelte/store' + import { Vector2 } from 'three' + import { SmoothMotionController } from './smoothMotionController' + import { onMount } from 'svelte' - // const rotate90 = () => { - // const originalRot = rot - // } + /* This is the root scene where the robot visualization is built. + It renders an infinite grid (it's not actually infinite, but we shouldn't run out + of space in realistic use), and a 3D model of the robot. The camera is locked + to the model, and the model is rotated to match the robot's orientation. + A PID controller is used to smoothly rotate the model to match the robot's + orientation and dampen out jittering. How does it work? Who knows! + 75% percent of this was created while reading + https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation, + and rest was generated by AI! + The rest of this codebase is remarkably jank-free, but this visualization module + is the most esoteric and jank code ever written. + */ - // onMount(() => { - // setTimeout(rotate90, 5000) - // }) + let shouldOrbit = true // CONSTANTS const maxAngularVelocity = 2 // Max angular velocity, in radians per second @@ -26,23 +41,8 @@ // Proportional control factor const kP = 2 // Adjust this value based on responsiveness and stability needs - // simulate some turning for testing - // const simulateTurning = () => { - // let delay = Math.random() * 4500 + 500 - // let randOffset = Math.random() * 170 * (Math.random() < 0.5 ? -1 : 1) - // telemetryStore.update({ - // ...get(telemetryReadonlyStore), - // orientation: get(telemetryReadonlyStore)['orientation'] + randOffset, - // }) - // setTimeout(simulateTurning, delay) - // } - // simulateTurning() - // Sync robot orientation with target rotation let targetRot = 0 - telemetryReadonlyStore.subscribe(value => { - targetRot = (value['orientation'] * Math.PI) / 180 // convert deg to rad - }) // Updates rotation to match target with PID controller (intended to be invoked in useTask) let rot = 0 // (initial) rotation in radians @@ -80,38 +80,54 @@ } } - // Assuming useTask is called every frame with the time delta + let robotPos: Vector3 = new Vector3(0, 0, 0) + + const robotPosition = new Vector2(0, 0) // Initial position + const initialVelocity = { x: 0, y: 0 } // Initial velocity + // The smooth motion controller utilizes a cubic hermite spline to interpolate between + // the current simulation velocity and the robot's actual velocity + const controller = new SmoothMotionController(robotPosition, initialVelocity) + + onMount(() => { + telemetryReadonlyStore.subscribe(value => { + targetRot = (value['orientation'] * Math.PI) / 180 // convert deg to rad + controller.setTargetVelocity({ + x: value['chassis-x-speed'], + y: value['chassis-y-speed'], + }) + shouldOrbit = value.gear === 'park' || value.gear === '-999' + if (shouldOrbit) { + robotPos = new Vector3(0, 0, 0) + controller.reset() + } + }) + }) + useTask(delta => { - updateRotation(delta) + if (!shouldOrbit) { + updateRotation(delta) + + controller.update(delta) + robotPos.x = controller.getPosition().x + robotPos.z = controller.getPosition().y + } }) let capsule: Group - let capRef: Camera + let capRef: Group $: if (capsule) { - // typescript hacks because i dont know what im doing - capRef = capsule as unknown as Camera + capRef = capsule } - - - - - + @@ -122,20 +138,20 @@ cellColor="#ffffff" sectionColor="#ffffff" sectionThickness={0} - fadeDistance={50} + fadeDistance={75} cellSize={2} + infiniteGrid /> - - - diff --git a/client/src/lib/Dashboard/Visualization/smoothMotionController.ts b/client/src/lib/Dashboard/Visualization/smoothMotionController.ts new file mode 100644 index 0000000..ee296c2 --- /dev/null +++ b/client/src/lib/Dashboard/Visualization/smoothMotionController.ts @@ -0,0 +1,54 @@ +import { Vector2 } from 'three' + +interface Velocity { + x: number + y: number +} + +export class SmoothMotionController { + private currentPosition: Vector2 + private currentVelocity: Vector2 + private targetVelocity: Velocity + private dampingFactor: number + + constructor( + initialPosition: Vector2, + initialVelocity: Velocity, + dampingFactor: number = 0.1 + ) { + this.currentPosition = initialPosition + this.currentVelocity = new Vector2(initialVelocity.x, initialVelocity.y) + this.targetVelocity = { ...initialVelocity } + this.dampingFactor = dampingFactor + } + + setTargetVelocity(velocity: Velocity) { + this.targetVelocity = velocity + } + + update(delta: number) { + // Apply cubic interpolation to smoothly transition the current velocity towards the target velocity + this.currentVelocity.x += + (this.targetVelocity.x - this.currentVelocity.x) * + this.dampingFactor * + delta + this.currentVelocity.y += + (this.targetVelocity.y - this.currentVelocity.y) * + this.dampingFactor * + delta + + // Update position based on the current velocity and the time delta + this.currentPosition.x += this.currentVelocity.x * delta * 3 + this.currentPosition.y += this.currentVelocity.y * delta * 3 + } + + getPosition(): Vector2 { + return this.currentPosition + } + + public reset() { + this.currentPosition = new Vector2(0, 0) + this.currentVelocity = new Vector2(0, 0) + this.targetVelocity = { x: 0, y: 0 } + } +}