fatigué

script.js

@@ -1,18 +1,27 @@
 const ROBOT_URL = "192.168.185.166";
 const CURL_URL = "localhost:4444";
-const INFO_POSITION = 0x01;
+const INFOS_POSITION = 0x01;
-const INFO_LED = 0x02;
+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);
+            const mask = view.getUint8(pos);
-        
+            pos++;
-            if (mask & INFO_POSITION) {
+
-                const posX = view.getFloat32(1);
+            if (mask & INFOS_POSITION) {
-                const poxY = view.getFloat32(5);
+                this.posX = view.getFloat32(pos + 0);
-                const posR = view.getFloat32(9);
+                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();
+            let currentNode = this.getCaseAnalysee();
-            this.analyseEnvironnement(currentNode); // met à jour ses connaissances (à dessiner au fur et à mesure ?)
+            currentNode = await 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
+            await this.deplacer(currentNode); // récupère un chemin non encore parcourus en fonction de ses connaissances
-            sleep(5000);
         }
+        led.off();
     }
 
     isObjectifTrouve(){
@@ -348,48 +339,72 @@ class ModeAutomatique {
 
     //////////////////////////////////////// ANALYSE ENVIRONNEMENT
 
-    isObstacleDevant (currentNode, rotation) {
+    async isObstacleDevant () {
-        return false;
+        //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(){
+    async tourne(){
-        turtle.tourner(-90);
+        await turtle.tourner(90);
-        this.rotation++;
+        this.rotation--;
-        this.rotation %= 4;
+        if (this.rotation < 0) {
+            this.rotation = 3;
+        }
     }
 
     addCheminPossible(){
-        this.chemin = new Array();
+        const chemin = new Array();
-        if(this.rotation = Orientation.HAUT){
+        console.log("rotation : " + this.rotation);
-            this.chemin.push(new Noeud(posX+1, posY));
+        if(this.rotation == 0){
-        } else if(this.rotation = Orientation.BAS){
+            console.log("rotation 0 ");
-            this.chemin.push(new Noeud(posX-1, posY));
+            chemin.push(new Noeud(this.posX, this.posY+1));
-        }else if(this.rotation = Orientation.GAUCHE){
+        } else if(this.rotation == 2){
-            this.chemin.push(new Noeud(posX, posY-1));
+            console.log("rotation 2 ");
-        }else if(this.rotation = Orientation.DROITE){
+            chemin.push(new Noeud(this.posX, this.posY-1));
-            this.chemin.push(new Noeud(posX, 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();
+
+
 
 /*
-
+    led.red();
-
+    await sleep(400);
-    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}`);
-    };
     
-    socket.addEventListener("message", e => {
+    led.green();
-        const data = e.data;
+    await sleep(400);
-        console.log(data);
+    
-    });
+    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),
-  });
-}
-
-
-