diff --git a/arduino_alvik.py b/arduino_alvik.py index 3d4d56c..acd0d90 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -49,6 +49,9 @@ def __init__(self): self.right_tof = None self.top_tof = None self.bottom_tof = None + self.linear_velocity = None + self.angular_velocity = None + self.last_ack = '' self.version = [None, None, None] def begin(self) -> int: @@ -84,6 +87,24 @@ def _stop_update_thread(cls): """ cls._update_thread_running = False + def rotate(self, angle: float): + """ + Rotates the robot by given angle + :param angle: + :return: + """ + self.packeter.packetC1F(ord('R'), angle) + uart.write(self.packeter.msg[0:self.packeter.msg_size]) + + def move(self, distance: float): + """ + Moves the robot by given distance + :param distance: + :return: + """ + self.packeter.packetC1F(ord('G'), distance) + uart.write(self.packeter.msg[0:self.packeter.msg_size]) + def stop(self): """ Stops all Alvik operations @@ -174,6 +195,13 @@ def drive(self, linear_velocity: float, angular_velocity: float): self.packeter.packetC2F(ord('V'), linear_velocity, angular_velocity) uart.write(self.packeter.msg[0:self.packeter.msg_size]) + def get_drive_speed(self) -> (float, float): + """ + Returns linear and angular velocity of the robot + :return: linear_velocity, angular_velocity + """ + return self.linear_velocity, self.angular_velocity + def set_servo_positions(self, a_position: int, b_position: int): """ Sets A/B servomotor angle @@ -184,6 +212,13 @@ def set_servo_positions(self, a_position: int, b_position: int): self.packeter.packetC2B(ord('S'), a_position & 0xFF, b_position & 0xFF) uart.write(self.packeter.msg[0:self.packeter.msg_size]) + def get_ack(self): + """ + Resets and returns last acknowledgement + :return: + """ + return self.last_ack + # def send_ack(self): # self.packeter.packetC1B(ord('X'), ACK_) # uart.write(self.packeter.msg[0:self.packeter.msg_size]) @@ -286,9 +321,15 @@ def _parse_message(self) -> int: elif code == ord('q'): # imu position _, self.roll, self.pitch, self.yaw = self.packeter.unpacketC3F() - if code == ord('w'): + elif code == ord('w'): # wheels position _, self.left_wheel._position, self.right_wheel._position = self.packeter.unpacketC2F() + elif code == ord('v'): + # robot velocity + _, self.linear_velocity, self.angular_velocity = self.packeter.unpacketC2F() + elif code == ord('x'): + # robot ack + _, self.last_ack = self.packeter.unpacketC1B() elif code == 0x7E: # firmware version _, *self.version = self.packeter.unpacketC3B() diff --git a/examples/move_example.py b/examples/move_example.py index 18c9e46..361e4ad 100644 --- a/examples/move_example.py +++ b/examples/move_example.py @@ -7,14 +7,18 @@ while True: try: - alvik.set_wheels_speed(10, 10) - sleep_ms(1000) + alvik.move(100.0) + while (ack := alvik.get_ack()) != ord('M'): + print(f'moving... not on target yet ack={ack}') + sleep_ms(200) + print("on target after move") - alvik.set_wheels_speed(30, 60) - sleep_ms(1000) + alvik.rotate(90.0) + while (ack := alvik.get_ack()) != ord('R'): + print(f'rotating... not on target yet ack={ack}') + sleep_ms(200) + print("on target after rotation") - alvik.set_wheels_speed(60, 30) - sleep_ms(1000) except KeyboardInterrupt as e: print('over') alvik.stop() diff --git a/examples/wheels_speed.py b/examples/wheels_speed.py new file mode 100644 index 0000000..18c9e46 --- /dev/null +++ b/examples/wheels_speed.py @@ -0,0 +1,21 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep_ms +import sys + +alvik = ArduinoAlvik() +alvik.begin() + +while True: + try: + alvik.set_wheels_speed(10, 10) + sleep_ms(1000) + + alvik.set_wheels_speed(30, 60) + sleep_ms(1000) + + alvik.set_wheels_speed(60, 30) + sleep_ms(1000) + except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit()