554 lines
13 KiB
JavaScript
554 lines
13 KiB
JavaScript
const ROBOT_URL = "192.168.185.166";
|
|
const CURL_URL = "localhost:4444";
|
|
const INFO_POSITION = 0x01;
|
|
const INFO_LED = 0x02;
|
|
|
|
const DEBUG = false;
|
|
|
|
let socketMoteur = undefined;
|
|
let socketInfos = undefined;
|
|
let moteur = undefined;
|
|
let led = undefined;
|
|
let position = undefined;
|
|
let pid = undefined;
|
|
let turtle = undefined;
|
|
|
|
|
|
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.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 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;
|
|
|
|
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;
|
|
|
|
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);
|
|
|
|
if (DEBUG) {
|
|
console.log(
|
|
{"posX": posX, "posY": poxY, "Rot": posR}
|
|
);
|
|
}
|
|
|
|
pid.get(posX, this.posY, posR);
|
|
}
|
|
});
|
|
}
|
|
|
|
async reset() {
|
|
await fetch("http://" + ROBOT_URL + "/position/reset", {
|
|
method: 'get',
|
|
headers: { "Content-Type": "application/x-www-form-urlencoded" },
|
|
mode: "no-cors"
|
|
});
|
|
}
|
|
}
|
|
|
|
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),
|
|
});
|
|
}
|
|
|
|
async tourner(angle) {
|
|
const data = {
|
|
'type': 'angle',
|
|
'angle': angle,
|
|
}
|
|
await fetch("http://" + CURL_URL, {
|
|
method: "POST",
|
|
mode: "no-cors",
|
|
body: JSON.stringify(data),
|
|
});
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
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;
|
|
chemins = new Array();
|
|
}
|
|
|
|
addCheminPossible(chemin){
|
|
this.chemins.push(chemin);
|
|
}
|
|
|
|
getCheminsPossibles(){
|
|
return this.chemins;
|
|
}
|
|
|
|
toString(){
|
|
console.log("node : (" + currentNode.getX + "; " + currentNode.getY + ")");
|
|
}
|
|
|
|
toStringWithVoisins(){
|
|
console.log("node : (" + currentNode.getX + "; " + currentNode.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;
|
|
|
|
run(){
|
|
let iter = 0;
|
|
while(!this.isObjectifTrouve()){
|
|
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);
|
|
}
|
|
}
|
|
|
|
isObjectifTrouve(){
|
|
return false;
|
|
}
|
|
|
|
//////////////////////////////////////// ANALYSE ENVIRONNEMENT
|
|
|
|
isObstacleDevant (currentNode, rotation) {
|
|
return false;
|
|
}
|
|
|
|
analyseEnvironnement(currentNode){
|
|
// SI case déjà visitée, alors on connait ses obstacles
|
|
if (currentNode == null) {
|
|
|
|
// pour les 4 directions possibles
|
|
for (let i = 0; i < 4; i++) {
|
|
if (!this.isObstacleDevant(currentNode, this.rotation)){
|
|
this.addCheminPossible(currentNode);
|
|
}
|
|
this.tourne(); // changement de l'angle de 90 degrés
|
|
}
|
|
this.noeudsVisites.push(currentNode);
|
|
}
|
|
}
|
|
|
|
tourne(){
|
|
turtle.tourner(-90);
|
|
this.rotation++;
|
|
this.rotation %= 4;
|
|
}
|
|
|
|
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));
|
|
}
|
|
this.cheminsPossibles.push(this.chemin);
|
|
}
|
|
|
|
//////////////////////////////////////// MOVE
|
|
|
|
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
|
|
this.move();
|
|
}
|
|
|
|
// parcours en profondeur : il s'agit de LIFOs
|
|
getNextNode(){
|
|
if (this.cheminsPossibles[this.cheminsPossibles.length - 1] > 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){
|
|
nextNode = this.getCaseAnalysee();
|
|
// on ajoute le noeud visité à toutes les listes sauf la dernière
|
|
for (let i=0; i < this.cheminsPossibles -1; i++){
|
|
const cheminPossible = this.cheminsPossibles[i];
|
|
if (nextNode == 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 = Orientation.GAUCHE
|
|
this.posY--;
|
|
} else if (currentNode.getX < nextNode.getX){
|
|
this.rotation = Orientation.DROITE
|
|
this.pos++;
|
|
} else if (currentNode.getY > nextNode.getY){
|
|
this.rotation = Orientation.BAS
|
|
this.posX--;
|
|
} else {
|
|
this.rotation = Orientation.HAUT
|
|
this.posX++;
|
|
}
|
|
console.log("new coordonnees : (" + this.posX + "; " + this.posY + ")");
|
|
}
|
|
|
|
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;
|
|
}
|
|
aRotater *= 90;
|
|
turtle.tourner(aRotater);
|
|
turtle.avancer(100);
|
|
}
|
|
|
|
/////////////////////////////////////// GETTERS
|
|
|
|
getCaseAnalysee(){
|
|
let index = 0;
|
|
this.noeudsVisites.forEach((noeud) => {
|
|
if (noeud == null) {
|
|
console.error(this.noeudsVisites);
|
|
console.error("INDEX NULL : " + index);
|
|
}
|
|
index++;
|
|
if (noeud.posX == this.posX && noeud.posY == this.posY){
|
|
return noeud;
|
|
}
|
|
});
|
|
return null;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
async function run() {
|
|
connectSockets();
|
|
moteur = new Moteur();
|
|
led = new Led();
|
|
position = new Position(socketInfos);
|
|
pid = new PID();
|
|
turtle = new Turtle();
|
|
led.red();
|
|
|
|
|
|
/*
|
|
|
|
|
|
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 => {
|
|
const data = e.data;
|
|
console.log(data);
|
|
});
|
|
|
|
*/
|
|
|
|
}
|
|
|
|
|
|
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),
|
|
});
|
|
}
|
|
|
|
|
|
|