real-time control of nexus robot

nexus.py

@@ -1,63 +1,160 @@
 import serial
 import time
 
+
 class NexusRobot:
+    '''
+    Nexus Robot Library for moving
+    '''
+    def __init__(self, port):
+        self.ser = serial.serial_for_url(port, 9600, do_not_open=True)
+        self.ser.open()
+        self.pause(3)
+        self._spd_calib = 1
+        self._ang_calib = 1
+        self._dis_calib = 1
 
-  def __init__(self, port):
+    def _calib_spd(self, speed):
-    self.ser = serial.serial_for_url(port, 9600, do_not_open = True)
+        return int(speed * self._spd_calib)
-    self.ser.open()
-    time.sleep(3)
 
-  def _wait(self):
+    def _calib_ang(self, angle):
-    self.ser.readline()
+        return int(angle * self._ang_calib)
-    self.ser.readline()
 
-  def pause(self):
+    def _calib_dis(self, distance):
-    time.sleep(1)
+        return int(distance * self._dis_calib)
 
-  def forward(self, speed, dist):
+    def _wait(self):
-    self.ser.write("g%d,%d;" % (speed, dist))
+        self.ser.readline()
-    self._wait()
+        self.ser.readline()
 
-  def reverse(self, speed, dist):
+    def pause(self, duration=1):
-    self.ser.write("b%d,%d;" % (speed, dist))
+        time.sleep(duration)
-    self._wait()
 
-  def turnRight(self, speed, angle):
+    def forward(self, speed, dist):
-    self.ser.write("rr%d,%d;" % (speed, angle))
+        self.ser.write(
-    self._wait()
+            "g{0},{1};".format(self._calib_spd(speed), self._calib_dis(dist))
+        )
+        if dist > 0:
+            self._wait()
 
-  def turnLeft(self, speed, angle):
+    def backward(self, speed, dist):
-    self.ser.write("rl%d,%d;" % (speed, angle))
+        self.ser.write(
-    self._wait()
+            "b{0},{1};".format(self._calib_spd(speed), self._calib_dis(dist))
+        )
+        if dist > 0:
+            self._wait()
 
-  def stop(self, speed=0):
+    def turnRight(self, speed, angle=-1):
-    self.ser.write("s%d;" % (speed))
+        self.ser.write(
-    self._wait()
+            "rr{0},{1};".format(self._calib_spd(speed), self._calib_ang(angle))
+        )
+        if angle > 0:
+            self._wait()
+
+    def turnLeft(self, speed, angle=-1):
+        self.ser.write(
+            "rl{0},{1};".format(self._calib_spd(speed), self._calib_ang(angle))
+        )
+        if angle > 0:
+            self._wait()
+
+    def rotateLeft(self, speed, angle=-1):
+        self.ser.write(
+            "r-{0},{1},-1;".format(
+                self._calib_spd(speed), self._calib_spd(speed))
+        )
+        # self._wait()
+
+    def rotateRight(self, speed, angle=-1):
+        self.ser.write(
+            "r{0},-{1},-1;".format(
+                self._calib_spd(speed), self._calib_spd(speed))
+        )
+        # self._wait()
+
+    def stop(self, speed=0):
+        self.ser.write("s{0};".format(self._calib_spd(speed)))
+
+    def getInfo(self):
+        self.ser.write("h;")
+        actual_l_speed = int(self.ser.readline().rstrip().split(':')[-1])
+        target_l_speed = int(self.ser.readline().rstrip().split(':')[-1])
+        actual_r_speed = int(self.ser.readline().rstrip().split(':')[-1])
+        target_r_speed = int(self.ser.readline().rstrip().split(':')[-1])
+        return actual_l_speed, target_l_speed, actual_r_speed, target_r_speed
 
 
+if __name__ == '__main__':
+    import sys
+    import tty
+    import termios
 
-if __name__ == '__main__'':
+    class _Getch:
-  try:
+        def __call__(self):
-    robot = NexusRobot('/dev/cu.usbserial-AL00YYCA')
+                fd = sys.stdin.fileno()
-    while 1:
+                old_settings = termios.tcgetattr(fd)
-      robot.forward(200,20)
+                try:
-      robot.pause()
+                    tty.setraw(sys.stdin.fileno())
-      robot.reverse(200,20)
+                    ch = sys.stdin.read(1)
-      robot.pause()
+                finally:
-#     robot.forward(200,10)
+                    termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
-#     robot.pause()
+                return ch
-#     robot.turnRight(200,90)
+
-#     robot.pause()
+    def get():
-#     robot.forward(200,10)
+        inkey = _Getch()
-#     robot.pause()
+        while 1:
-#     robot.turnRight(200,90)
+            k = inkey()
-#     robot.pause()
+            if k != '':
-#     robot.forward(200,10)
+                break
-#     robot.pause()
+        return k
-#     robot.turnRight(200,90)
+
-#     robot.pause()
+    robot = None
-  except KeyboardInterrupt:
+    speed = 100
-    robot.stop()
+    try:
-  except Exception:
+        print('Initializing..')
-    raise
+        robot = NexusRobot('/dev/cu.usbserial-AL00YYCA')
+        print('Initialized')
+        prev_k = None
+        while 1:
+            k = get()
+            if k == 'w' and prev_k != k:
+                print('Going Forward')
+                robot.forward(speed, -1)
+            elif k == 's' and prev_k != k:
+                print('Going Backward')
+                robot.backward(speed, -1)
+            elif k == 'a' and prev_k != k:
+                print('Turing left')
+                robot.rotateLeft(speed, -1)
+            elif k == 'd' and prev_k != k:
+                print('Turing right')
+                robot.rotateRight(speed, -1)
+            elif k == ' ':
+                print('Stopping')
+                robot.stop()
+            elif k == 'h':
+                print('Printing info..')
+                print(robot.getInfo())
+            elif k == '+':
+                speed += 20
+                if speed > 200:
+                    speed = 200
+                print('Config speed to: {0}'.format(speed))
+            elif k == '-':
+                speed -= 20
+                if speed < 20:
+                    speed = 20
+                print('Config speed to: {0}'.format(speed))
+            elif k == '\x03':
+                raise KeyboardInterrupt
+            else:
+                print('unrecognized key: {0}'.format(k))
+            prev_k = k
+            # robot.pause()
+    except serial.serialutil.SerialException, e:
+        print(str(e))
+    except KeyboardInterrupt:
+        if robot:
+            robot.stop()
+    except Exception:
+        raise