From 9441662d95cd8757c7999ad128506fc76ecfef40 Mon Sep 17 00:00:00 2001
From: leo <leo.lemaire007@orange.fr>
Date: Wed, 23 Nov 2022 12:23:40 +0100
Subject: [PATCH] flow control

---
 adapter_ard.h |  1 +
 main.cpp      | 11 ++++++++++-
 parser.py     | 10 +++++++++-
 robot.py      | 23 ++++++++++++++++-------
 simulateur.py |  9 +++++----
 5 files changed, 41 insertions(+), 13 deletions(-)

diff --git a/adapter_ard.h b/adapter_ard.h
index b93396f..8bad6c6 100644
--- a/adapter_ard.h
+++ b/adapter_ard.h
@@ -5,6 +5,7 @@
 void loop();
 void setup();
 long millis();
+long micros();
 int abs(int in);
 void pinMode(int pin, int mode);
 int analogRead(int pin);
diff --git a/main.cpp b/main.cpp
index d2375fb..04b3162 100644
--- a/main.cpp
+++ b/main.cpp
@@ -33,8 +33,10 @@ int main(int argc, char** argv){
       }
       cmd_buff[cmd_buff_ind++] = c;
     }
+    printf("$dt,%ld;", micros());
     loop();
-    printf("millis %ld\n", millis());
+    printf("*");
+    fflush(stdout);
   }
 }
 
@@ -45,6 +47,13 @@ long millis(){
   return milli;
 }
 
+long micros(){
+  clock_gettime(CLOCK_MONOTONIC_RAW, &end);
+  uint64_t milli = (end.tv_sec - start.tv_sec) * 1000000 + (end.tv_nsec - start.tv_nsec) / 1000;
+  
+  return milli;
+}
+
 int abs(int in){
   return abs(in);
 }
diff --git a/parser.py b/parser.py
index 273986e..fb2f6b5 100644
--- a/parser.py
+++ b/parser.py
@@ -9,9 +9,15 @@ class aip_parser:
             return
         self.robot.analogWrite(*map(int, args))
         
+    def updateDt(self, args):
+        if len(args) != 1:
+            return
+        self.robot.setTime(int(args[0]))
+        
     cmds = {
         "dw" : digitalWrite,
-        "aw" : analogWrite
+        "aw" : analogWrite,
+        "dt" : updateDt,
     }
         
     def __init__(self, stdin, stdout, robot):
@@ -23,6 +29,8 @@ class aip_parser:
     def poll(self):
         c=self.stdout.read(1)
         if not c:
+            return True
+        if c == b'*':
             return False
         try:
             print(c.decode(), end='')
diff --git a/robot.py b/robot.py
index d7e885d..4ea9f62 100644
--- a/robot.py
+++ b/robot.py
@@ -2,9 +2,9 @@ from math import cos, sin, pi, atan2, tan
 
 class Robot:
     # position
-    pos=(1,2)
+    pos=(1,1.5)
     # orientation
-    rot=0
+    rot=-1
     # velocite
     v=0
     # velocite angulaire
@@ -12,7 +12,7 @@ class Robot:
     # velocite des roues (non angulaire)
     wv=[0,0]
     # velocite max
-    wmv=0.01
+    wmv=0.1
     # espace entre les deux roues
     l=0.1
     # time
@@ -28,7 +28,7 @@ class Robot:
     # ecart entre les deux photores
     photores_l = 0.1
     # longeur du séparateur entre les photorésistances
-    photores_sep_l = 0.1
+    photores_sep_l = 0.05
     # constantes de résistance des photoresistances
     PHOTORES_LIGHT = 100
     PHOTORES_SHADOW = 20000
@@ -36,13 +36,17 @@ class Robot:
     pr_l_pin = 0 # A0 voir adapter_ard.h
     pr_r_pin = 1 # A1
 
-    def __init__(self, dt):
-        self.dt = dt
+    def __init__(self):
         self.pr_max_ang = tan((self.photores_l/2)/self.photores_sep_l)
+        self.t = 0
+        self.dt = -1
+        self.angle2spot = 0
         
     def update(self):
+        if(self.dt == -1):
+            return
         self.compute_light()
-        self.t += self.dt
+        print("***********", self.t, "****", self.dt)
         # https://robotics.stackexchange.com/questions/106/what-is-a-suitable-model-for-two-wheeled-robots
         self.v = 1/2*(self.wv[0]+self.wv[1])
         self.angv = 1/self.l*(self.wv[1]-self.wv[0])
@@ -81,4 +85,9 @@ class Robot:
         
     def digitalWrite(self, pin, level):
         self.analogWrite(pin, level*255)
+        
+    def setTime(self, t):
+        self.dt = (t/1000000 - self.t)
+        self.t = t/1000000
+        assert(self.dt < 0.1)
         
\ No newline at end of file
diff --git a/simulateur.py b/simulateur.py
index 3569bf5..02bfc86 100644
--- a/simulateur.py
+++ b/simulateur.py
@@ -4,6 +4,7 @@ from parser import aip_parser
 from subprocess import Popen, PIPE
 import fcntl
 import os
+from time import sleep
 
 # Pygame
 from math import degrees, cos, sin
@@ -17,7 +18,7 @@ black = 0, 0, 0
 screen = pygame.display.set_mode(size)
 
 # le robot
-r = Robot(1/60)
+r = Robot()
 
 # on lance le programme arduino
 ard_process = Popen(["build/app"], stdin=PIPE, stdout=PIPE);
@@ -42,11 +43,12 @@ while True:
     
     # On demarre un tour de simulation
     ard_process.stdin.write(b'@')
-    
+    ard_process.stdin.flush()
+
     # tant qu'il y as qqu chose sur le bus on le parse
     while(parser.poll()):
         pass
-    
+        
     # on met à jour la simulation    
     r.update()
     
@@ -66,4 +68,3 @@ while True:
     center = rsurf_r.get_rect().center
     screen.blit(rsurf_r, (r.pos[0]*100-center[0],r.pos[1]*100-center[1]))
     pygame.display.flip()
-