diff --git a/main.cpp b/main.cpp index b1adde9..e0fc0a5 100644 --- a/main.cpp +++ b/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){ diff --git a/parser.py b/parser.py new file mode 100644 index 0000000..d382c7f --- /dev/null +++ b/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 = "" \ No newline at end of file diff --git a/robot.py b/robot.py index ff8e9a2..acb85df 100644 --- a/robot.py +++ b/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): - print("yo") + def __init__(self, dt): + self.dt = dt - def update(self, dt): - self.t += dt + def update(self): + 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.pos = (self.pos[0]+self.v*sin(self.rot)*dt,self.pos[1]+self.v*cos(self.rot)*dt) - if(self.pos[0]>=10): - print(self.t) - sys.exit() \ No newline at end of file + self.rot += self.angv*self.dt + self.pos = (self.pos[0]+self.v*sin(self.rot)*self.dt,self.pos[1]+self.v*cos(self.rot)*self.dt) + print(self.pins) + + 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) + \ No newline at end of file diff --git a/simulateur.py b/simulateur.py index f3aff55..7e1c13b 100644 --- a/simulateur.py +++ b/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);