Keep researching BB8 and joyystick

examples/bb8joystick/__init__.py

@@ -13,22 +13,40 @@ if __name__ == "__main__":
     joystick = Joystick()
 
 
-    def set_bb_color(r, g, b):
-        print("Color", r, g, b)
-        bb8.color(r, g, b)
+    def set_bb_color(flag):
+        if flag:
+            bb8.color(255, 255, 255)
+        else:
+            bb8.color(0, 0, 0)
 
 
     def set_heading(angle):
         a = int(angle) % 360
         if a < 0:
-            a = 360 - a
+            a = 359 - a
         print("Angle", a)
         bb8.heading(a)
 
 
+    def roll(speed, direction):
+        print("Roll", speed, direction)
+        if speed < 0.1:
+            speed = 0
+        bb8.roll(speed, direction)
+
+
+    def stop(state):
+        if not state:
+            print("Stop")
+            bb8.roll(0, 0)
+            bb8.stabilize()
+
+
     try:
-        #joystick.on_color_sensor(set_bb_color)
-        joystick.on_external_motor(set_heading)
+        joystick.on_button(set_bb_color)
+        joystick.on_button(stop)
+        joystick.on_rotation(set_heading)
+        joystick.on_joystick(roll)
         print("All set up")
         time.sleep(600)
     finally:

examples/bb8joystick/bb8.py

@@ -56,10 +56,10 @@ class BB8(object):
         print("Started to wake up BB-8...")
         self._sphero = _SpheroImproved()
         self._loop.run_until_complete(self._sphero.connect(num_retry_attempts=3, use_ble=True, search_name=name))
-        # self._loop.run_until_complete(self._sphero.set_stabilization(True))
-        # self._loop.run_until_complete(self._sphero.set_rotation_rate(1))
+        self._loop.run_until_complete(self._sphero.set_stabilization(True))
+        self._loop.run_until_complete(self._sphero.set_rotation_rate(1))
         self.color(0, 0xFF, 0)
-        # self.stabilize()
+        self.stabilize()
         print("BB-8 is ready for commands")
 
     def disconnect(self):
@@ -72,13 +72,13 @@ class BB8(object):
 
     def heading(self, heading):
         self._wait_loop()
-        self._loop.run_until_complete(self._sphero.roll(1, heading, spheropy.RollMode.IN_PLACE_ROTATE))
-        # self._loop.run_until_complete(self._sphero.set_heading(heading))
+        self._loop.run_until_complete(self._sphero.set_heading(heading))
+        self._loop.run_until_complete(self._sphero.roll(1, 0, spheropy.RollMode.IN_PLACE_ROTATE))
 
-    def roll(self, speed=1.0):
+    def roll(self, speed=1.0, direction=0):
         self._wait_loop()
         speed = int(255 * speed)
-        self._loop.run_until_complete(self._sphero.roll(speed, 0))
+        self._loop.run_until_complete(self._sphero.roll(speed, direction))
 
     def stop(self):
         self._wait_loop()

examples/bb8joystick/joystick.py

@@ -1,4 +1,6 @@
 import logging
+import math
+import sys
 import time
 
 from pylgbst.hub import MoveHub
@@ -11,6 +13,7 @@ def _clamp(minvalue, value, maxvalue):
 class Joystick(object):
     def __init__(self):
         super(Joystick, self).__init__()
+        self._on_joystick = set()
         self._hub = MoveHub()
 
         self._reset_sensors()
@@ -18,14 +21,14 @@ class Joystick(object):
         self.button_pressed = False
         self._hub.button.subscribe(self._on_btn)
 
-        self.angle_A = 0
-        self.on_motor_a(self._on_a)
+        self._angle_A = 0
+        self._on_motor_a(self._on_a)
 
         self.angle_B = 0
-        self.on_button(self._on_b)
+        self.on_rotation(self._on_b)
 
-        self.angle_C = 0
-        self.on_motor_a(self._on_c)
+        self._angle_C = 0
+        self._on_motor_c(self._on_c)
 
         logging.info("Done initializing")
 
@@ -49,20 +52,16 @@ class Joystick(object):
 
         self._hub.button.subscribe(wrapper)
 
