Ajout code Antonin

1 files changed, 64 insertions, 0 deletions

diff --git a/robot.js b/robot.js
new file mode 100644
index 0000000..5c259fe
--- /dev/null
+++ b/robot.js
@@ -0,0 +1,64 @@
+const mqtt = require("mqtt");
+const client = mqtt.connect("mqtt://marcelus.net");
+
+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(x,y)
+{
+    var dist = Math.sqrt(x * x + y * y)
+    console.log('dist is '+dist)
+    var angle = (Math.atan2(y,x))*180/Math.PI
+    console.log(angle.toString())
+    var str = 'T'+(Math.floor(angle)-90).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');
+    }
+    
+}
+
+client.on('connect', function () {
+    console.log('Connecté au courtier MQTT');
+    client.subscribe('move');
+    client.publish('move','ULT1')
+});
+
+client.on('message', function (topic, message) {
+    console.log('Received from ', topic, ' : ', message.toString());
+});
+
+client.on('close', function () {
+    console.log('Déconnecté du courtier MQTT');
+});
+
+moveTo(1000,1000)
\ No newline at end of file