fix: zones bugs and adapted to real world use

src/lib/Controller.svelte

@@ -5,7 +5,8 @@
     import { onMount } from 'svelte';
 
 
-    const WEBSOCKET_URL = "http://localhost:1567";
+    // const WEBSOCKET_URL = "http://localhost:1567";
+    const WEBSOCKET_URL = "http://192.168.1.128:1567";
     const REAL_SEND_MODE = true;
     const MAX_SPEED = 50;
     const MOTORS = ["frontLeft", "frontRight", "backLeft", "backRight"];
@@ -16,12 +17,14 @@
     const ZONE_DIRECTIONS_MAPPING = ["E", "NE", "N", "NW", "W", "SW", "S", "SE"];
 
     let joystickBankEnabled = false;
+    let navigationEnabled = true;
 
     let rotationZones = [];
     let translationZones = [];
 
     let l2initialized;
-    let gpDictState;
+    let lastGpDictState;
+    let newGpDictState;
     let prettyGpState;
 
     let direction;
@@ -88,17 +91,54 @@
         }
     }
 
-    function processNewGamepadState(receivedState) {
+    function getValueInDict(dict, keys) {
-        gpDictState = getGamepadStateAsDict(lastGpState);
+        if (keys.length == 0) {
+            return dict;
+        }
+        return getValueInDict(dict[keys[0]], keys.slice(1))
+    }
 
-        if (!l2initialized && gpDictState.axes.l2 != 0) {
+    function hasGpChange(paths) {
+        // return true if any of the paths has changed (dot separated)
+        if (!lastGpDictState || !newGpDictState) { return true; }
+        return paths.some(path => {
+            let keys = path.split('.')
+            return getValueInDict(lastGpDictState, keys) !== getValueInDict(newGpDictState, keys)
+        })
+    }
+
+    function processNewGamepadState() {
+        console.log(lastGpDictState, newGpDictState)
+        if (!l2initialized && newGpDictState.axes.l2 != 0) {
             l2initialized = true;
         }
 
-        prettyGpState = JSON.stringify(gpDictState, " ", 4);
+        prettyGpState = JSON.stringify(newGpDictState, " ", 4);
 
-        leftJoystick.onGamepadJoystickChange(gpDictState.axes.left_joystick_x, gpDictState.axes.left_joystick_y);
+        if (newGpDictState.buttons.o.pressed) {
-        rightJoystick.onGamepadJoystickChange(gpDictState.axes.right_joystick_x, gpDictState.axes.right_joystick_y);
+            navigationEnabled = !navigationEnabled;
+            console.log("Navigation enabled", navigationEnabled);
+            currentWs.send("stop_robot", {});
+            return;
+        }
+        if (newGpDictState.buttons.x.pressed) {
+            // one time action
+            console.log("Stop robot action");
+            currentWs.send("stop_robot", {});
+        }
+
+        if (hasGpChange(['axes.left_joystick_x', 'axes.left_joystick_y'])) {
+            leftJoystick.onGamepadJoystickChange(
+                newGpDictState.axes.left_joystick_x,
+                newGpDictState.axes.left_joystick_y
+            );
+        }
+        if (hasGpChange(['axes.right_joystick_x', 'axes.right_joystick_y'])) {
+            rightJoystick.onGamepadJoystickChange(
+                newGpDictState.axes.right_joystick_x,
+                newGpDictState.axes.right_joystick_y
+            );
+        }
     }
 
 
@@ -109,14 +149,19 @@
             // console.log(lastGpState == receivedState, lastGpState, JSON.stringify(lastGpState), JSON.stringify(receivedState))
             if (lastGpState == null || !areSameObjects(lastGpState, receivedState)) {
                 // console.log("changed")
+                lastGpDictState = newGpDictState;
+                newGpDictState = getGamepadStateAsDict(receivedState);
+                processNewGamepadState();
                 lastGpState = receivedState;
-                processNewGamepadState(receivedState);
             }
         }
         window.requestAnimationFrame(loop);
     }
 
     function updateNavigation(angle, distance, zoneIndex) {
+        if (!navigationEnabled) {
+            return
+        }
         if (distance == 0) {
             currentWs.send("stop_robot", {})
             return
@@ -134,7 +179,7 @@
 
         // update the speed
         // TODO: Have the speed overwrite by non-gamepad users
-        let speedForced = l2initialized ? Math.round(((gpDictState.axes.l2 + 1)/2)*MAX_SPEED) : 0;
+        let speedForced = l2initialized ? Math.round(((newGpDictState.axes.l2 + 1)/2)*MAX_SPEED) : 0;
         let speedNormal = Math.round(0.50*distance*MAX_SPEED);
         let speed = Math.max(speedNormal, speedForced);
         finalSpeed = clamp(speed, 0, MAX_SPEED);
@@ -159,8 +204,8 @@
     onMount(() => {
         translationZones = generateTranslationZones();
         rotationZones = [
-            { from: -(6/8)*Math.PI, to: -(2/8)*Math.PI },
+            { from: Math.PI/2+ -(6/8)*Math.PI, to: Math.PI/2+ -(2/8)*Math.PI },
-            { to: (6/8)*Math.PI, from: (2/8)*Math.PI },
+            { to: Math.PI/2+ (6/8)*Math.PI, from: Math.PI/2+ (2/8)*Math.PI },
         ]
 
         initGamepads();
@@ -180,14 +225,30 @@
 
     function onLeftJoystickChange({ angle, distance, zone }) {
         console.log("onLeftJoystickChange", angle, distance, zone);
+        if (distance == 0) {
+            currentWs.send("stop_robot", {})
+            return
+        }
+        if (zone == 1) {
+            // up
+            currentWs.send("set_rotation", {"rotation": "CW"});
+        }
+        if (zone == 0) {
+            // down
+            // rotation 
+            currentWs.send("set_rotation", {"rotation": "ACW"});
+        }
+        let speedForced = l2initialized ? Math.round(((newGpDictState.axes.l2 + 1)/2)*MAX_SPEED) : 0;
+        let speedNormal = Math.round(0.50*distance*MAX_SPEED);
+        let speed = Math.max(speedNormal, speedForced);
+        finalSpeed = clamp(speed, 0, MAX_SPEED);
+
+        currentWs.send("set_speed", {"speed": finalSpeed});
     }
 
     function onRightJoystickChange({ angle, distance, zone }) {
-        updateNavigation(angle, distance, zone);
         console.log("onRightJoystickChange", angle, distance, zone);
-        
+        updateNavigation(angle, distance, zone);
-        // updateSpeed(distance);
-        // updateNavigation(angle);
     }
 
     $: updateRangesMap(inputRangesMap);

src/lib/Joystick.svelte

@@ -49,11 +49,21 @@
     function getZoneFromAngle(angle) {
         for (var i = 0; i < zonesDescriptions.length; i++) {
             let zone = zonesDescriptions[i];
+            // handle the special last zone with negative from
+            if (zone.from < 0) {
+                zone.from = 2*Math.PI+zone.from
+                if (angle >= zone.from) {
+                    return i;
+                }
+            }
             if (angle >= zone.from && angle <= zone.to) {
                 return i;
             }
         }
-        return 0;
+        if (angle >= zonesDescriptions[0].from || angle <= zonesDescriptions[0].to) {
+            return 0;
+        }
+        return -1;
     }
 
     function handleJoystickChange() {

src/lib/WebSocketService.js

@@ -64,6 +64,7 @@ export default class WebSocketService extends EventTarget {
             }
             this.isConnected = false
             this.recovery = true
+            console.log("retrying in 1000ms")
             setTimeout(this.start.bind(this), 1000)
         }
     }
@@ -120,7 +121,7 @@ export default class WebSocketService extends EventTarget {
 
     send(cmd, args = {}) {
         const payload = {cmd, args};
-        if (!this.ws.readyState || !this.ws.isEnabled) {
+        if (!this.ws.readyState || !this.isEnabled) {
             console.log("Would have sent", payload)
             return
         }

src/styles/app.scss

@@ -0,0 +1,3 @@
+body {
+    background: red;
+}