parser python

main.cpp

@@ -29,7 +29,7 @@ int abs(int in){
 
 void pinMode(int pin, int mode){
   printf("set pin %d in mode %d\n", pin, mode);
-  printf("$pm,%d,%d",pin, mode);
+  printf("$pm,%d,%d;",pin, mode);
 }
 
 int analogRead(int pin){
@@ -38,12 +38,12 @@ int analogRead(int pin){
 
 void digitalWrite(int pin, int level){
   printf("set pin %d at level %d\n", pin, level);
-  printf("$dw,%d,%d", pin, level);
+  printf("$dw,%d,%d;", pin, level);
 }
 
 void analogWrite(int pin, int dc){
   printf("set pin %d at duty cycle %d\n", pin, dc);
-  printf("$aw,%d,%d", pin, dc);
+  printf("$aw,%d,%d;", pin, dc);
 }
 
 void serial_c::begin(int br){

parser.py

@@ -0,0 +1,43 @@
+class aip_parser:
+    def digitalWrite(self, args):
+        if len(args) != 2:
+            return
+        self.robot.digitalWrite(*map(int, args))
+    
+    def analogWrite(self, args):
+        if len(args) != 2:
+            return
+        self.robot.analogWrite(*map(int, args))
+        
+    cmds = {
+        "dw" : digitalWrite,
+        "aw" : analogWrite
+    }
+        
+    def __init__(self, stream, robot):
+        self.stream = stream
+        self.cmd_buff = ""
+        self.robot = robot
+    
+    def poll(self):
+        c=self.stream.read(1)
+        if len(c)==0:
+            print("end")
+            return False
+        if len(self.cmd_buff)==0:
+            if c != b'$':
+                return True
+            self.cmd_buff = "_"
+        else:
+            if c == b';':
+                self.parse_line()
+            else:
+                self.cmd_buff += c.decode()
+        return True
+    
+    def parse_line(self):
+        cmd = self.cmd_buff[1:].split(',')
+        cmdFct = self.cmds.get(cmd[0])
+        if cmdFct:
+            cmdFct(self, cmd[1:])
+        self.cmd_buff = ""

robot.py

@@ -15,17 +15,29 @@ class Robot:
     l=0.1
     # time
     t = 0
+    # pins level
+    pins = [0]*16
+    # acceleration maximal avant de perdre l'adhérance
+    maxRotAccel = 10
 
-    def __init__(self):
+    def __init__(self, dt):
-        print("yo")
+        self.dt = dt
         
-    def update(self, dt):
+    def update(self):
-        self.t += dt
+        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])
-        self.rot += self.angv*dt
+        self.rot += self.angv*self.dt
-        self.pos = (self.pos[0]+self.v*sin(self.rot)*dt,self.pos[1]+self.v*cos(self.rot)*dt)
+        self.pos = (self.pos[0]+self.v*sin(self.rot)*self.dt,self.pos[1]+self.v*cos(self.rot)*self.dt)
-        if(self.pos[0]>=10):
+        print(self.pins)
-            print(self.t)
+            
-            sys.exit()
+    def analogWrite(self, pin, dc):
+        print(f"set pin {pin} to {dc}")
+        speedDelta = (self.pins[pin] - dc) / self.dt
+        assert(speedDelta < self.maxRotAccel)
+        self.pins[pin] = dc
+        
+    def digitalWrite(self, pin, level):
+        self.analogWrite(pin, level*255)
+        

simulateur.py

@@ -1,4 +1,7 @@
 from robot import Robot
+from parser import aip_parser
+
+from subprocess import Popen, PIPE
 
 from math import degrees
 import sys, pygame
@@ -10,7 +13,11 @@ black = 0, 0, 0
 
 screen = pygame.display.set_mode(size)
 
-r = Robot()
+r = Robot(1/60)
+
+ard_process = Popen(["build/app"], stdin=PIPE, stdout=PIPE);
+
+parser = aip_parser(ard_process.stdout, r);
 
 while True:
     for event in pygame.event.get():
@@ -18,9 +25,11 @@ while True:
         if event.type == pygame.KEYDOWN:
             if event.key == K_q:
                 sys.exit()
-    
+                
+    while(parser.poll()):
+        pass
     screen.fill(black)
-    r.update(1/60)
+    r.update()
     robotShape = pygame.Rect(0,0, 10,20);
     rsurf = pygame.Surface((10,20)).convert_alpha();
     pygame.draw.rect(rsurf, (255,255,255), robotShape);