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 }
+ }
+}