From a4b322b865415547ff0941fc2dcfa12fd39af2c9 Mon Sep 17 00:00:00 2001 From: Matthieu Bessat Date: Sat, 8 Apr 2023 09:34:16 +0200 Subject: [PATCH] fix: zones bugs and adapted to real world use --- src/lib/Controller.svelte | 93 ++++++++++++++++++++++++++++++------- src/lib/Joystick.svelte | 12 ++++- src/lib/WebSocketService.js | 3 +- src/styles/app.scss | 3 ++ 4 files changed, 93 insertions(+), 18 deletions(-) create mode 100644 src/styles/app.scss diff --git a/src/lib/Controller.svelte b/src/lib/Controller.svelte index 03bcc6e..6f00b87 100644 --- a/src/lib/Controller.svelte +++ b/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) { - gpDictState = getGamepadStateAsDict(lastGpState); + function getValueInDict(dict, keys) { + 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); - rightJoystick.onGamepadJoystickChange(gpDictState.axes.right_joystick_x, gpDictState.axes.right_joystick_y); + if (newGpDictState.buttons.o.pressed) { + 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 }, - { to: (6/8)*Math.PI, from: (2/8)*Math.PI }, + { from: Math.PI/2+ -(6/8)*Math.PI, to: Math.PI/2+ -(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); - - // updateSpeed(distance); - // updateNavigation(angle); + updateNavigation(angle, distance, zone); } $: updateRangesMap(inputRangesMap); diff --git a/src/lib/Joystick.svelte b/src/lib/Joystick.svelte index bd798d5..34e686b 100644 --- a/src/lib/Joystick.svelte +++ b/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() { diff --git a/src/lib/WebSocketService.js b/src/lib/WebSocketService.js index 84d5a6a..c7083dd 100644 --- a/src/lib/WebSocketService.js +++ b/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 } diff --git a/src/styles/app.scss b/src/styles/app.scss new file mode 100644 index 0000000..5c5e887 --- /dev/null +++ b/src/styles/app.scss @@ -0,0 +1,3 @@ +body { + background: red; +}