From 85e39b69483fa62f76644f3c71106807f3826abb Mon Sep 17 00:00:00 2001 From: Nogard Date: Sun, 23 Mar 2025 08:28:48 +0100 Subject: [PATCH] =?UTF-8?q?fatigu=C3=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- script.js | 376 ++++++++++++++++++++++++++++++------------------------ 1 file changed, 206 insertions(+), 170 deletions(-) diff --git a/script.js b/script.js index abd7add..e0a582a 100644 --- a/script.js +++ b/script.js @@ -1,18 +1,27 @@ const ROBOT_URL = "192.168.185.166"; const CURL_URL = "localhost:4444"; -const INFO_POSITION = 0x01; -const INFO_LED = 0x02; +const INFOS_POSITION = 0x01; +const INFOS_LED = 0x02; +const INFOS_MOTORS = 0x04; +const INFOS_WHEELS = 0x08; +const INFOS_SPEED = 0x10; +const INFOS_RANGEFINDER = 0x20; -const DEBUG = false; +const DEBUG = true; + +const MUR_THRESHOLD = 80; +const MASTER_SLEEP_TIME = 500; let socketMoteur = undefined; let socketInfos = undefined; let moteur = undefined; let led = undefined; let position = undefined; -let pid = undefined; let turtle = undefined; +const sleep = async (time) => { + await new Promise(r => setTimeout(r, time)); +} const connectSockets = () => { if ((socketMoteur != undefined) || (socketInfos != undefined)) { @@ -41,6 +50,7 @@ const connectSockets = () => { new Promise(r => setTimeout(r, 2000)).then(() => { led.off(); }); + socketInfos.send(new Uint8Array([0x32, 0x20])); }; socketInfos.onclose = function(event) { @@ -110,76 +120,6 @@ class Moteur { } } -class PID { - - kpx = 0; - kix = 0; - kdx = 0; - - kpy = 0; - kiy = 0; - kdy = 0; - - kpr = 0; - kir = 0; - kdr = 0; - - px = 0; - ix = 0; - dx = 0; - - py = 0; - iy = 0; - dy = 0; - - pr = 0; - ir = 0; - dr = 0; - - tagetX = 0; - tagetY = 0; - targetR = 0; - run = false; - - lastTime = undefined; - - set(targetX, targetY, targetR) { - this.targetX = targetX; - this.px = 0; - this.ix = 0; - this.dx = 0; - - this.targetY = targetY; - this.py = 0; - this.iy = 0; - this.dy = 0; - - this.targetR = targetR; - this.pr = 0; - this.ir = 0; - this.dr = 0; - - this.lastTime = Date.now().valueOf(); - this.run = true; - } - - - get(currentX, currentY, currentR) { - if (!this.run) { - return; - } - const currentTime = Date.now().valueOf(); - const deltaTime = (currentTime - this.lastTime) / 1000.0; - - this.p - - - console.log("Time delta : " + deltaTime); - this.lastTime = currentTime; - } - -} - class Led { http = undefined; @@ -216,26 +156,64 @@ class Position { posX = 0; posY = 0; Rot = 0; + capteur_distance = 0; + distance_mur_moyenne = 0; constructor(socket) { socket.addEventListener("message", e => { const view = new DataView(e.data); let pos = 0; - - const mask = view.getUint8(0); - - if (mask & INFO_POSITION) { - const posX = view.getFloat32(1); - const poxY = view.getFloat32(5); - const posR = view.getFloat32(9); - + + const mask = view.getUint8(pos); + pos++; + + if (mask & INFOS_POSITION) { + this.posX = view.getFloat32(pos + 0); + this.posY = view.getFloat32(pos + 4); + this.Rot = view.getFloat32(pos + 8); + pos += 3*4; if (DEBUG) { console.log( - {"posX": posX, "posY": poxY, "Rot": posR} + {"posX": this.posX, "posY": this.posY, "Rot": this.Rot} ); } + } - pid.get(posX, this.posY, posR); + if (mask & INFOS_LED) { + const led_r = view.getUint8(pos + 0); + const led_g = view.getUint8(pos + 1); + const led_b = view.getUint8(pos + 2); + pos += 3; + } + + if (mask & INFOS_MOTORS) { + const ml = view.getFloat32(pos + 0); + const mr = view.getFloat32(pos + 4); + pos += 2*4; + } + + if (mask & INFOS_WHEELS) { + const wl = view.getInt16(pos + 0); + const wr = view.getInt16(pos + 2); + const tl = view.getInt16(pos + 4); + const tr = view.getInt16(pos + 6); + pos += 4*2; + } + + if (mask & INFOS_SPEED) { + const w = view.getFloat32(pos + 0); + const v = view.getFloat32(pos + 4); + pos += 4*2; + } + + if (mask & INFOS_RANGEFINDER) { + this.capteur_distance = view.getUint16(pos + 0); + this.distance_mur_moyenne = (this.distance_mur_moyenne*2 + this.capteur_distance)/3; + pos += 2; + if (DEBUG) { + console.log("Capteur distance : " + this.capteur_distance); + console.log("Distance moyenne : " + this.distance_mur_moyenne); + } } }); } @@ -247,6 +225,14 @@ class Position { mode: "no-cors" }); } + + getCapteurDistance() { + return this.capteur_distance; + } + + isMurDevant() { + return this.distance_mur_moyenne < MUR_THRESHOLD; + } } class Turtle { @@ -261,6 +247,7 @@ class Turtle { mode: "no-cors", body: JSON.stringify(data), }); + await sleep(MASTER_SLEEP_TIME); } async tourner(angle) { @@ -273,8 +260,7 @@ class Turtle { mode: "no-cors", body: JSON.stringify(data), }); - - + await sleep(MASTER_SLEEP_TIME); } @@ -297,7 +283,7 @@ class Noeud { constructor(coordX, coordY){ this.coordX = coordX; this.coordY = coordY; - chemins = new Array(); + this.chemins = new Array(); } addCheminPossible(chemin){ @@ -309,11 +295,11 @@ class Noeud { } toString(){ - console.log("node : (" + currentNode.getX + "; " + currentNode.getY + ")"); + console.log("node : (" + this.getX + "; " + this.getY + ")"); } toStringWithVoisins(){ - console.log("node : (" + currentNode.getX + "; " + currentNode.getY + ")"); + console.log("node : (" + this.getX + "; " + this.getY + ")"); console.log("voisins connus : " + this.chemins.forEach((chemin) => chemin.toString())); } } @@ -329,17 +315,22 @@ class ModeAutomatique { posY=0; rotation = Orientation.HAUT; lastRotation = this.rotation; + + isFirstNode = true; - run(){ + async run(){ + led.blue(); let iter = 0; while(!this.isObjectifTrouve()){ + if (iter >= 50) + break; console.log("Iteration " + iter++); console.log(this.cheminsPossibles); - const currentNode = this.getCaseAnalysee(); - this.analyseEnvironnement(currentNode); // met à jour ses connaissances (à dessiner au fur et à mesure ?) - this.deplacer(currentNode); // récupère un chemin non encore parcourus en fonction de ses connaissances - sleep(5000); + let currentNode = this.getCaseAnalysee(); + currentNode = await this.analyseEnvironnement(currentNode); // met à jour ses connaissances (à dessiner au fur et à mesure ?) + await this.deplacer(currentNode); // récupère un chemin non encore parcourus en fonction de ses connaissances } + led.off(); } isObjectifTrouve(){ @@ -348,48 +339,72 @@ class ModeAutomatique { //////////////////////////////////////// ANALYSE ENVIRONNEMENT - isObstacleDevant (currentNode, rotation) { - return false; + async isObstacleDevant () { + //await turtle.avancer(20); + await sleep(2000); + const mur = position.isMurDevant(); + //await turtle.avancer(-20); + return mur; } - analyseEnvironnement(currentNode){ + async analyseEnvironnement(currentNode){ // SI case déjà visitée, alors on connait ses obstacles if (currentNode == null) { + console.log(">>>>>>>>> iteration ") + currentNode = new Noeud(0, 0); // pour les 4 directions possibles for (let i = 0; i < 4; i++) { - if (!this.isObstacleDevant(currentNode, this.rotation)){ + if (!await this.isObstacleDevant()){ this.addCheminPossible(currentNode); + console.log("chemins possibles : "); + console.log(this.cheminsPossibles); + } + await this.tourne(); // changement de l'angle de 90 degrés + if (i == 1 && !this.isFirstNode) { + this.tourne(); // On scan pas notre pt d'entrée + this.isFirstNode = false; + i++; } - this.tourne(); // changement de l'angle de 90 degrés } this.noeudsVisites.push(currentNode); + console.log("noeuds visités : " + this.noeudsVisites) } + return currentNode; } - tourne(){ - turtle.tourner(-90); - this.rotation++; - this.rotation %= 4; + async tourne(){ + await turtle.tourner(90); + this.rotation--; + if (this.rotation < 0) { + this.rotation = 3; + } } addCheminPossible(){ - this.chemin = new Array(); - if(this.rotation = Orientation.HAUT){ - this.chemin.push(new Noeud(posX+1, posY)); - } else if(this.rotation = Orientation.BAS){ - this.chemin.push(new Noeud(posX-1, posY)); - }else if(this.rotation = Orientation.GAUCHE){ - this.chemin.push(new Noeud(posX, posY-1)); - }else if(this.rotation = Orientation.DROITE){ - this.chemin.push(new Noeud(posX, posY+1)); + const chemin = new Array(); + console.log("rotation : " + this.rotation); + if(this.rotation == 0){ + console.log("rotation 0 "); + chemin.push(new Noeud(this.posX, this.posY+1)); + } else if(this.rotation == 2){ + console.log("rotation 2 "); + chemin.push(new Noeud(this.posX, this.posY-1)); + }else if(this.rotation == 3){ + console.log("rotation 3 "); + chemin.push(new Noeud(this.posX-1, this.posY)); + }else if(this.rotation == 1){ + console.log("rotation 1"); + chemin.push(new Noeud(this.posX+1, this.posY)); } - this.cheminsPossibles.push(this.chemin); + console.log(chemin); + this.cheminsPossibles.push(chemin); + console.log(this.cheminsPossibles); } //////////////////////////////////////// MOVE - deplacer(currentNode){ + async deplacer(currentNode){ // on choisi le prochain noeud const nextNode = this.getNextNode(); if (nextNode != null) { @@ -401,12 +416,12 @@ class ModeAutomatique { // on met à jour les coordonnées this.updateCoordonnees(currentNode, nextNode); // move - this.move(); + await this.move(); } // parcours en profondeur : il s'agit de LIFOs getNextNode(){ - if (this.cheminsPossibles[this.cheminsPossibles.length - 1] > 0) { + if (this.cheminsPossibles.length > 0) { // on récupère la dernière liste const cheminChoisi = this.cheminsPossibles[this.cheminsPossibles.length - 1]; // on récupère le dernier élément de cette liste @@ -420,11 +435,11 @@ class ModeAutomatique { } updatePaths(nextNode){ - nextNode = this.getCaseAnalysee(); // on ajoute le noeud visité à toutes les listes sauf la dernière - for (let i=0; i < this.cheminsPossibles -1; i++){ + for (let i=0; i < this.cheminsPossibles.length -1; i++){ const cheminPossible = this.cheminsPossibles[i]; - if (nextNode == null){ + + if (this.getCaseAnalyseeWithCoord(nextNode.getX, nextNode.getY == null)){ // on ajoute le noeud en cours à la liste des noeuds visités cheminPossible.push(nextNode); } else { @@ -445,22 +460,22 @@ class ModeAutomatique { console.log("coordonnees : (" + this.posX + "; " + this.posY + ")"); this.lastRotation = this.rotation; if (currentNode.getX > nextNode.getX){ - this.rotation = Orientation.GAUCHE + this.rotation = 3 this.posY--; } else if (currentNode.getX < nextNode.getX){ - this.rotation = Orientation.DROITE + this.rotation = 1 this.pos++; } else if (currentNode.getY > nextNode.getY){ - this.rotation = Orientation.BAS + this.rotation = 2 this.posX--; } else { - this.rotation = Orientation.HAUT + this.rotation = 0 this.posX++; } console.log("new coordonnees : (" + this.posX + "; " + this.posY + ")"); } - move(){ + async move(){ // déplacer robot (les coordonnées sont déjà à jour) let aRotater = this.rotation - this.lastRotation; if (aRotater > 2) { @@ -469,22 +484,42 @@ class ModeAutomatique { if (aRotater < 2) { aRotater += 2; } + if (aRotater > 0) { + aRotater = 4 - aRotater; + } aRotater *= 90; turtle.tourner(aRotater); - turtle.avancer(100); + await turtle.avancer(190); } /////////////////////////////////////// GETTERS getCaseAnalysee(){ let index = 0; + console.log(this.noeudsVisites) this.noeudsVisites.forEach((noeud) => { if (noeud == null) { console.error(this.noeudsVisites); - console.error("INDEX NULL : " + index); + console.error("INDEX NULL : " + (index++)); + return; } - index++; - if (noeud.posX == this.posX && noeud.posY == this.posY){ + if ((noeud.posX == this.posX) && (noeud.posY == this.posY)){ + return noeud; + } + }); + return null; + } + + getCaseAnalyseeWithCoord(x, y){ + let index = 0; + console.log(this.noeudsVisites) + this.noeudsVisites.forEach((noeud) => { + if (noeud == null) { + console.error(this.noeudsVisites); + console.error("INDEX NULL : " + (index++)); + return; + } + if ((noeud.posX == x) && (noeud.posY == y)){ return noeud; } }); @@ -492,62 +527,63 @@ class ModeAutomatique { } } +let doRun = false; +async function isMurDevant() { + turtle.avancer(20); + await sleep(2000); + const mur = position.isMurDevant(); + turtle.avancer(-20); + + return mur; +} + +async function algo() { + doRun = true; + while(doRun) { + + await turtle.avancer(1500); + await turtle.avancer(-20); + await turtle.tourner(30); + /* + + let mur = isMurDevant(); + if (!mur) { + await turtle.avancer(75); + } + await turtle.tourner(90); + mur = isMurDevant(); + if (mur) { + await turtle.tourner(-90); + await turtle.tourner(-90); + await turtle.tourner(-90); + } + */ + } +} async function run() { connectSockets(); moteur = new Moteur(); led = new Led(); position = new Position(socketInfos); - pid = new PID(); turtle = new Turtle(); led.red(); + algo(); + + /* - - - const socket = new WebSocket("ws://127.0.0.1:1337/ws"); - socket.onclose = function(event) { - if (event.wasClean) { - console.log(`[socket] Connection closed cleanly, code=${event.code} reason=${event.reason}`); - } else { - console.log('[socket] Connection died'); - } - }; - - socket.onopen = function(e) { - console.log("[socket] Connection established"); - socket.send("Test"); - }; - - socket.onerror = function(error) { - console.log(`[socket] ERROR : ${error}`); - }; + led.red(); + await sleep(400); - socket.addEventListener("message", e => { - const data = e.data; - console.log(data); - }); - + led.green(); + await sleep(400); + + led.blue(); + await sleep(400); */ } - - -async function test() { - let d = 190; - const data = { - 'type': 'dist', - 'dist': d, - } - await fetch("http://127.0.0.1:4444/", { - method: "POST", - mode: "no-cors", - body: JSON.stringify(data), - }); -} - - -