fix: zones bugs and adapted to real world use

This commit is contained in:
Matthieu Bessat 2023-04-08 09:34:16 +02:00
parent acefcb0cd8
commit a4b322b865
4 changed files with 93 additions and 18 deletions

View file

@ -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);

View file

@ -49,12 +49,22 @@
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;
}
}
if (angle >= zonesDescriptions[0].from || angle <= zonesDescriptions[0].to) {
return 0;
}
return -1;
}
function handleJoystickChange() {
// distance between 0 and 1

View file

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

3
src/styles/app.scss Normal file
View file

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