From c9112d8fe4232b633a0057a8bf210f9e8412627b Mon Sep 17 00:00:00 2001 From: Andrey Pokhilko Date: Wed, 14 Aug 2019 23:29:08 +0300 Subject: [PATCH] Progressing --- examples/bb8joystick/__init__.py | 8 ++--- examples/bb8joystick/bb8.py | 5 ++-- examples/bb8joystick/joystick.py | 50 +++++++++++++++----------------- pylgbst/hub.py | 2 +- 4 files changed, 32 insertions(+), 33 deletions(-) diff --git a/examples/bb8joystick/__init__.py b/examples/bb8joystick/__init__.py index 6767d4e..6f2ba89 100644 --- a/examples/bb8joystick/__init__.py +++ b/examples/bb8joystick/__init__.py @@ -22,8 +22,6 @@ if __name__ == "__main__": def set_heading(angle): a = int(angle) % 360 - if a < 0: - a = 359 - a print("Angle", a) bb8.heading(a) @@ -48,7 +46,9 @@ if __name__ == "__main__": joystick.on_rotation(set_heading) joystick.on_joystick(roll) print("All set up") - time.sleep(600) + + while joystick._hub.connection.is_alive(): + time.sleep(1) finally: - joystick.disconnect() bb8.disconnect() + joystick.disconnect() diff --git a/examples/bb8joystick/bb8.py b/examples/bb8joystick/bb8.py index 10b28a5..0e05126 100644 --- a/examples/bb8joystick/bb8.py +++ b/examples/bb8joystick/bb8.py @@ -63,6 +63,7 @@ class BB8(object): print("BB-8 is ready for commands") def disconnect(self): + print("BB8 enters sleep") self._loop.run_until_complete(self._sphero.sleep(0)) self._sphero.disconnect() @@ -72,8 +73,8 @@ class BB8(object): def heading(self, heading): self._wait_loop() - 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)) + # self._loop.run_until_complete(self._sphero.set_heading(heading)) + self._loop.run_until_complete(self._sphero.roll(1, heading, spheropy.RollMode.IN_PLACE_ROTATE)) def roll(self, speed=1.0, direction=0): self._wait_loop() diff --git a/examples/bb8joystick/joystick.py b/examples/bb8joystick/joystick.py index beefebc..256faf5 100644 --- a/examples/bb8joystick/joystick.py +++ b/examples/bb8joystick/joystick.py @@ -11,28 +11,29 @@ def _clamp(minvalue, value, maxvalue): class Joystick(object): + RANGE_A = 40 + RANGE_C = 30 + def __init__(self): super(Joystick, self).__init__() self._on_joystick = set() - self._hub = MoveHub() - - self._reset_sensors() - self.button_pressed = False - self._hub.button.subscribe(self._on_btn) - self._angle_A = 0 - self._on_motor_a(self._on_a) - self.angle_B = 0 - self.on_rotation(self._on_b) - self._angle_C = 0 + + print("Starting search for Joystick...") + self._hub = MoveHub() + self._reset_sensors() + self._hub.button.subscribe(self._on_btn) + self._on_motor_a(self._on_a) + self.on_rotation(self._on_b) self._on_motor_c(self._on_c) - logging.info("Done initializing") + print("Joystick is ready") def disconnect(self): + print("Joystick disconnects") self._hub.disconnect() def _reset_sensors(self): @@ -55,8 +56,7 @@ class Joystick(object): def _on_motor_a(self, callback): def wrapper(angle): logging.debug("Raw angle: %s", angle) - spread = 25 - angle = _clamp(-spread, angle, spread) + angle = _clamp(-self.RANGE_A, angle, self.RANGE_A) callback(angle) self._hub.motor_A.subscribe(wrapper) @@ -69,15 +69,14 @@ class Joystick(object): def wrapper(angle): logging.debug("Raw angle: %s", angle) val = angle % 360 - callback(val if val >= 0 else 360 - val) + callback(val if val >= 0 else 360 - val - 1) self._hub.motor_B.subscribe(wrapper) def _on_motor_c(self, callback): def wrapper(angle): logging.debug("Raw angle: %s", angle) - spread = 25 - angle = _clamp(-spread, angle, spread) + angle = _clamp(-self.RANGE_C, angle, self.RANGE_C) callback(angle) self._hub.motor_external.subscribe(wrapper) @@ -109,37 +108,36 @@ class Joystick(object): 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) + norm_a = self._angle_A / self.RANGE_A + norm_b = self._angle_C / self.RANGE_C + logging.debug("%s / %s", self._angle_A, self._angle_C) + logging.debug("%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)) + callback(speed, 360 - int(direction)) if __name__ == '__main__': logging.basicConfig(level=logging.INFO) + stick = Joystick() + 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) diff --git a/pylgbst/hub.py b/pylgbst/hub.py index c8ec0cb..f3ddf2a 100644 --- a/pylgbst/hub.py +++ b/pylgbst/hub.py @@ -221,7 +221,7 @@ class MoveHub(Hub): if not get_dev_set: get_dev_set = lambda: (self.motor_A, self.motor_B, self.motor_AB, self.led, self.tilt_sensor, self.current, self.voltage) - for num in range(0, 60): + for num in range(0, 100): devices = get_dev_set() if all(devices): log.debug("All devices are present: %s", devices)