From 069f14ddf78e3d8d55c7ec0aba708d3d56f56e08 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Mon, 22 Jul 2024 13:57:11 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=99=8A=20Adds=20sonar=20support?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/src/lib/components/Visualization.svelte   | 13 ++++++++++++-
 app/src/lib/sceneBuilder.ts                   |  8 ++++++--
 app/src/lib/stores/socket-store.ts            |  1 +
 app/src/routes/+layout.svelte                 |  1 +
 esp32/features.ini                            |  1 +
 esp32/lib/ESP32-sveltekit/FeaturesService.cpp |  1 +
 6 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte
index 4c3082d..9350c92 100644
--- a/app/src/lib/components/Visualization.svelte
+++ b/app/src/lib/components/Visualization.svelte
@@ -1,6 +1,6 @@
 <script lang="ts">
 	import { onDestroy, onMount } from 'svelte';
-	import { BufferGeometry, Line, LineBasicMaterial, Mesh, MeshBasicMaterial, Object3D, SphereGeometry, Vector3, type NormalBufferAttributes, type Object3DEventMap } from 'three';
+	import { BufferGeometry, ConeGeometry, Line, LineBasicMaterial, Mesh, MeshBasicMaterial, Object3D, SphereGeometry, Vector3, type NormalBufferAttributes, type Object3DEventMap } from 'three';
 	import uzip from 'uzip';
 	import { ModesEnum, kinematicData, mode, model, outControllerData, servoAnglesOut, servoAngles, mpu, jointNames } from '$lib/stores';
 	import { footColor, isEmbeddedApp, throttler, toeWorldPositions } from '$lib/utilities';
@@ -178,6 +178,17 @@
 			trace_lines.push(geometry);
 			sceneManager.scene.add(line);
 		}
+
+        for (let i = 0; i < 2; i++) {
+            const coneGeometry = new ConeGeometry(0.1, 20+0.01*i, 32); 
+            const coneMaterial = new MeshBasicMaterial({ color: 0x00ff00, transparent: true, opacity: 0.2 });
+            const cone = new Mesh(coneGeometry, coneMaterial); 
+            cone.rotation.x = Math.PI / 2
+            cone.position.y = 2
+            cone.position.x = -0.25 + 0.5*i
+            cone.position.z = -10
+            sceneManager.scene.add(cone);
+        }
 	};
 
     const renderTraceLines = (foot_positions: Vector3[]) => {
diff --git a/app/src/lib/sceneBuilder.ts b/app/src/lib/sceneBuilder.ts
index a202fee..708f6f9 100644
--- a/app/src/lib/sceneBuilder.ts
+++ b/app/src/lib/sceneBuilder.ts
@@ -18,7 +18,8 @@ import {
 	EquirectangularReflectionMapping,
 	ACESFilmicToneMapping,
 	MathUtils,
-	MeshStandardMaterial
+	MeshStandardMaterial,
+	Group
 } from 'three';
 import { Sky } from 'three/addons/objects/Sky.js';
 import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls';
@@ -72,6 +73,7 @@ export default class SceneBuilder {
 	highlightMaterial: any;
 	sky!: Sky;
 	transformControl: TransformControls;
+	public modelGroup!: Group;
 
 	constructor() {
 		this.scene = new Scene();
@@ -282,8 +284,10 @@ export default class SceneBuilder {
 	};
 
 	public addModel = (model: any) => {
+		this.modelGroup = new Group();
+		this.modelGroup.add(model);
 		this.model = model;
-		this.scene.add(model);
+		this.scene.add(this.modelGroup);
 		return this;
 	};
 
diff --git a/app/src/lib/stores/socket-store.ts b/app/src/lib/stores/socket-store.ts
index 11dc6f6..963807a 100644
--- a/app/src/lib/stores/socket-store.ts
+++ b/app/src/lib/stores/socket-store.ts
@@ -10,6 +10,7 @@ export const servoAngles: Writable<number[]> = writable([
 export const logs = writable([] as string[]);
 export const battery = writable({});
 export const mpu = writable({ heading: 0 });
+export const sonar = writable([0, 0]);
 export const distances = writable({});
 
 export interface socketDataCollection {
diff --git a/app/src/routes/+layout.svelte b/app/src/routes/+layout.svelte
index c11d8e3..4ae1ec6 100644
--- a/app/src/routes/+layout.svelte
+++ b/app/src/routes/+layout.svelte
@@ -46,6 +46,7 @@
 		if ($page.data.features.analytics) socket.on('analytics', handleAnalytics);
 		if ($page.data.features.battery) socket.on('battery', handleBattery);
 		if ($page.data.features.download_firmware) socket.on('otastatus', handleOAT);
+        if ($page.data.features.sonar) socket.on('sonar', data => console.log(data))
 	};
 
     const removeEventListeners = () => {
diff --git a/esp32/features.ini b/esp32/features.ini
index c476c13..27828db 100644
--- a/esp32/features.ini
+++ b/esp32/features.ini
@@ -15,5 +15,6 @@ build_flags =
   -D FT_BMP=1
   -D FT_GPS=0
   -D FT_WS2812=1
+  -D FT_USS=0
   -D FT_SERVO=1
   -D FT_ADS1115=1
diff --git a/esp32/lib/ESP32-sveltekit/FeaturesService.cpp b/esp32/lib/ESP32-sveltekit/FeaturesService.cpp
index 0833a8c..5956139 100644
--- a/esp32/lib/ESP32-sveltekit/FeaturesService.cpp
+++ b/esp32/lib/ESP32-sveltekit/FeaturesService.cpp
@@ -33,6 +33,7 @@ void FeaturesService::begin() {
         root["imu"] = FT_IMU;
         root["mag"] = FT_MAG;
         root["bmp"] = FT_BMP;
+        root["sonar"] = FT_USS;
         root["firmware_version"] = APP_VERSION;
         root["firmware_name"] = APP_NAME;
         root["firmware_built_target"] = BUILD_TARGET;