Added IMU client to drive controller, as well as minor changes

Author: aolkin

controller/drive.py

@@ -3,13 +3,52 @@
 SLIGHT_TIME = 0.1
 TURN_TIME = 0.75
 
+class IMUProtocol(asyncio.Protocol):
+    def __init__(self, up):
+        self.up = up
+
+    def data_received(self, data):
+        text = data.decode().strip().split()[-1]
+        rgbc = tuple([int(i) for i in text.split(",")])
+        self.up._received(rgbc)
+
+class IMU:
+    def __init__(self):
+        self.loop = asyncio.get_event_loop()
+        self.data = (0,0,0)
+        self.initial = False
+        self._mark = 0
+        self.loop.run_until_complete(self.connect())
+
+    @asyncio.coroutine
+    def connect(self):
+        yield from self.loop.connect_read_pipe(
+            lambda up=self: IMUProtocol(up), open("/var/run/euler0"))
+        
+    def _received(self, euler):
+        self.data = euler
+        self.heading = euler[0]
+        if not initial:
+            self.initial = euler
+
+    def mark(self):
+        self._mark = self.heading
+            
+    @property
+    def from_start(self):
+        return self.heading - self.initial[0]
+    @property
+    def from_mark(self):
+        return self.heading - self._mark
+
 class DriveMotors:
     def __init__(self, left=0, right=1):
         self.loop = asyncio.get_event_loop()
         self._left = open("/var/run/motor{}".format(left),"w")
         self._right = open("/var/run/motor{}".format(right),"w")
         self.__left = 0
         self.__right = 0
+        self.imu = IMU()
         self.loop.run_until_complete(self.connect())
         self.__handle = None
 
@@ -25,7 +64,7 @@ def left(self):
     @left.setter
     def left(self, val):
         self.cancel()
-        val = val * 0.999
+        #val = val * 0.999
         self.__left = val
         #if val == 0:
             #traceback.print_stack()
@@ -38,50 +77,40 @@ def right(self):
     @right.setter
     def right(self, val):
         self.cancel()
-        val = val * 0.9
+        #val = val * 0.9
         self.__right = val
         self._right.write("{}\n".format(val).encode())
 
     def cancel(self):
         if self.__handle: self.__handle.cancel()
 
+    def __set(self, right, left, t=None):
+        self.imu.mark()
+        self.right = right
+        self.left = left
+        if t:
+            self.__handle = self.loop.call_later(t, self.forward)
+        
     def stop(self):
-        self.right = 0
-        self.left = 0
+        self.__set(0, 0)
 
     def forward(self):
-        self.right = -1
-        self.left = 1
+        self.__set(-1, 1)
 
     def backward(self, t=None):
-        self.right = 1
-        self.left = -1
-        if t:
-            self.__handle = self.loop.call_later(t, self.forward)
+        self.__set(1, -1, t)
 
     def turnright(self, t=TURN_TIME):
-        self.right = 1
-        self.left = 1
-        if t:
-            self.__handle = self.loop.call_later(t, self.forward)
+        self.__set(1, 1, t)
 
     def turnleft(self, t=TURN_TIME):
-        self.right = -1
-        self.left = -1
-        if t:
-            self.__handle = self.loop.call_later(t, self.forward)
+        self.__set(-1, -1, t)
 
     def slightright(self, t=SLIGHT_TIME):
         print("Slight right")
-        self.right = -0.09
-        self.left = 1
-        if t:
-            self.__handle = self.loop.call_later(t, self.forward)
+        self.__set(-0.09, 1, t)
 
     def slightleft(self, t=SLIGHT_TIME):
         print("Slight left")
-        self.right = -1
-        self.left = 0.09
-        if t:
-            self.__handle = self.loop.call_later(t, self.forward)
+        self.__set(-1, 0.09, t)
 

debug_control.py

@@ -39,7 +39,7 @@ def display_iterable(scr, iterable, start, x=10, name=None):
     for i in iterable:
         curses.setsyx(start, x)
         scr.clrtoeol()
-        scr.addstr(start, x, str(i))
+        scr.addstr(start, x, str(round(i,2)))
         start += 1
     curses.setsyx(sy, sx)
     scr.refresh()
@@ -76,6 +76,7 @@ def main():
         key = scr.getch()
 
         if key == Q:
+            dm.stop()
             break
 
         {