590 lines
15 KiB
JavaScript
590 lines
15 KiB
JavaScript
const ROBOT_URL = "192.168.185.166";
|
|
const CURL_URL = "localhost:4444";
|
|
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 = 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 turtle = undefined;
|
|
|
|
const sleep = async (time) => {
|
|
await new Promise(r => setTimeout(r, time));
|
|
}
|
|
|
|
const connectSockets = () => {
|
|
if ((socketMoteur != undefined) || (socketInfos != undefined)) {
|
|
disconnectSocket();
|
|
}
|
|
socketMoteur = new WebSocket("ws://" + ROBOT_URL + "/motors.ws");
|
|
socketInfos = new WebSocket("ws://" + ROBOT_URL + "/infos.ws");
|
|
socketInfos.binaryType = "arraybuffer";
|
|
|
|
socketMoteur.onclose = function(event) {
|
|
if (event.wasClean) {
|
|
console.log(`[socketMoteur] Connection closed cleanly, code=${event.code} reason=${event.reason}`);
|
|
} else {
|
|
console.log('[socketMoteur] Connection died');
|
|
}
|
|
};
|
|
|
|
socketMoteur.onopen = function(e) {
|
|
console.log("[socketMoteur] Connection established");
|
|
};
|
|
|
|
socketInfos.onopen = function(e) {
|
|
console.log("[socketInfos] Connection established");
|
|
position.reset();
|
|
led.green();
|
|
new Promise(r => setTimeout(r, 2000)).then(() => {
|
|
led.off();
|
|
});
|
|
socketInfos.send(new Uint8Array([0x32, 0x20]));
|
|
};
|
|
|
|
socketInfos.onclose = function(event) {
|
|
if (event.wasClean) {
|
|
console.log(`[socketInfos] Connection closed cleanly, code=${event.code} reason=${event.reason}`);
|
|
} else {
|
|
console.log('[socketInfos] Connection died');
|
|
}
|
|
};
|
|
|
|
socketMoteur.onerror = function(error) {
|
|
console.log(`[socketMoteur] ERROR : ${error}`);
|
|
};
|
|
|
|
socketInfos.onerror = function(error) {
|
|
console.log(`[socketInfos] ERROR : ${error}`);
|
|
};
|
|
}
|
|
|
|
const disconnectSocket = () => {
|
|
if (socketMoteur != undefined) {
|
|
socketMoteur.close();
|
|
}
|
|
if (socketInfos != undefined) {
|
|
socketInfos.close();
|
|
}
|
|
|
|
socketMoteur = undefined;
|
|
socketInfos = undefined;
|
|
}
|
|
|
|
|
|
|
|
|
|
class Moteur {
|
|
|
|
motors_buf = new Float32Array(2);
|
|
|
|
send(d, g, v) {
|
|
if (socketMoteur == undefined) {
|
|
return;
|
|
}
|
|
this.motors_buf[0] = d * v;
|
|
this.motors_buf[1] = g * v;
|
|
|
|
socketMoteur.send(this.motors_buf);
|
|
}
|
|
|
|
avancer(v) {
|
|
this.send(1, 1, v);
|
|
}
|
|
|
|
reculer(v) {
|
|
this.send(-1, -1, v);
|
|
}
|
|
|
|
droite(v) {
|
|
this.send(1, -1, v);
|
|
}
|
|
|
|
gauche(v) {
|
|
this.send(-1, 1, v);
|
|
}
|
|
|
|
stop() {
|
|
this.send(0, 0, 0);
|
|
}
|
|
}
|
|
|
|
class Led {
|
|
|
|
http = undefined;
|
|
|
|
async send(color){
|
|
await fetch('http://' + ROBOT_URL + '/led/set_color', {
|
|
method: 'post',
|
|
headers: { "Content-Type": "application/x-www-form-urlencoded" },
|
|
mode: "no-cors",
|
|
body: 'ledcolor=' + color
|
|
});
|
|
}
|
|
|
|
red(){
|
|
this.send("#ff0000");
|
|
}
|
|
|
|
green(){
|
|
this.send("#00ff00");
|
|
}
|
|
|
|
blue(){
|
|
this.send("#0000ff");
|
|
}
|
|
|
|
off(){
|
|
this.send("#000000");
|
|
}
|
|
|
|
}
|
|
|
|
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(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": this.posX, "posY": this.posY, "Rot": this.Rot}
|
|
);
|
|
}
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|
|
});
|
|
}
|
|
|
|
async reset() {
|
|
await fetch("http://" + ROBOT_URL + "/position/reset", {
|
|
method: 'get',
|
|
headers: { "Content-Type": "application/x-www-form-urlencoded" },
|
|
mode: "no-cors"
|
|
});
|
|
}
|
|
|
|
getCapteurDistance() {
|
|
return this.capteur_distance;
|
|
}
|
|
|
|
isMurDevant() {
|
|
return this.distance_mur_moyenne < MUR_THRESHOLD;
|
|
}
|
|
}
|
|
|
|
class Turtle {
|
|
|
|
async avancer(distance) {
|
|
const data = {
|
|
'type': 'dist',
|
|
'dist': distance,
|
|
}
|
|
await fetch("http://" + CURL_URL, {
|
|
method: "POST",
|
|
mode: "no-cors",
|
|
body: JSON.stringify(data),
|
|
});
|
|
await sleep(MASTER_SLEEP_TIME);
|
|
}
|
|
|
|
async tourner(angle) {
|
|
const data = {
|
|
'type': 'angle',
|
|
'angle': angle,
|
|
}
|
|
await fetch("http://" + CURL_URL, {
|
|
method: "POST",
|
|
mode: "no-cors",
|
|
body: JSON.stringify(data),
|
|
});
|
|
await sleep(MASTER_SLEEP_TIME);
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
const Orientation = Object.freeze({
|
|
HAUT: 0,
|
|
DROITE: 1,
|
|
BAS: 2,
|
|
GAUCHE: 3,
|
|
});
|
|
|
|
|
|
class Noeud {
|
|
coordX;
|
|
coordY;
|
|
chemins;
|
|
|
|
constructor(coordX, coordY){
|
|
this.coordX = coordX;
|
|
this.coordY = coordY;
|
|
this.chemins = new Array();
|
|
}
|
|
|
|
addCheminPossible(chemin){
|
|
this.chemins.push(chemin);
|
|
}
|
|
|
|
getCheminsPossibles(){
|
|
return this.chemins;
|
|
}
|
|
|
|
toString(){
|
|
console.log("node : (" + this.getX + "; " + this.getY + ")");
|
|
}
|
|
|
|
toStringWithVoisins(){
|
|
console.log("node : (" + this.getX + "; " + this.getY + ")");
|
|
console.log("voisins connus : " + this.chemins.forEach((chemin) => chemin.toString()));
|
|
}
|
|
}
|
|
|
|
|
|
class ModeAutomatique {
|
|
|
|
cheminsPossibles = new Array(); // liste des listes de noeuds
|
|
noeudsVisites = new Array();
|
|
|
|
// valeurs pour la première position
|
|
posX=0;
|
|
posY=0;
|
|
rotation = Orientation.HAUT;
|
|
lastRotation = this.rotation;
|
|
|
|
isFirstNode = true;
|
|
|
|
async run(){
|
|
led.blue();
|
|
let iter = 0;
|
|
while(!this.isObjectifTrouve()){
|
|
if (iter >= 50)
|
|
break;
|
|
console.log("Iteration " + iter++);
|
|
console.log(this.cheminsPossibles);
|
|
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(){
|
|
return false;
|
|
}
|
|
|
|
//////////////////////////////////////// ANALYSE ENVIRONNEMENT
|
|
|
|
async isObstacleDevant () {
|
|
//await turtle.avancer(20);
|
|
await sleep(2000);
|
|
const mur = position.isMurDevant();
|
|
//await turtle.avancer(-20);
|
|
return mur;
|
|
}
|
|
|
|
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 (!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.noeudsVisites.push(currentNode);
|
|
console.log("noeuds visités : " + this.noeudsVisites)
|
|
}
|
|
return currentNode;
|
|
}
|
|
|
|
async tourne(){
|
|
await turtle.tourner(90);
|
|
this.rotation--;
|
|
if (this.rotation < 0) {
|
|
this.rotation = 3;
|
|
}
|
|
}
|
|
|
|
addCheminPossible(){
|
|
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));
|
|
}
|
|
console.log(chemin);
|
|
this.cheminsPossibles.push(chemin);
|
|
console.log(this.cheminsPossibles);
|
|
}
|
|
|
|
//////////////////////////////////////// MOVE
|
|
|
|
async deplacer(currentNode){
|
|
// on choisi le prochain noeud
|
|
const nextNode = this.getNextNode();
|
|
if (nextNode != null) {
|
|
console.log("next node : ");
|
|
nextNode.toString();
|
|
}
|
|
// on met à jour la liste des chemins en ajoutant le noeud courant dans les chemins non choisis
|
|
this.updatePaths(nextNode);
|
|
// on met à jour les coordonnées
|
|
this.updateCoordonnees(currentNode, nextNode);
|
|
// move
|
|
await this.move();
|
|
}
|
|
|
|
// parcours en profondeur : il s'agit de LIFOs
|
|
getNextNode(){
|
|
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
|
|
const nodeChoisi = cheminChoisi[cheminChoisi.length - 1];
|
|
console.log("noeud choisi :");
|
|
nodeChoisi.toString();
|
|
// on créé un nouveau noeud
|
|
return new Noeud(nodeChoisi.posX, nodeChoisi.posY);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
updatePaths(nextNode){
|
|
// on ajoute le noeud visité à toutes les listes sauf la dernière
|
|
for (let i=0; i < this.cheminsPossibles.length -1; i++){
|
|
const cheminPossible = this.cheminsPossibles[i];
|
|
|
|
if (this.getCaseAnalyseeWithCoord(nextNode.getX, nextNode.getY == null)){
|
|
// on ajoute le noeud en cours à la liste des noeuds visités
|
|
cheminPossible.push(nextNode);
|
|
} else {
|
|
// on vire le noeud visité de toutes les listes s'il a déjà été visité avant (on fait demi tour)
|
|
cheminPossible.pop();
|
|
}
|
|
}
|
|
|
|
// on supprime les listes vides
|
|
this.cheminsPossibles = this.cheminsPossibles.filter(function (el) {
|
|
return el.length > 0;
|
|
});
|
|
console.log("nombre de chemins restants : " + this.cheminsPossibles.length)
|
|
console.log("chemins possibles : " + this.cheminsPossibles)
|
|
}
|
|
|
|
updateCoordonnees(currentNode, nextNode) {
|
|
console.log("coordonnees : (" + this.posX + "; " + this.posY + ")");
|
|
this.lastRotation = this.rotation;
|
|
if (currentNode.getX > nextNode.getX){
|
|
this.rotation = 3
|
|
this.posY--;
|
|
} else if (currentNode.getX < nextNode.getX){
|
|
this.rotation = 1
|
|
this.pos++;
|
|
} else if (currentNode.getY > nextNode.getY){
|
|
this.rotation = 2
|
|
this.posX--;
|
|
} else {
|
|
this.rotation = 0
|
|
this.posX++;
|
|
}
|
|
console.log("new coordonnees : (" + this.posX + "; " + this.posY + ")");
|
|
}
|
|
|
|
async move(){
|
|
// déplacer robot (les coordonnées sont déjà à jour)
|
|
let aRotater = this.rotation - this.lastRotation;
|
|
if (aRotater > 2) {
|
|
aRotater -= 2;
|
|
}
|
|
if (aRotater < 2) {
|
|
aRotater += 2;
|
|
}
|
|
if (aRotater > 0) {
|
|
aRotater = 4 - aRotater;
|
|
}
|
|
aRotater *= 90;
|
|
turtle.tourner(aRotater);
|
|
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++));
|
|
return;
|
|
}
|
|
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;
|
|
}
|
|
});
|
|
return null;
|
|
}
|
|
}
|
|
|
|
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);
|
|
turtle = new Turtle();
|
|
led.red();
|
|
|
|
algo();
|
|
|
|
|
|
|
|
/*
|
|
led.red();
|
|
await sleep(400);
|
|
|
|
led.green();
|
|
await sleep(400);
|
|
|
|
led.blue();
|
|
await sleep(400);
|
|
*/
|
|
|
|
}
|