finalHEADmain

6 files changed, 97 insertions, 297 deletions

diff --git a/index.html b/index.html
index 45ec687..1623c45 100644
--- a/index.html
+++ b/index.html
@@ -19,7 +19,7 @@
         </canvas>
 
         <script src="https://unpkg.com/mqtt/dist/mqtt.min.js"></script>
-        <script src="./temp_robot.js"></script>
-        <script src="./temp.js"></script>
+        <script src="./robot.js"></script>
+        <script src="./index.js"></script>
     </body>
 </html>
diff --git a/index.js b/index.js
index b4b6154..0a06b9e 100644
--- a/index.js
+++ b/index.js
@@ -9,8 +9,16 @@ const canvasOffsetX = canvas.offsetLeft;
 const canvasOffsetY = canvas.offsetTop;
 
 let scale = 2
-let angle = 0
 let drawings = []
+let prev = 0
+let angle = 0
+let reading = false
+
+let speed = 200 // mm/s
+let dx = 0
+let dy = 0
+let conv = 10
+let animation = 0
 
 canvas.width = window.innerWidth - canvasOffsetX * 2;
 canvas.height = window.innerHeight - canvasOffsetY * 2;
@@ -36,6 +44,10 @@ class Wall {
 }
 
 canvas.addEventListener('mousedown', (e) => {
+    reading = false
+    if (animation != 0) {
+        cancelAnimationFrame(animation)
+    }
     cross.style.visibility = "visible";
 
     x = e.layerX - canvas.width/2 - 2*canvasOffsetX
@@ -43,8 +55,7 @@ canvas.addEventListener('mousedown', (e) => {
     cross.style.left = e.layerX - canvasOffsetX + "px"
     cross.style.top = e.layerY - canvasOffsetY + "px"
 
-    
-    temp = angle
+    prev = angle
     angle = Math.atan(x/Math.abs(y))
 
     sym = 1
@@ -55,73 +66,110 @@ canvas.addEventListener('mousedown', (e) => {
     if (y > 0) {
         angle = sym*Math.PI - angle
     }
-    console.log(temp)
-    console.log(angle)
 
     robot.style.transform = 'translate(-50%, -50%) rotate('+ angle +'rad)';
-    a = -(e.layerX - canvasOffsetX - canvas.width/2) * scale
-    b = -(e.layerY - canvasOffsetY - canvas.height/2) * scale
-    rot = (angle - temp)
-    moveTo(a*Math.cos(-rot)- b * Math.sin(-rot),a * Math.sin(-rot) + b*Math.cos(-rot))
+    dist = Math.sqrt(x*x + y*y)
+    t = (dist/speed) * 1000/conv //miss conv
+    dx = x/(t)
+    dy = y/(t)
+    diff = angle - prev
+
+    if(angle * prev < 0 ) {
+        diff = (Math.abs(angle) + Math.abs(prev))
+        if (angle < 0)
+            diff = -diff
+    }
+    console.log("net angle :" + angle * 180 / Math.PI)
+
+    if(diff > Math.PI) {
+        diff = diff - 2*Math.PI
+    }
+    if (diff < -Math.PI) {
+        diff = diff + 2*Math.PI
+    }
+    moveTo(dist,diff)
     
-    moveAll(x,y)
+    moveAll()
 });
 
 client.on('message', function (topic, message) {
     let val = message.toString();
-    var n = parseInt(val);
-    if (!isNaN(n)){
-        if (n < 150){
-	        wall = new Wall(n * Math.sin(angle), - n * Math.cos(angle), angle)
-            drawings.push(wall)
-            draw(wall)
+    if (val == 'STOP') {
+        stop()
+    }
+    else {
+        if (reading) {
+            var n = parseInt(val);
+            if (!isNaN(n)){
+                if (n < 150){
+                    wall = new Wall(n * Math.sin(angle), - n * Math.cos(angle), angle)
+                    drawings.push(wall)
+                    draw(wall)
+                }
+            }
         }
     }
 });
 
-/*
+
 function test(wall) {
     drawings.push(wall)
     draw(wall)
 }
-*/
+
+function stop() {
+    dx = 0
+    dy = 0
+    if (animation != 0) {
+        cancelAnimationFrame(animation)
+    }
+    reading = true
+}
+
 
 function draw(wall) {
-            ctx.save()
-            x = wall.x
-            y = wall.y
-            wall.x = 0
-            wall.y = 0
-            ctx.translate(x,y)
-            ctx.rotate(wall.a)
-            wall.draw(ctx)
-            wall.x = x
-            wall.y = y
-            ctx.restore()
+    ctx.save()
+    x = wall.x
+    y = wall.y
+    wall.x = 0
+    wall.y = 0
+    ctx.translate(x,y)
+    ctx.rotate(wall.a)
+    wall.draw(ctx)
+    wall.x = x
+    wall.y = y
+    ctx.restore()
 }
 
-function moveAll(x,y) {
+function moveAll() {
+
     a = canvas.width/2 + canvasOffsetX
     b = canvas.height/2 + canvasOffsetY
     ctx.clearRect(-a,-b,canvas.width,canvas.height)
 
     drawings.forEach((e) => {
-        e.x -= x
-        e.y -= y
+        e.x -= dx
+        e.y -= dy
         draw(e) 
     })
+    animation = requestAnimationFrame(moveAll)
 }
 
 reset.onclick = () => {
+    a = canvas.width/2 + canvasOffsetX
+    b = canvas.height/2 + canvasOffsetY
+    ctx.clearRect(-a,-b,canvas.width,canvas.height)
+    cross.style.visibility = 'hidden'
     drawings = [];
     angle = 0
+    prev = 0
+    dx = 0
+    dy = 0
     robot.style.transform = 'translate(-50%, -50%) rotate(0deg)';
-    a = canvas.width/2 + canvasOffsetX;
-    b = canvas.height/2 + canvasOffsetY;
-    ctx.clearRect(-a,-b,canvas.width,canvas.height);
 }
 
 function resizeCanvas() {
+    cross.style.visibility = 'hidden'
     canvas.width = window.innerWidth - canvasOffsetX * 2
     canvas.height = window.innerHeight - canvasOffsetY *2
     reset.style.top = canvasOffsetY * 2 + "px";
diff --git a/robot.js b/robot.js
index e67aba4..d3ad0fd 100644
--- a/robot.js
+++ b/robot.js
@@ -1,4 +1,4 @@
-const client = mqtt.connect("ws://marcelus.net:9001");
+const client = mqtt.connect("ws://marcellus.cc:9001");
 
 function wait(ms)
 {
@@ -22,31 +22,22 @@ function wait(ms)
     });
 }
 
-function getTime(dist)
+function getTime(dist,angle)
 {
-    let res = (dist / 200)*1000
-    console.log('time is '+res)
+    let res = (dist / 200)*1500 + 1850 * Math.abs(angle/180)
+    console.log('time is '+res + ",\n supp :" + 1850 *Math.abs(angle/180))
     return res
 }
 
-async function moveTo(x,y)
+async function moveTo(dist,angle)
 {
-    var dist = Math.sqrt(x * x + y * y)
-    console.log('dist is '+dist)
-    var angle = (Math.atan2(y,x))*180/Math.PI
-    if(angle>180)
-    {
-        angle-=180
-    }
-    else if(angle<-180)
-    {
-        angle+=180
-    }
-    console.log(angle.toString())
-    var str = 'T'+(Math.floor(angle)-90).toString()
+    console.log('dist is '+ dist)
+    ang = angle * 180 /Math.PI
+    console.log('ang is :' + ang.toString())
+    var str = 'T'+(Math.floor(ang)).toString()
     client.publish('move',str)
     client.publish('move','D200')
-    const time = getTime(dist)
+    const time = getTime(dist,ang)
     let tmp = await wait(time);
     if (tmp == 0){
         client.publish('move', 'STOP');
diff --git a/robot/main.py b/robot/main.py
index 722a533..5d172a7 100644
--- a/robot/main.py
+++ b/robot/main.py
@@ -28,7 +28,7 @@ UltraSensor = UltrasonicSensor(Port.S4)
 get_tick_rate = 0.1
 send_tick_rate = 0.5
 sensitivity = 400
-safe_distance = 150
+safe_distance = 200
 ultraSend = [True]
 
 
@@ -61,6 +61,7 @@ def sendUltra() :
         publish(dist)
     if (dist < safe_distance) :
         bob.stop()
+        publish('STOP')
 
 
 # MQTT connection
diff --git a/temp.js b/temp.js
deleted file mode 100644
index 1accb19..0000000
--- a/temp.js
+++ /dev/null
@@ -1,181 +0,0 @@
-const canvas = document.getElementById('drawing-board');
-const ctx = canvas.getContext('2d');
-const reset = document.getElementById("reset");
-
-robot = document.getElementById('robot');
-cross = document.getElementById('cross');
-
-const canvasOffsetX = canvas.offsetLeft;
-const canvasOffsetY = canvas.offsetTop;
-
-let scale = 2
-let drawings = []
-let prev = 0
-let angle = 0
-
-let speed = 200 // mm/s
-let dx = 0
-let dy = 0
-let conv = 10
-let animation = 0
-
-canvas.width = window.innerWidth - canvasOffsetX * 2;
-canvas.height = window.innerHeight - canvasOffsetY * 2;
-
-ctx.translate(canvas.width/2 + canvasOffsetX, canvas.height/2 + canvasOffsetY)
-
-reset.style.top = canvasOffsetY * 2 + "px";
-reset.style.right = canvasOffsetX * 2 + "px";
-robot.style.left = (canvas.width/2 + canvasOffsetX)/canvas.width *100 + "%"
-robot.style.top = (canvas.height/2 + canvasOffsetY)/canvas.height *100 + "%"
-
-class Wall {
-    constructor(x,y,a) {
-        this.x = x
-        this.y = y
-        this.h = 5
-        this.a = a
-    }
-
-    draw(context) {
-        context.fillRect(this.x - robot.offsetWidth/2, this.y - this.h/2,robot.offsetWidth,this.h)
-    }
-}
-
-canvas.addEventListener('mousedown', (e) => {
-    if (animation != 0) {
-        cancelAnimationFrame(animation)
-    }
-    cross.style.visibility = "visible";
-
-    x = e.layerX - canvas.width/2 - 2*canvasOffsetX
-    y = e.layerY - canvas.height/2 - 2*canvasOffsetY
-    cross.style.left = e.layerX - canvasOffsetX + "px"
-    cross.style.top = e.layerY - canvasOffsetY + "px"
-
-    prev = angle
-    angle = Math.atan(x/Math.abs(y))
-
-    sym = 1
-    if (x < 0) {
-        sym = -1
-    }
-
-    if (y > 0) {
-        angle = sym*Math.PI - angle
-    }
-
-    robot.style.transform = 'translate(-50%, -50%) rotate('+ angle +'rad)';
-    dist = Math.sqrt(x*x + y*y)
-    t = (dist/speed) * 1000/conv //miss conv
-    dx = x/(t)
-    dy = y/(t)
-    diff = angle - prev
-
-    if(angle * prev < 0 ) {
-        diff = (Math.abs(angle) + Math.abs(prev))
-        if (angle < 0)
-            diff = -diff
-    }
-    console.log("net angle :" + angle * 180 / Math.PI)
-
-    if(diff > Math.PI) {
-        diff = diff - 2*Math.PI
-    }
-    if (diff < -Math.PI) {
-        diff = diff + 2*Math.PI
-    }
-    moveTo(dist,diff)
-    
-    moveAll()
-});
-/*
-client.on('message', function (topic, message) {
-    let val = message.toString();
-    if (val == 'STOP') {
-        dx = 0
-        dy = 0
-    }
-    else {
-        var n = parseInt(val);
-        if (!isNaN(n)){
-            if (n < 150){
-                wall = new Wall(n * Math.sin(angle), - n * Math.cos(angle), angle)
-                drawings.push(wall)
-                draw(wall)
-            }
-        }
-    }
-});*/
-
-
-function test(wall) {
-    drawings.push(wall)
-    draw(wall)
-}
-
-function stop() {
-    dx = 0
-    dy = 0
-    if (animation != 0) {
-        cancelAnimationFrame(animation)
-    }
-}
-
-
-function draw(wall) {
-    ctx.save()
-    x = wall.x
-    y = wall.y
-    wall.x = 0
-    wall.y = 0
-    ctx.translate(x,y)
-    ctx.rotate(wall.a)
-    wall.draw(ctx)
-    wall.x = x
-    wall.y = y
-    ctx.restore()
-}
-
-function moveAll() {
-
-    a = canvas.width/2 + canvasOffsetX
-    b = canvas.height/2 + canvasOffsetY
-    ctx.clearRect(-a,-b,canvas.width,canvas.height)
-
-    drawings.forEach((e) => {
-        e.x -= dx
-        e.y -= dy
-        draw(e) 
-    })
-    animation = requestAnimationFrame(moveAll)
-}
-
-reset.onclick = () => {
-    a = canvas.width/2 + canvasOffsetX
-    b = canvas.height/2 + canvasOffsetY
-    ctx.clearRect(-a,-b,canvas.width,canvas.height)
-    cross.style.visibility = 'hidden'
-    drawings = [];
-    angle = 0
-    prev = 0
-    dx = 0
-    dy = 0
-    robot.style.transform = 'translate(-50%, -50%) rotate(0deg)';
-}
-
-function resizeCanvas() {
-    cross.style.visibility = 'hidden'
-    canvas.width = window.innerWidth - canvasOffsetX * 2
-    canvas.height = window.innerHeight - canvasOffsetY *2
-    reset.style.top = canvasOffsetY * 2 + "px";
-    reset.style.right = canvasOffsetX * 2 + "px";
-    robot.style.left = (canvas.width/2 + canvasOffsetX)/canvas.width *100 + "%"
-    robot.style.top = (canvas.height/2 + canvasOffsetY)/canvas.height *10
-
-    ctx.setTransform(1, 0, 0, 1, 0, 0);
-    ctx.translate(canvas.width/2 + canvasOffsetX, canvas.height/2 + canvasOffsetY)
-    moveAll(0,0)
-}
-
-window.onresize = resizeCanvas;
\ No newline at end of file
diff --git a/temp_robot.js b/temp_robot.js
deleted file mode 100644
index b47ace2..0000000
--- a/temp_robot.js
+++ /dev/null
@@ -1,59 +0,0 @@
-const client = mqtt.connect("ws://marcellus.cc:9001");
-
-function wait(ms)
-{
-    return new Promise((resolve, reject) => {
-        let isObstacle = false;
-
-        const timer = setTimeout(() => {
-            if(!isObstacle){
-                resolve(0);
-            } // Le délai s'est écoulé sans détecter d'obstacle
-        }, ms);
-
-        // Supposons que `client` est un objet qui écoute les messages
-        client.on('message', (topic, message) => {
-            if (topic === 'move' && parseInt(message.toString()) < 150) {
-                isObstacle = true;
-                clearTimeout(timer);
-                resolve(1); // Un obstacle a été détecté
-            }
-        });
-    });
-}
-
-function getTime(dist)
-{
-    let res = (dist / 200)*1000
-    //console.log('time is '+res)
-    return res
-}
-
-async function moveTo(dist,angle)
-{
-    console.log('dist is '+ dist)
-    ang = angle * 180 /Math.PI
-    console.log('ang is :' + ang.toString())
-    var str = 'T'+(Math.floor(ang)).toString()
-    client.publish('move',str)
-    client.publish('move','D200')
-    const time = getTime(dist)
-    let tmp = await wait(time);
-    if (tmp == 0){
-        client.publish('move', 'STOP');
-    }
-    stop()
-    
-}
-
-client.on('connect', () => {
-    console.log('Connecté au courtier MQTT');
-    client.subscribe('move')
-});
-// client.publish('move','ULT1')
-
-
-/* client.on('close', function () {
-    console.log('Déconnecté du courtier MQTT');
-});*/
-