diff --git a/demo.py b/demo.py index 71936bd..fccaebb 100644 --- a/demo.py +++ b/demo.py @@ -176,7 +176,7 @@ if __name__ == '__main__': # time.sleep(1) # mtr.stop() - - mtr.constant(0.3, -0.5) + mtr.test() time.sleep(3) mtr.stop() + time.sleep(1) diff --git a/pylgbst/comms.py b/pylgbst/comms.py index cb7f36f..6d0613b 100644 --- a/pylgbst/comms.py +++ b/pylgbst/comms.py @@ -158,7 +158,7 @@ class DebugServer(object): self.sock.close() def _notify_dummy(self, handle, data): - log.debug("Dropped notification from handle %s: %s", handle, binascii.hexlify(data.strip())) + log.debug("Dropped notification from handle %s: %s", handle, binascii.hexlify(data)) self._check_shutdown(data) def _notify(self, conn, handle, data): diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py index 5655be5..0d56e90 100644 --- a/pylgbst/peripherals.py +++ b/pylgbst/peripherals.py @@ -134,6 +134,8 @@ class EncodedMotor(Peripheral): CONSTANT_SINGLE = b'\x01' CONSTANT_GROUP = b'\x02' + SOME_SINGLE = b'\x07' + SOME_GROUP = b'\x08' TIMED_SINGLE = b'\x09' TIMED_GROUP = b'\x0A' ANGLED_SINGLE = b'\x0B' @@ -207,8 +209,19 @@ class EncodedMotor(Peripheral): self._wrap_and_write(command, speed_primary, speed_secondary) self.__wait_sync(async) + def some(self, speed_primary=1, speed_secondary=None, async=False): + if speed_secondary is None: + speed_secondary = speed_primary + + # movement type + command = self.SOME_GROUP if self.port == PORT_AB else self.SOME_SINGLE + + self.started() + self._wrap_and_write(command, speed_primary, speed_secondary) + self.__wait_sync(async) + def stop(self): - self.constant(0) + self.constant(0, async=True) def test(self, speed_primary=1, speed_secondary=None): if speed_secondary is None: @@ -219,10 +232,12 @@ class EncodedMotor(Peripheral): # self._wrap_and_write(command, 0.2, 0.2) # set for port - command = self.MOVEMENT_TYPE + b"\x02" + command = self.MOVEMENT_TYPE + b"\x07" + command += pack('') + 1:]) + yyy = float(parts[2][parts[2].rfind('>') + 1:]) + zzz = float(parts[3][parts[3].rfind('>') + 1:]) + return ranged(xxx), ranged(yyy), ranged(zzz) + + +def ranged(param): + return float(param / 10) + + +udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) +udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) +udp_sock.settimeout(0) + +logging.basicConfig(level=logging.INFO) +robot = Vernie() +robot.button.subscribe(on_btn) +robot.motor_AB.stop() + +try: + udp_sock.bind((host, port)) + time.sleep(1) + + while running: + message = "" + while True: + try: + data = udp_sock.recv(8192) + message = data + except KeyboardInterrupt: + raise + except: + break + + if not message: + time.sleep(0.05) + continue + + messageString = message.decode("utf-8") + a, b, c = decode_xml(messageString) + sa = int(10 * (c + b / 2.0)) / 10.0 + sb = int(10 * (c - b / 2.0)) / 10.0 + logging.info("SpeedA=%s, SpeedB=%s", sa, sb) + robot.motor_AB.constant(sa, sb, async=True) + time.sleep(0.5) +finally: + robot.motor_AB.stop() + udp_sock.close()