diff --git a/algo.js b/algo.js deleted file mode 100644 index 4a23bec..0000000 --- a/algo.js +++ /dev/null @@ -1,92 +0,0 @@ -const Orientation = Object.freeze({ - HAUT: Symbol("haut"), - BAS: Symbol("bas"), - GAUCHE: Symbol("gauche"), - DROITE: Symbol("droite") -}); - - -class ModeAutomatique { - - casesParcourues = new Array(); - cheminsParcours = new Array(); - cheminsConnus = new Array(); - - - constructor(){ - // valeurs pour la première position - posX=0; - posY=0; - rotation=HAUT; - } - - run(){ - this.analyseEnvironnement(); // met à jour les obstacles - this.move(); // en fonction des chemins connus non encore parcourus - - //obstacle - if (!this.aBouge){ - // recule - // teste de 30 degrés - // tente d'avancer - // teste de 30 degrés - // tente d'avancer - // - } - } - - analyseEnvironnement(){ - if (isCaseAnalysee()) { - isObstacleDevant(); - tourne(); // - isObstacleDevant(); - tourne(); - isObstacleDevant(); - tourne(); - isObstacleDevant(); - tourne(); - } - - // SI case déjà visitée, alors on connait ses obstacles - } - - isCaseAnalysee(){ - // TODO - } - - updateCoordonnees() { - switch (this.rotation) { - case Orientation.HAUT: - - break; - case Orientation.BAS: - case Orientation.GAUCHE: - console.log("Mangoes and papayas are $2.79 a pound."); - // Expected output: "Mangoes and papayas are $2.79 a pound." - break; - case Orientation.DROITE: - console.log(`Sorry, we are out of ${expr}.`); - } - } - -} - -class Case { - 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 - } -} \ No newline at end of file diff --git a/curl.py b/curl.py new file mode 100644 index 0000000..c569c7b --- /dev/null +++ b/curl.py @@ -0,0 +1,39 @@ +import requests + +import aiohttp +from aiohttp import web, WSCloseCode +import asyncio + +async def http_handler(request): + payload = await request.text() + print(payload) + + url = 'http://192.168.185.166/turtle/send' + headers = {'content-type': 'application/json'} + r = requests.post(url, data=payload, headers=headers) + + print(r.text) + + return web.Response(text=r.text) + + +def create_runner(): + app = web.Application() + app.add_routes([ + web.post('/', http_handler), + ]) + return web.AppRunner(app) + + +async def start_server(host="127.0.0.1", port=4444): + runner = create_runner() + await runner.setup() + site = web.TCPSite(runner, host, port) + await site.start() + + +if __name__ == "__main__": + loop = asyncio.get_event_loop() + loop.run_until_complete(start_server()) + loop.run_forever() + diff --git a/script.js b/script.js index 56ff520..abd7add 100644 --- a/script.js +++ b/script.js @@ -1,4 +1,5 @@ -const URL = "192.168.185.20"; +const ROBOT_URL = "192.168.185.166"; +const CURL_URL = "localhost:4444"; const INFO_POSITION = 0x01; const INFO_LED = 0x02; @@ -9,14 +10,16 @@ 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://" + URL + "/motors.ws"); - socketInfos = new WebSocket("ws://" + URL + "/infos.ws"); + socketMoteur = new WebSocket("ws://" + ROBOT_URL + "/motors.ws"); + socketInfos = new WebSocket("ws://" + ROBOT_URL + "/infos.ws"); socketInfos.binaryType = "arraybuffer"; socketMoteur.onclose = function(event) { @@ -34,6 +37,10 @@ const connectSockets = () => { 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) { @@ -101,7 +108,75 @@ class Moteur { 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; + } } @@ -109,23 +184,29 @@ class Led { http = undefined; - send(color){ - this.http = new XMLHttpRequest(); - this.http.open('POST', 'http://' + URL + '/led/set_color', true); - this.http.setRequestHeader('Content-type', 'application/x-www-form-urlencoded'); - this.http.send('ledcolor=' + color); + 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") + this.send("#ff0000"); } green(){ - this.send("#00ff00") + this.send("#00ff00"); } blue(){ - this.send("#0000ff") + this.send("#0000ff"); + } + + off(){ + this.send("#000000"); } } @@ -144,34 +225,329 @@ class Position { const mask = view.getUint8(0); if (mask & INFO_POSITION) { - const position_x = view.getFloat32(1); - const position_y = view.getFloat32(5); - const position_a = view.getFloat32(9); + const posX = view.getFloat32(1); + const poxY = view.getFloat32(5); + const posR = view.getFloat32(9); if (DEBUG) { console.log( - {"posX": position_x, "posY": position_y, "Rot": position_a } + {"posX": posX, "posY": poxY, "Rot": posR} ); } + + pid.get(posX, this.posY, posR); } }); } - reset() { - try { - const request = new XMLHttpRequest(); - request.open("GET", "http://" + URL + "/position/reset"); - request.send(); - } catch (e) {} + 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())); } } -function run() { +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), + }); +} + + + diff --git a/simulateur/main.py b/simulateur/main.py new file mode 100644 index 0000000..9fed92b --- /dev/null +++ b/simulateur/main.py @@ -0,0 +1,47 @@ +#!/usr/bin/python3.7 + +import aiohttp +from aiohttp import web, WSCloseCode +import asyncio + + +async def http_handler(request): + return web.Response(text='Hello, world') + + +async def websocket_handler(request): + ws = web.WebSocketResponse() + await ws.prepare(request) + + async for msg in ws: + if msg.type == aiohttp.WSMsgType.TEXT: + if msg.data == 'close': + await ws.close() + else: + await ws.send_str('some websocket message payload') + elif msg.type == aiohttp.WSMsgType.ERROR: + print('ws connection closed with exception %s' % ws.exception()) + + return ws + + +def create_runner(): + app = web.Application() + app.add_routes([ + web.get('/', http_handler), + web.get('/ws', websocket_handler), + ]) + return web.AppRunner(app) + + +async def start_server(host="127.0.0.1", port=1337): + runner = create_runner() + await runner.setup() + site = web.TCPSite(runner, host, port) + await site.start() + + +if __name__ == "__main__": + loop = asyncio.get_event_loop() + loop.run_until_complete(start_server()) + loop.run_forever() \ No newline at end of file