diff --git a/examples/bb8joystick/__init__.py b/examples/bb8joystick/__init__.py index d742022..6767d4e 100644 --- a/examples/bb8joystick/__init__.py +++ b/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: diff --git a/examples/bb8joystick/bb8.py b/examples/bb8joystick/bb8.py index 1625a36..10b28a5 100644 --- a/examples/bb8joystick/bb8.py +++ b/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() diff --git a/examples/bb8joystick/joystick.py b/examples/bb8joystick/joystick.py index 45aa120..beefebc 100644 --- a/examples/bb8joystick/joystick.py +++ b/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)