-    def on_motor_a(self, callback):
-        """
-        Notifies about A motor rotation. ``callback(state)`` gets single int parameter from 0 to 359
-        """
-
+    def _on_motor_a(self, callback):
         def wrapper(angle):
             logging.debug("Raw angle: %s", angle)
-            range = 25
-            angle = _clamp(-range, angle, range)
+            spread = 25
+            angle = _clamp(-spread, angle, spread)
             callback(angle)
 
         self._hub.motor_A.subscribe(wrapper)
 
-    def on_motor_b(self, callback):
+    def on_rotation(self, callback):
         """
         Notifies about B motor rotation. ``callback(state)`` gets single int parameter from 0 to 359
         """
@@ -74,15 +73,11 @@ class Joystick(object):
 
         self._hub.motor_B.subscribe(wrapper)
 
-    def on_motor_c(self, callback):
-        """
-        Notifies about C motor rotation. ``callback(state)`` gets single int parameter from 0 to 359
-        """
-
+    def _on_motor_c(self, callback):
         def wrapper(angle):
             logging.debug("Raw angle: %s", angle)
-            range = 25
-            angle = _clamp(-range, angle, range)
+            spread = 25
+            angle = _clamp(-spread, angle, spread)
             callback(angle)
 
         self._hub.motor_external.subscribe(wrapper)
@@ -92,7 +87,8 @@ class Joystick(object):
 
     def _on_a(self, angle):
         logging.debug("A rotated: %s", angle)
-        self.angle_A = angle
+        self._angle_A = angle
+        self._calc_joystick()
 
     def _on_b(self, angle):
         logging.debug("B rotated: %s", angle)
@@ -100,14 +96,50 @@ class Joystick(object):
 
     def _on_c(self, angle):
         logging.debug("C rotated: %s", angle)
-        self.angle_C = angle
+        self._angle_C = angle
+        self._calc_joystick()
+
+    def on_joystick(self, callback):
+        """
+        Notifies about joystick change. ``callback(speed, direction)`` gets parameters:
+        - ``speed`` - float value from 0.0 to 1.0
+        - ``direction`` - int value from 0 to 359
+
+        """
+        self._on_joystick.add(callback)
+
+    def _calc_joystick(self):
+        norm_a = self._angle_A / 25
+        norm_b = self._angle_C / 25
+        logging.info("%s / %s", self._angle_A, self._angle_C)
+        logging.info("%s / %s", norm_a, norm_b)
+        speed = math.sqrt(norm_a ** 2 + norm_b ** 2) / math.sqrt(2)
+        speed = _clamp(-1.0, speed, 1.0)
+
+        maxsize = sys.maxsize if norm_a >= 0 else -sys.maxsize
+        direction = math.atan(norm_a / norm_b if norm_b else maxsize)
+        direction *= 180 / math.pi
+        if norm_a >= 0 and norm_b >= 0:
+            logging.info("Sector 1")
+            direction = 90 - direction
+        elif norm_a < 0 and norm_b >= 0:
+            logging.info("Sector 2")
+            direction = 90 - direction
+        elif norm_a < 0 and norm_b < 0:
+            logging.info("Sector 3")
+            direction = 270 - direction
+        else:
+            logging.info("Sector 4")
+            direction = 270 - direction
+
+        for callback in self._on_joystick:
+            callback(speed, 359 - int(direction))
 
 
 if __name__ == '__main__':
-    logging.basicConfig(level=logging.DEBUG)
+    logging.basicConfig(level=logging.INFO)
     stick = Joystick()
-    stick.on_button(lambda x: print("Button: %s" % x))
-    stick.on_motor_a(lambda x: print("Motor A: %s" % x))
-    stick.on_motor_b(lambda x: print("Motor B: %s" % x))
-    stick.on_motor_c(lambda x: print("Motor C: %s" % x))
+    stick.on_button(lambda x: logging.info("Button: %s" % x))
+    stick.on_rotation(lambda x: logging.info("Motor B: %s" % x))
+    stick.on_joystick(lambda speed, head: logging.info("Speed: %.2f, dir: %s" % (speed, head)))
     time.sleep(100)