diff --git a/arduino_alvik.py b/arduino_alvik.py deleted file mode 100644 index 9161470..0000000 --- a/arduino_alvik.py +++ /dev/null @@ -1,899 +0,0 @@ -import gc - -from uart import uart -import _thread -from time import sleep_ms - -from ucPack import ucPack - -from conversions import * -from pinout_definitions import * -from robot_definitions import * -from constants import * - - -class ArduinoAlvik: - - _update_thread_running = False - _update_thread_id = None - - def __new__(cls): - if not hasattr(cls, 'instance'): - cls.instance = super(ArduinoAlvik, cls).__new__(cls) - return cls.instance - - def __init__(self): - self.packeter = ucPack(200) - self.left_wheel = _ArduinoAlvikWheel(self.packeter, ord('L')) - self.right_wheel = _ArduinoAlvikWheel(self.packeter, ord('R')) - self.led_state = [None] - self.left_led = _ArduinoAlvikRgbLed(self.packeter, 'left', self.led_state, - rgb_mask=[0b00000100, 0b00001000, 0b00010000]) - self.right_led = _ArduinoAlvikRgbLed(self.packeter, 'right', self.led_state, - rgb_mask=[0b00100000, 0b01000000, 0b10000000]) - self.battery_perc = None - self.touch_bits = None - self.behaviour = None - self.red = None - self.green = None - self.blue = None - self._white_cal = None - self._black_cal = None - self.left_line = None - self.center_line = None - self.right_line = None - self.roll = None - self.pitch = None - self.yaw = None - self.x = None - self.y = None - self.theta = None - self.ax = None - self.ay = None - self.az = None - self.gx = None - self.gy = None - self.gz = None - self.left_tof = None - self.center_left_tof = None - self.center_tof = None - self.center_right_tof = None - 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: - """ - Begins all Alvik operations - :return: - """ - if not CHECK_STM32.value(): - print("\nTurn on your Arduino Alvik!\n") - return -1 - self._begin_update_thread() - sleep_ms(100) - self._reset_hw() - while uart.any(): - uart.read(1) - sleep_ms(1000) - while self.last_ack != 0x00: - sleep_ms(20) - sleep_ms(2000) - self.set_illuminator(True) - self.set_behaviour(1) - self._set_color_reference() - return 0 - - def _begin_update_thread(self): - """ - Runs robot background operations (e.g. threaded update) - :return: - """ - - if not self.__class__._update_thread_running: - self.__class__._update_thread_running = True - self.__class__._update_thread_id = _thread.start_new_thread(self._update, (1,)) - - @classmethod - def _stop_update_thread(cls): - """ - Stops the background operations - :return: - """ - cls._update_thread_running = False - - def _wait_for_target(self): - while not self.is_target_reached(): - pass - - def is_target_reached(self) -> bool: - if self.last_ack != ord('M') and self.last_ack != ord('R'): - sleep_ms(50) - return False - else: - self.packeter.packetC1B(ord('X'), ord('K')) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) - sleep_ms(200) - return True - - def set_behaviour(self, behaviour: int): - """ - Sets the behaviour of Alvik - :param behaviour: behaviour code - :return: - """ - self.packeter.packetC1B(ord('B'), behaviour & 0xFF) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) - - def rotate(self, angle: float, unit: str = 'deg', blocking: bool = True): - """ - Rotates the robot by given angle - :param angle: - :param blocking: - :param unit: the angle unit - :return: - """ - angle = convert_angle(angle, unit, 'deg') - sleep_ms(200) - self.packeter.packetC1F(ord('R'), angle) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) - if blocking: - self._wait_for_target() - - def move(self, distance: float, unit: str = 'cm', blocking: bool = True): - """ - Moves the robot by given distance - :param distance: - :param blocking: - :param unit: the distance unit - :return: - """ - distance = convert_distance(distance, unit, 'mm') - sleep_ms(200) - self.packeter.packetC1F(ord('G'), distance) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) - if blocking: - self._wait_for_target() - - def stop(self): - """ - Stops all Alvik operations - :return: - """ - # stop engines - self.set_wheels_speed(0, 0) - - # turn off UI leds - self._set_leds(0x00) - - # stop the update thrad - self._stop_update_thread() - - # delete instance - del self.__class__.instance - gc.collect() - - @staticmethod - def _reset_hw(): - """ - Resets the STM32 - :return: - """ - - RESET_STM32.value(0) - sleep_ms(100) - RESET_STM32.value(1) - sleep_ms(100) - - def get_wheels_speed(self, unit: str = 'rpm') -> (float, float): - """ - Returns the speed of the wheels - :return: left_wheel_speed, right_wheel_speed - """ - return self.left_wheel.get_speed(unit), self.right_wheel.get_speed(unit) - - def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'rpm'): - """ - Sets left/right motor speed - :param left_speed: - :param right_speed: - :param unit: the speed unit of measurement (default: 'rpm') - :return: - """ - - if unit == '%': - left_speed = (left_speed/100)*MOTOR_MAX_RPM - right_speed = (right_speed/100)*MOTOR_MAX_RPM - else: - left_speed = convert_rotational_speed(left_speed, unit, 'rpm') - right_speed = convert_rotational_speed(right_speed, unit, 'rpm') - - self.packeter.packetC2F(ord('J'), left_speed, right_speed) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) - - def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = 'deg'): - """ - Sets left/right motor angle - :param left_angle: - :param right_angle: - :param unit: the speed unit of measurement (default: 'rpm') - :return: - """ - self.packeter.packetC2F(ord('A'), convert_angle(left_angle, unit, 'deg'), - convert_angle(right_angle, unit, 'deg')) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) - - def get_wheels_position(self, unit: str = 'deg') -> (float, float): - """ - Returns the angle of the wheels - :return: left_wheel_angle, right_wheel_angle - """ - return (convert_angle(self.left_wheel.get_position(), 'deg', unit), - convert_angle(self.right_wheel.get_position(), 'deg', unit)) - - def get_orientation(self) -> (float, float, float): - """ - Returns the orientation of the IMU - :return: roll, pitch, yaw - """ - - return self.roll, self.pitch, self.yaw - - def get_accelerations(self) -> (float, float, float): - """ - Returns the 3-axial acceleration of the IMU - :return: ax, ay, az - """ - return self.ax, self.ay, self.az - - def get_gyros(self) -> (float, float, float): - """ - Returns the 3-axial angular acceleration of the IMU - :return: gx, gy, gz - """ - return self.gx, self.gy, self.gz - - def get_imu(self) -> (float, float, float, float, float, float): - """ - Returns all the IMUs readouts - :return: ax, ay, az, gx, gy, gz - """ - return self.ax, self.ay, self.az, self.gx, self.gy, self.gz - - def get_line_sensors(self) -> (int, int, int): - """ - Returns the line sensors readout - :return: left_line, center_line, right_line - """ - - return self.left_line, self.center_line, self.right_line - - def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: str = 'cm/s', - angular_unit: str = 'deg/s'): - """ - Drives the robot by linear and angular velocity - :param linear_velocity: - :param angular_velocity: - :param linear_unit: output linear velocity unit of meas - :param angular_unit: output angular velocity unit of meas - :return: - """ - linear_velocity = convert_speed(linear_velocity, linear_unit, 'mm/s') - if angular_unit == '%': - angular_velocity = (angular_velocity/100)*ROBOT_MAX_DEG_S - else: - angular_velocity = convert_rotational_speed(angular_velocity, angular_unit, 'deg/s') - self.packeter.packetC2F(ord('V'), linear_velocity, angular_velocity) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) - - def brake(self): - """ - Brakes the robot - :return: - """ - self.drive(0, 0) - - def get_drive_speed(self, linear_unit: str = 'cm/s', angular_unit: str = 'deg/s') -> (float, float): - """ - Returns linear and angular velocity of the robot - :param linear_unit: output linear velocity unit of meas - :param angular_unit: output angular velocity unit of meas - :return: linear_velocity, angular_velocity - """ - if angular_unit == '%': - angular_velocity = (self.angular_velocity/ROBOT_MAX_DEG_S)*100 - else: - angular_velocity = convert_rotational_speed(self.angular_velocity, 'deg/s', angular_unit) - - return convert_speed(self.linear_velocity, 'mm/s', linear_unit), angular_velocity - - def reset_pose(self, x: float, y: float, theta: float, distance_unit: str = 'cm', angle_unit: str = 'deg'): - """ - Resets the robot pose - :param x: x coordinate of the robot - :param y: y coordinate of the robot - :param theta: angle of the robot - :param distance_unit: angle of the robot - :param angle_unit: angle of the robot - :return: - """ - x = convert_distance(x, distance_unit, 'mm') - y = convert_distance(y, distance_unit, 'mm') - theta = convert_angle(theta, angle_unit, 'deg') - self.packeter.packetC3F(ord('Z'), x, y, theta) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) - sleep_ms(1000) - - def get_pose(self, distance_unit: str = 'cm', angle_unit: str = 'deg') -> (float, float, float): - """ - Returns the current pose of the robot - :param distance_unit: unit of x and y outputs - :param angle_unit: unit of theta output - :return: x, y, theta - """ - return (convert_distance(self.x, 'mm', distance_unit), - convert_distance(self.y, 'mm', distance_unit), - convert_angle(self.theta, 'deg', angle_unit)) - - def set_servo_positions(self, a_position: int, b_position: int): - """ - Sets A/B servomotor angle - :param a_position: position of A servomotor (0-180°) - :param b_position: position of B servomotor (0-180°) - :return: - """ - 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]) - - def _set_leds(self, led_state: int): - """ - Sets the LEDs state - :param led_state: one byte 0->builtin 1->illuminator 2->left_red 3->left_green 4->left_blue - 5->right_red 6->right_green 7->right_blue - :return: - """ - self.led_state[0] = led_state & 0xFF - self.packeter.packetC1B(ord('L'), self.led_state[0]) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) - - def set_builtin_led(self, value: bool): - """ - Turns on/off the builtin led - :param value: - :return: - """ - if self.led_state[0] is None: - self._set_leds(0x00) - self.led_state[0] = self.led_state[0] | 0b00000001 if value else self.led_state[0] & 0b11111110 - self._set_leds(self.led_state[0]) - - def set_illuminator(self, value: bool): - """ - Turns on/off the illuminator led - :param value: - :return: - """ - if self.led_state[0] is None: - self._set_leds(0x00) - self.led_state[0] = self.led_state[0] | 0b00000010 if value else self.led_state[0] & 0b11111101 - self._set_leds(self.led_state[0]) - - def _update(self, delay_=1): - """ - Updates the robot status reading/parsing messages from UART. - This method is blocking and meant as a thread callback - Use the method stop to terminate _update and exit the thread - :param delay_: while loop delay (ms) - :return: - """ - while True: - if not ArduinoAlvik._update_thread_running: - break - if self._read_message(): - self._parse_message() - sleep_ms(delay_) - - def _read_message(self) -> bool: - """ - Read a message from the uC - :return: True if a message terminator was reached - """ - while uart.any(): - b = uart.read(1)[0] - self.packeter.buffer.push(b) - if b == self.packeter.end_index and self.packeter.checkPayload(): - return True - return False - - def _parse_message(self) -> int: - """ - Parse a received message - :return: -1 if parse error 0 if ok - """ - code = self.packeter.payloadTop() - if code == ord('j'): - # joint speed - _, self.left_wheel._speed, self.right_wheel._speed = self.packeter.unpacketC2F() - elif code == ord('l'): - # line sensor - _, self.left_line, self.center_line, self.right_line = self.packeter.unpacketC3I() - elif code == ord('c'): - # color sensor - _, self.red, self.green, self.blue = self.packeter.unpacketC3I() - elif code == ord('i'): - # imu - _, self.ax, self.ay, self.az, self.gx, self.gy, self.gz = self.packeter.unpacketC6F() - elif code == ord('p'): - # battery percentage - _, self.battery_perc = self.packeter.unpacketC1F() - elif code == ord('d'): - # distance sensor - _, self.left_tof, self.center_tof, self.right_tof = self.packeter.unpacketC3I() - elif code == ord('t'): - # touch input - _, self.touch_bits = self.packeter.unpacketC1B() - elif code == ord('b'): - # behaviour - _, self.behaviour = self.packeter.unpacketC1B() - elif code == ord('f'): - # tof matrix - (_, self.left_tof, self.center_left_tof, self.center_tof, - self.center_right_tof, self.right_tof, self.top_tof, self.bottom_tof) = self.packeter.unpacketC7I() - elif code == ord('q'): - # imu position - _, self.roll, self.pitch, self.yaw = self.packeter.unpacketC3F() - 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 == ord('z'): - # robot ack - _, self.x, self.y, self.theta = self.packeter.unpacketC3F() - elif code == 0x7E: - # firmware version - _, *self.version = self.packeter.unpacketC3B() - else: - return -1 - - return 0 - - def _get_touch(self) -> int: - """ - Returns the touch sensor's state - :return: touch_bits - """ - return self.touch_bits - - def get_touch_any(self) -> bool: - """ - Returns true if any button is pressed - :return: - """ - return bool(self.touch_bits & 0b00000001) - - def get_touch_ok(self) -> bool: - """ - Returns true if ok button is pressed - :return: - """ - return bool(self.touch_bits & 0b00000010) - - def get_touch_cancel(self) -> bool: - """ - Returns true if cancel button is pressed - :return: - """ - return bool(self.touch_bits & 0b00000100) - - def get_touch_center(self) -> bool: - """ - Returns true if center button is pressed - :return: - """ - return bool(self.touch_bits & 0b00001000) - - def get_touch_up(self) -> bool: - """ - Returns true if up button is pressed - :return: - """ - return bool(self.touch_bits & 0b00010000) - - def get_touch_left(self) -> bool: - """ - Returns true if left button is pressed - :return: - """ - return bool(self.touch_bits & 0b00100000) - - def get_touch_down(self) -> bool: - """ - Returns true if down button is pressed - :return: - """ - return bool(self.touch_bits & 0b01000000) - - def get_touch_right(self) -> bool: - """ - Returns true if right button is pressed - :return: - """ - return bool(self.touch_bits & 0b10000000) - - @staticmethod - def _limit(value: float, lower: float, upper: float) -> float: - """ - Utility function to limit a value between a lower and upper limit - :param value: - :param lower: - :param upper: - :return: - """ - assert lower < upper - if value > upper: - value = upper - if value < lower: - value = lower - return value - - def _set_color_reference(self): - try: - from color_calibration import BLACK_CAL as _B - except ImportError: - _B = BLACK_CAL - try: - from color_calibration import WHITE_CAL as _W - except ImportError: - _W = WHITE_CAL - - self._black_cal = _B - self._white_cal = _W - - def color_calibration(self, background: str = 'white') -> None: - """ - Calibrates the color sensor - :param background: str white or black - :return: - """ - if background not in ['black', 'white']: - return - - red_avg = green_avg = blue_avg = 0 - - for _ in range(0, 100): - red, green, blue = self.get_color_raw() - red_avg += red - green_avg += green - blue_avg += blue - sleep_ms(10) - - red_avg = int(red_avg/100) - green_avg = int(green_avg/100) - blue_avg = int(blue_avg/100) - - if background == 'white': - self._white_cal = [red_avg, green_avg, blue_avg] - elif background == 'black': - self._black_cal = [red_avg, green_avg, blue_avg] - - file_path = './color_calibration.py' - - try: - with open(file_path, 'r') as file: - content = file.read().split('\n') - lines = [l + '\n' for l in content if l] - except OSError: - open(file_path, 'a').close() - lines = [] - - found_param_line = False - - for i, line in enumerate(lines): - if line.startswith(background.upper()): - lines[i] = f'{background.upper()}_CAL = [{red_avg}, {green_avg}, {blue_avg}]\n' - found_param_line = True - break - - if not found_param_line: - lines.extend([f'{background.upper()}_CAL = [{red_avg}, {green_avg}, {blue_avg}]\n']) - - with open(file_path, 'w') as file: - for line in lines: - file.write(line) - - def get_color_raw(self) -> (int, int, int): - """ - Returns the color sensor's raw readout - :return: red, green, blue - """ - - return self.red, self.green, self.blue - - def _normalize_color(self, r: float, g: float, b: float) -> (float, float, float): - """ - Color normalization - :param r: - :param g: - :param b: - :return: - """ - r = self._limit(r, self._black_cal[0], self._white_cal[0]) - g = self._limit(g, self._black_cal[1], self._white_cal[1]) - b = self._limit(b, self._black_cal[2], self._white_cal[2]) - - r = (r - self._black_cal[0])/(self._white_cal[0] - self._black_cal[0]) - g = (g - self._black_cal[1])/(self._white_cal[1] - self._black_cal[1]) - b = (b - self._black_cal[2])/(self._white_cal[2] - self._black_cal[2]) - - return r, g, b - - @staticmethod - def rgb2hsv(r: float, g: float, b: float) -> (float, float, float): - """ - Converts normalized rgb to hsv - :param r: - :param g: - :param b: - :return: - """ - min_ = min(r, g, b) - max_ = max(r, g, b) - - v = max_ - delta = max_ - min_ - - if delta < 0.00001: - h = 0 - s = 0 - return h, s, v - - if max_ > 0: - s = delta / max_ - else: - s = 0 - h = None - return h, s, v - - if r >= max_: - h = (g - b) / delta # color is between yellow and magenta - elif g >= max_: - h = 2.0 + (b - r) / delta - else: - h = 4.0 + (r - g) / delta - - h *= 60.0 - if h < 0: - h += 360.0 - - return h, s, v - - def get_color(self, color_format: str = 'rgb') -> (float, float, float): - """ - Returns the normalized color readout of the color sensor - :param color_format: rgb or hsv only - :return: - """ - assert color_format in ['rgb', 'hsv'] - - if color_format == 'rgb': - return self._normalize_color(*self.get_color_raw()) - elif color_format == 'hsv': - return self.rgb2hsv(*self._normalize_color(*self.get_color_raw())) - @staticmethod - def get_color_label(h, s, v) -> str: - label = 'UNDEFINED' - - if s < 0.1: - if v < 0.05: - label = 'BLACK' - elif v < 0.15: - label = 'GREY' - elif v < 0.8: - label = 'LIGHT GREY' - else: - label = 'WHITE' - else: - if v > 0.1: - if 20 <= h < 90: - label = 'YELLOW' - elif 90 <= h < 140: - label = 'LIGHT GREEN' - elif 140 <= h < 170: - label = 'GREEN' - elif 170 <= h < 210: - label = 'LIGHT BLUE' - elif 210 <= h < 260: - label = 'BLUE' - elif 260 <= h < 280: - label = 'VIOLET' - elif 260 <= h < 280: - label = 'VIOLET' - else: # h<20 or h>=280 is more problematic - if v < 0.5 and s < 0.45: - label = 'BROWN' - else: - if v > 0.77: - label = 'ORANGE' - else: - label = 'RED' - else: - label = 'BLACK' - return label - - def get_distance(self, unit: str = 'cm') -> (float, float, float, float, float): - """ - Returns the distance readout of the TOF sensor - :param unit: distance output unit - :return: left_tof, center_left_tof, center_tof, center_right_tof, right_tof - """ - return (convert_distance(self.left_tof, 'mm', unit), - convert_distance(self.center_left_tof, 'mm', unit), - convert_distance(self.center_tof, 'mm', unit), - convert_distance(self.center_right_tof, 'mm', unit), - convert_distance(self.right_tof, 'mm', unit)) - - def get_distance_top(self, unit: str = 'cm') -> float: - """ - Returns the obstacle top distance readout - :param unit: - :return: - """ - return convert_distance(self.top_tof, 'mm', unit) - - def get_distance_bottom(self, unit: str = 'cm') -> float: - """ - Returns the obstacle bottom distance readout - :param unit: - :return: - """ - return convert_distance(self.bottom_tof, 'mm', unit) - - def get_version(self) -> str: - """ - Returns the firmware version of the Alvik - :return: - """ - return f'{self.version[0]}.{self.version[1]}.{self.version[2]}' - - def print_status(self): - """ - Prints the Alvik status - :return: - """ - for a in vars(self): - if str(a).startswith('_'): - continue - print(f'{str(a).upper()} = {getattr(self, str(a))}') - - -class _ArduinoAlvikWheel: - - def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEEL_DIAMETER_MM): - self._packeter = packeter - self._label = label - self._wheel_diameter_mm = wheel_diameter_mm - self._speed = None - self._position = None - - def reset(self, initial_position: float = 0.0, unit: str = 'deg'): - """ - Resets the wheel reference position - :param initial_position: - :param unit: reference position unit (defaults to deg) - :return: - """ - initial_position = convert_angle(initial_position, unit, 'deg') - self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('Z'), initial_position) - uart.write(self._packeter.msg[0:self._packeter.msg_size]) - - def set_pid_gains(self, kp: float = MOTOR_KP_DEFAULT, ki: float = MOTOR_KI_DEFAULT, kd: float = MOTOR_KD_DEFAULT): - """ - Set PID gains for Alvik wheels - :param kp: proportional gain - :param ki: integration gain - :param kd: derivative gain - :return: - """ - - self._packeter.packetC1B3F(ord('P'), self._label & 0xFF, kp, ki, kd) - uart.write(self._packeter.msg[0:self._packeter.msg_size]) - - def stop(self): - """ - Stop Alvik wheel - :return: - """ - self.set_speed(0) - - def set_speed(self, velocity: float, unit: str = 'rpm'): - """ - Sets the motor speed - :param velocity: the speed of the motor - :param unit: the unit of measurement - :return: - """ - - if unit == '%': - velocity = (velocity/100)*MOTOR_MAX_RPM - else: - velocity = convert_rotational_speed(velocity, unit, 'rpm') - - self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('V'), velocity) - uart.write(self._packeter.msg[0:self._packeter.msg_size]) - - def get_speed(self, unit: str = 'rpm') -> float: - """ - Returns the current RPM speed of the wheel - :param unit: the unit of the output speed - :return: - """ - if unit == '%': - speed = (self._speed/MOTOR_MAX_RPM)*100 - else: - speed = convert_rotational_speed(self._speed, 'rpm', unit) - return speed - - def get_position(self, unit: str = 'deg') -> float: - """ - Returns the wheel position (angle with respect to the reference) - :param unit: the unit of the output position - :return: - """ - return convert_angle(self._position, 'deg', unit) - - def set_position(self, position: float, unit: str = 'deg'): - """ - Sets left/right motor speed - :param position: the speed of the motor - :param unit: the unit of measurement - :return: - """ - self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'), - convert_angle(position, unit, 'deg')) - uart.write(self._packeter.msg[0:self._packeter.msg_size]) - - -class _ArduinoAlvikRgbLed: - def __init__(self, packeter: ucPack, label: str, led_state: list[int | None], rgb_mask: list[int]): - self._packeter = packeter - self.label = label - self._rgb_mask = rgb_mask - self._led_state = led_state - - def set_color(self, red: bool, green: bool, blue: bool): - """ - Sets the LED's r,g,b state - :param red: - :param green: - :param blue: - :return: - """ - led_status = self._led_state[0] - if led_status is None: - return - led_status = led_status | self._rgb_mask[0] if red else led_status & (0b11111111 - self._rgb_mask[0]) - led_status = led_status | self._rgb_mask[1] if green else led_status & (0b11111111 - self._rgb_mask[1]) - led_status = led_status | self._rgb_mask[2] if blue else led_status & (0b11111111 - self._rgb_mask[2]) - self._led_state[0] = led_status - self._packeter.packetC1B(ord('L'), led_status & 0xFF) - uart.write(self._packeter.msg[0:self._packeter.msg_size]) diff --git a/arduino_alvik/__init__.py b/arduino_alvik/__init__.py new file mode 100644 index 0000000..72f6180 --- /dev/null +++ b/arduino_alvik/__init__.py @@ -0,0 +1 @@ +from .arduino_alvik import * diff --git a/arduino_alvik/arduino_alvik.py b/arduino_alvik/arduino_alvik.py new file mode 100644 index 0000000..506295f --- /dev/null +++ b/arduino_alvik/arduino_alvik.py @@ -0,0 +1,1324 @@ +import sys +import gc +import struct +from machine import I2C +import _thread +from time import sleep_ms + +from ucPack import ucPack + +from .uart import uart +from .conversions import * +from .pinout_definitions import * +from .robot_definitions import * +from .constants import * + + +class ArduinoAlvik: + + _update_thread_running = False + _update_thread_id = None + _touch_events_thread_running = False + _touch_events_thread_id = None + + def __new__(cls): + if not hasattr(cls, '_instance'): + cls._instance = super(ArduinoAlvik, cls).__new__(cls) + return cls._instance + + def __init__(self): + self._packeter = ucPack(200) + self.left_wheel = _ArduinoAlvikWheel(self._packeter, ord('L')) + self.right_wheel = _ArduinoAlvikWheel(self._packeter, ord('R')) + self._led_state = list((None,)) + self.left_led = _ArduinoAlvikRgbLed(self._packeter, 'left', self._led_state, + rgb_mask=[0b00000100, 0b00001000, 0b00010000]) + self.right_led = _ArduinoAlvikRgbLed(self._packeter, 'right', self._led_state, + rgb_mask=[0b00100000, 0b01000000, 0b10000000]) + self._battery_perc = None + self._touch_byte = None + self._behaviour = None + self._red = None + self._green = None + self._blue = None + self._white_cal = None + self._black_cal = None + self._left_line = None + self._center_line = None + self._right_line = None + self._roll = None + self._pitch = None + self._yaw = None + self._x = None + self._y = None + self._theta = None + self._ax = None + self._ay = None + self._az = None + self._gx = None + self._gy = None + self._gz = None + self._left_tof = None + self._center_left_tof = None + self._center_tof = None + self._center_right_tof = None + 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] + self._touch_events = _ArduinoAlvikTouchEvents() + + @staticmethod + def is_on() -> bool: + """ + Returns true if robot is on + :return: + """ + return CHECK_STM32.value() == 1 + + @staticmethod + def _progress_bar(percentage: float) -> None: + """ + Prints a progressbar + :param percentage: + :return: + """ + sys.stdout.write(bytes('\r'.encode('utf-8'))) + if percentage > 97: + marks_str = ' \U0001F50B' + else: + marks_str = ' \U0001FAAB' + word = marks_str + f" {percentage}% \t" + sys.stdout.write(bytes((word.encode('utf-8')))) + + def _idle(self, delay_=1, check_on_thread=False) -> None: + """ + Alvik's idle mode behaviour + :return: + """ + + NANO_CHK.value(1) + sleep_ms(500) + led_val = 0 + + try: + while not self.is_on(): + if check_on_thread and not self.__class__._update_thread_running: + break + _ESP32_SDA = Pin(A4, Pin.OUT) + _ESP32_SCL = Pin(A5, Pin.OUT) + _ESP32_SCL.value(1) + _ESP32_SDA.value(1) + sleep_ms(100) + _ESP32_SCL.value(0) + _ESP32_SDA.value(0) + + cmd = bytearray(1) + cmd[0] = 0x06 + i2c = I2C(0, scl=ESP32_SCL, sda=ESP32_SDA) + i2c.writeto(0x36, cmd) + soc_raw = struct.unpack('h', i2c.readfrom(0x36, 2))[0] + soc_perc = soc_raw * 0.00390625 + self._progress_bar(round(soc_perc)) + sleep_ms(delay_) + if soc_perc > 97: + LEDG.value(0) + LEDR.value(1) + else: + LEDR.value(led_val) + LEDG.value(1) + led_val = (led_val + 1) % 2 + print("Alvik is on") + except KeyboardInterrupt: + self.stop() + sys.exit() + except Exception as e: + pass + # print(f'Unable to read SOC: {e}') + finally: + LEDR.value(1) + LEDG.value(1) + NANO_CHK.value(0) + + @staticmethod + def _snake_robot(duration: int = 1000): + """ + Snake robot animation + :param duration: + :return: + """ + + robot = '\U0001F916' + snake = '\U0001F40D' + + cycles = int(duration / 200) + + frame = '' + for i in range(0, cycles): + sys.stdout.write(bytes('\r'.encode('utf-8'))) + pre = ' ' * i + between = ' ' * (i % 2 + 1) + post = ' ' * 5 + frame = pre + snake + between + robot + post + sys.stdout.write(bytes(frame.encode('utf-8'))) + sleep_ms(200) + + sys.stdout.write(bytes('\r'.encode('utf-8'))) + clear_frame = ' ' * len(frame) + sys.stdout.write(bytes(clear_frame.encode('utf-8'))) + + def begin(self) -> int: + """ + Begins all Alvik operations + :return: + """ + if not self.is_on(): + print("\nTurn on your Arduino Alvik!\n") + sleep_ms(1000) + self._idle(1000) + self._begin_update_thread() + sleep_ms(100) + if self._touch_events.has_callbacks(): + print('Starting touch events') + self._start_touch_events_thread() + self._reset_hw() + self._flush_uart() + self._snake_robot(1000) + self._wait_for_ack() + self._snake_robot(2000) + self.set_illuminator(True) + self.set_behaviour(1) + self._set_color_reference() + return 0 + + def _wait_for_ack(self) -> None: + """ + Waits until receives 0x00 ack from robot + :return: + """ + while self._last_ack != 0x00: + sleep_ms(20) + + @staticmethod + def _flush_uart(): + """ + Empties the UART buffer + :return: + """ + while uart.any(): + uart.read(1) + + def _begin_update_thread(self): + """ + Runs robot background operations (e.g. threaded update) + :return: + """ + + if not self.__class__._update_thread_running: + self.__class__._update_thread_running = True + self.__class__._update_thread_id = _thread.start_new_thread(self._update, (1,)) + + @classmethod + def _stop_update_thread(cls): + """ + Stops the background operations + :return: + """ + cls._update_thread_running = False + + def _wait_for_target(self): + while not self.is_target_reached(): + pass + + def is_target_reached(self) -> bool: + """ + Returns True if robot has sent an M or R acknowledgment. + It also responds with an ack received message + :return: + """ + if self._last_ack != ord('M') and self._last_ack != ord('R'): + sleep_ms(50) + return False + else: + self._packeter.packetC1B(ord('X'), ord('K')) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + sleep_ms(200) + return True + + def set_behaviour(self, behaviour: int): + """ + Sets the behaviour of Alvik + :param behaviour: behaviour code + :return: + """ + self._packeter.packetC1B(ord('B'), behaviour & 0xFF) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + def rotate(self, angle: float, unit: str = 'deg', blocking: bool = True): + """ + Rotates the robot by given angle + :param angle: + :param blocking: + :param unit: the angle unit + :return: + """ + angle = convert_angle(angle, unit, 'deg') + sleep_ms(200) + self._packeter.packetC1F(ord('R'), angle) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + if blocking: + self._wait_for_target() + + def move(self, distance: float, unit: str = 'cm', blocking: bool = True): + """ + Moves the robot by given distance + :param distance: + :param blocking: + :param unit: the distance unit + :return: + """ + distance = convert_distance(distance, unit, 'mm') + sleep_ms(200) + self._packeter.packetC1F(ord('G'), distance) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + if blocking: + self._wait_for_target() + + def stop(self): + """ + Stops all Alvik operations + :return: + """ + # stop engines + self.set_wheels_speed(0, 0) + + # turn off UI leds + self._set_leds(0x00) + + # stop the update thread + self._stop_update_thread() + + # stop touch events thread + self._stop_touch_events_thread() + + # delete _instance + del self.__class__._instance + gc.collect() + + @staticmethod + def _reset_hw(): + """ + Resets the STM32 + :return: + """ + + RESET_STM32.value(0) + sleep_ms(100) + RESET_STM32.value(1) + sleep_ms(100) + + def get_wheels_speed(self, unit: str = 'rpm') -> (float | None, float | None): + """ + Returns the speed of the wheels + :param unit: the speed unit of measurement (default: 'rpm') + :return: left_wheel_speed, right_wheel_speed + """ + return self.left_wheel.get_speed(unit), self.right_wheel.get_speed(unit) + + def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'rpm'): + """ + Sets left/right motor speed + :param left_speed: + :param right_speed: + :param unit: the speed unit of measurement (default: 'rpm') + :return: + """ + + if unit == '%': + left_speed = (left_speed / 100) * MOTOR_MAX_RPM + right_speed = (right_speed / 100) * MOTOR_MAX_RPM + else: + left_speed = convert_rotational_speed(left_speed, unit, 'rpm') + right_speed = convert_rotational_speed(right_speed, unit, 'rpm') + + self._packeter.packetC2F(ord('J'), left_speed, right_speed) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = 'deg'): + """ + Sets left/right motor angle + :param left_angle: + :param right_angle: + :param unit: the speed unit of measurement (default: 'rpm') + :return: + """ + self._packeter.packetC2F(ord('A'), convert_angle(left_angle, unit, 'deg'), + convert_angle(right_angle, unit, 'deg')) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + def get_wheels_position(self, unit: str = 'deg') -> (float | None, float | None): + """ + Returns the angle of the wheels + :param unit: the angle unit of measurement (default: 'deg') + :return: left_wheel_angle, right_wheel_angle + """ + return (convert_angle(self.left_wheel.get_position(), 'deg', unit), + convert_angle(self.right_wheel.get_position(), 'deg', unit)) + + def get_orientation(self) -> (float | None, float | None, float | None): + """ + Returns the orientation of the IMU + :return: roll, pitch, yaw + """ + + return self._roll, self._pitch, self._yaw + + def get_accelerations(self) -> (float | None, float | None, float | None): + """ + Returns the 3-axial acceleration of the IMU + :return: ax, ay, az + """ + return self._ax, self._ay, self._az + + def get_gyros(self) -> (float | None, float | None, float | None): + """ + Returns the 3-axial angular acceleration of the IMU + :return: gx, gy, gz + """ + return self._gx, self._gy, self._gz + + def get_imu(self) -> (float | None, float | None, float | None, float | None, float | None, float | None): + """ + Returns all the IMUs readouts + :return: ax, ay, az, gx, gy, gz + """ + return self._ax, self._ay, self._az, self._gx, self._gy, self._gz + + def get_line_sensors(self) -> (int | None, int | None, int | None): + """ + Returns the line sensors readout + :return: left_line, center_line, right_line + """ + + return self._left_line, self._center_line, self._right_line + + def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: str = 'cm/s', + angular_unit: str = 'deg/s'): + """ + Drives the robot by linear and angular velocity + :param linear_velocity: + :param angular_velocity: + :param linear_unit: output linear velocity unit of meas + :param angular_unit: output angular velocity unit of meas + :return: + """ + linear_velocity = convert_speed(linear_velocity, linear_unit, 'mm/s') + if angular_unit == '%': + angular_velocity = (angular_velocity / 100) * ROBOT_MAX_DEG_S + else: + angular_velocity = convert_rotational_speed(angular_velocity, angular_unit, 'deg/s') + self._packeter.packetC2F(ord('V'), linear_velocity, angular_velocity) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + def brake(self): + """ + Brakes the robot + :return: + """ + self.drive(0, 0) + + def get_drive_speed(self, linear_unit: str = 'cm/s', angular_unit: str = 'deg/s') -> (float | None, float | None): + """ + Returns linear and angular velocity of the robot + :param linear_unit: output linear velocity unit of meas + :param angular_unit: output angular velocity unit of meas + :return: linear_velocity, angular_velocity + """ + if angular_unit == '%': + angular_velocity = (self._angular_velocity / ROBOT_MAX_DEG_S) * 100 \ + if self._angular_velocity is not None else None + else: + angular_velocity = convert_rotational_speed(self._angular_velocity, 'deg/s', angular_unit) + + return convert_speed(self._linear_velocity, 'mm/s', linear_unit), angular_velocity + + def reset_pose(self, x: float, y: float, theta: float, distance_unit: str = 'cm', angle_unit: str = 'deg'): + """ + Resets the robot pose + :param x: x coordinate of the robot + :param y: y coordinate of the robot + :param theta: angle of the robot + :param distance_unit: angle of the robot + :param angle_unit: angle of the robot + :return: + """ + x = convert_distance(x, distance_unit, 'mm') + y = convert_distance(y, distance_unit, 'mm') + theta = convert_angle(theta, angle_unit, 'deg') + self._packeter.packetC3F(ord('Z'), x, y, theta) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + sleep_ms(1000) + + def get_pose(self, distance_unit: str = 'cm', angle_unit: str = 'deg') \ + -> (float | None, float | None, float | None): + """ + Returns the current pose of the robot + :param distance_unit: unit of x and y outputs + :param angle_unit: unit of theta output + :return: x, y, theta + """ + return (convert_distance(self._x, 'mm', distance_unit), + convert_distance(self._y, 'mm', distance_unit), + convert_angle(self._theta, 'deg', angle_unit)) + + def set_servo_positions(self, a_position: int, b_position: int): + """ + Sets A/B servomotor angle + :param a_position: position of A servomotor (0-180) + :param b_position: position of B servomotor (0-180) + :return: + """ + 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) -> str: + """ + 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]) + + def _set_leds(self, led_state: int): + """ + Sets the LEDs state + :param led_state: one byte 0->builtin 1->illuminator 2->left_red 3->left_green 4->left_blue + 5->right_red 6->right_green 7->right_blue + :return: + """ + self._led_state[0] = led_state & 0xFF + self._packeter.packetC1B(ord('L'), self._led_state[0]) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + def set_builtin_led(self, value: bool): + """ + Turns on/off the builtin led + :param value: + :return: + """ + if self._led_state[0] is None: + self._set_leds(0x00) + self._led_state[0] = self._led_state[0] | 0b00000001 if value else self._led_state[0] & 0b11111110 + self._set_leds(self._led_state[0]) + + def set_illuminator(self, value: bool): + """ + Turns on/off the illuminator led + :param value: + :return: + """ + if self._led_state[0] is None: + self._set_leds(0x00) + self._led_state[0] = self._led_state[0] | 0b00000010 if value else self._led_state[0] & 0b11111101 + self._set_leds(self._led_state[0]) + + def _update(self, delay_=1): + """ + Updates the robot status reading/parsing messages from UART. + This method is blocking and meant as a thread callback + Use the method stop to terminate _update and exit the thread + :param delay_: while loop delay (ms) + :return: + """ + while True: + if not self.is_on(): + print("Alvik is off") + self._idle(1000, check_on_thread=True) + self._reset_hw() + self._flush_uart() + sleep_ms(1000) + self._wait_for_ack() + sleep_ms(2000) + self.set_illuminator(True) + self.set_behaviour(1) + if not ArduinoAlvik._update_thread_running: + break + if self._read_message(): + self._parse_message() + sleep_ms(delay_) + + def _read_message(self) -> bool: + """ + Read a message from the uC + :return: True if a message terminator was reached + """ + while uart.any(): + b = uart.read(1)[0] + self._packeter.buffer.push(b) + if b == self._packeter.end_index and self._packeter.checkPayload(): + return True + return False + + def _parse_message(self) -> int: + """ + Parse a received message + :return: -1 if parse error 0 if ok + """ + code = self._packeter.payloadTop() + if code == ord('j'): + # joint speed + _, self.left_wheel._speed, self.right_wheel._speed = self._packeter.unpacketC2F() + elif code == ord('l'): + # line sensor + _, self._left_line, self._center_line, self._right_line = self._packeter.unpacketC3I() + elif code == ord('c'): + # color sensor + _, self._red, self._green, self._blue = self._packeter.unpacketC3I() + elif code == ord('i'): + # imu + _, self._ax, self._ay, self._az, self._gx, self._gy, self._gz = self._packeter.unpacketC6F() + elif code == ord('p'): + # battery percentage + _, self._battery_perc = self._packeter.unpacketC1F() + elif code == ord('d'): + # distance sensor + _, self._left_tof, self._center_tof, self._right_tof = self._packeter.unpacketC3I() + elif code == ord('t'): + # touch input + _, self._touch_byte = self._packeter.unpacketC1B() + elif code == ord('b'): + # behaviour + _, self._behaviour = self._packeter.unpacketC1B() + elif code == ord('f'): + # tof matrix + (_, self._left_tof, self._center_left_tof, self._center_tof, + self._center_right_tof, self._right_tof, self._top_tof, self._bottom_tof) = self._packeter.unpacketC7I() + elif code == ord('q'): + # imu position + _, self._roll, self._pitch, self._yaw = self._packeter.unpacketC3F() + 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 == ord('z'): + # robot ack + _, self._x, self._y, self._theta = self._packeter.unpacketC3F() + elif code == 0x7E: + # firmware version + _, *self._version = self._packeter.unpacketC3B() + else: + return -1 + + return 0 + + def get_battery_charge(self) -> int | None: + """ + Returns the battery SOC + :return: + """ + if self._battery_perc is None: + return None + if self._battery_perc > 100: + return 100 + return round(self._battery_perc) + + @property + def _touch_bits(self) -> int: + """ + Returns the touch sensor's state + :return: touch_bits + """ + return (self._touch_byte & 0xFF) if self._touch_byte is not None else 0x00 + + def get_touch_any(self) -> bool: + """ + Returns true if any button is pressed + :return: + """ + return bool(self._touch_bits & 0b00000001) + + def get_touch_ok(self) -> bool: + """ + Returns true if ok button is pressed + :return: + """ + return bool(self._touch_bits & 0b00000010) + + def get_touch_cancel(self) -> bool: + """ + Returns true if cancel button is pressed + :return: + """ + return bool(self._touch_bits & 0b00000100) + + def get_touch_center(self) -> bool: + """ + Returns true if center button is pressed + :return: + """ + return bool(self._touch_bits & 0b00001000) + + def get_touch_up(self) -> bool: + """ + Returns true if up button is pressed + :return: + """ + return bool(self._touch_bits & 0b00010000) + + def get_touch_left(self) -> bool: + """ + Returns true if left button is pressed + :return: + """ + return bool(self._touch_bits & 0b00100000) + + def get_touch_down(self) -> bool: + """ + Returns true if down button is pressed + :return: + """ + return bool(self._touch_bits & 0b01000000) + + def get_touch_right(self) -> bool: + """ + Returns true if right button is pressed + :return: + """ + return bool(self._touch_bits & 0b10000000) + + @staticmethod + def _limit(value: float, lower: float, upper: float) -> float: + """ + Utility function to limit a value between a lower and upper limit + :param value: + :param lower: + :param upper: + :return: + """ + assert lower < upper + if value > upper: + value = upper + if value < lower: + value = lower + return value + + def _set_color_reference(self): + try: + from color_calibration import BLACK_CAL as _B + except ImportError: + _B = BLACK_CAL + try: + from color_calibration import WHITE_CAL as _W + except ImportError: + _W = WHITE_CAL + + self._black_cal = _B + self._white_cal = _W + + def color_calibration(self, background: str = 'white') -> None: + """ + Calibrates the color sensor + :param background: str white or black + :return: + """ + if background not in ['black', 'white']: + return + + red_avg = green_avg = blue_avg = 0 + + for _ in range(0, 100): + red, green, blue = self.get_color_raw() + red_avg += red + green_avg += green + blue_avg += blue + sleep_ms(10) + + red_avg = int(red_avg / 100) + green_avg = int(green_avg / 100) + blue_avg = int(blue_avg / 100) + + if background == 'white': + self._white_cal = [red_avg, green_avg, blue_avg] + elif background == 'black': + self._black_cal = [red_avg, green_avg, blue_avg] + + file_path = './color_calibration.py' + + try: + with open(file_path, 'r') as file: + content = file.read().split('\n') + lines = [line + '\n' for line in content if line] + except OSError: + open(file_path, 'a').close() + lines = [] + + found_param_line = False + + for i, line in enumerate(lines): + if line.startswith(background.upper()): + lines[i] = f'{background.upper()}_CAL = [{red_avg}, {green_avg}, {blue_avg}]\n' + found_param_line = True + break + + if not found_param_line: + lines.extend([f'{background.upper()}_CAL = [{red_avg}, {green_avg}, {blue_avg}]\n']) + + with open(file_path, 'w') as file: + for line in lines: + file.write(line) + + def get_color_raw(self) -> (int | None, int | None, int | None): + """ + Returns the color sensor's raw readout + :return: red, green, blue + """ + + return self._red, self._green, self._blue + + def _normalize_color(self, r: float, g: float, b: float) -> (float, float, float): + """ + Color normalization + :param r: + :param g: + :param b: + :return: + """ + r = self._limit(r, self._black_cal[0], self._white_cal[0]) + g = self._limit(g, self._black_cal[1], self._white_cal[1]) + b = self._limit(b, self._black_cal[2], self._white_cal[2]) + + r = (r - self._black_cal[0]) / (self._white_cal[0] - self._black_cal[0]) + g = (g - self._black_cal[1]) / (self._white_cal[1] - self._black_cal[1]) + b = (b - self._black_cal[2]) / (self._white_cal[2] - self._black_cal[2]) + + return r, g, b + + @staticmethod + def rgb2hsv(r: float, g: float, b: float) -> (float, float, float): + """ + Converts normalized rgb to hsv + :param r: + :param g: + :param b: + :return: + """ + min_ = min(r, g, b) + max_ = max(r, g, b) + + v = max_ + delta = max_ - min_ + + if delta < 0.00001: + h = 0 + s = 0 + return h, s, v + + if max_ > 0: + s = delta / max_ + else: + s = 0 + h = None + return h, s, v + + if r >= max_: + h = (g - b) / delta # color is between yellow and magenta + elif g >= max_: + h = 2.0 + (b - r) / delta + else: + h = 4.0 + (r - g) / delta + + h *= 60.0 + if h < 0: + h += 360.0 + + return h, s, v + + def get_color(self, color_format: str = 'rgb') -> (float | None, float | None, float | None): + """ + Returns the normalized color readout of the color sensor + :param color_format: rgb or hsv only + :return: + """ + assert color_format in ['rgb', 'hsv'] + + if None in list(self.get_color_raw()): + return None, None, None + + if color_format == 'rgb': + return self._normalize_color(*self.get_color_raw()) + elif color_format == 'hsv': + return self.rgb2hsv(*self._normalize_color(*self.get_color_raw())) + + def get_color_label(self) -> str: + """ + Returns the label of the color as recognized by the sensor + :return: + """ + return self.hsv2label(*self.get_color(color_format='hsv')) + + @staticmethod + def hsv2label(h, s, v) -> str: + """ + Returns the color label corresponding to the given normalized HSV color input + :param h: + :param s: + :param v: + :return: + """ + + if None in [h, s, v]: + return 'UNDEFINED' + + if s < 0.1: + if v < 0.05: + label = 'BLACK' + elif v < 0.15: + label = 'GREY' + elif v < 0.8: + label = 'LIGHT GREY' + else: + label = 'WHITE' + else: + if v > 0.1: + if 20 <= h < 90: + label = 'YELLOW' + elif 90 <= h < 140: + label = 'LIGHT GREEN' + elif 140 <= h < 170: + label = 'GREEN' + elif 170 <= h < 210: + label = 'LIGHT BLUE' + elif 210 <= h < 260: + label = 'BLUE' + elif 260 <= h < 280: + label = 'VIOLET' + else: # h<20 or h>=280 is more problematic + if v < 0.5 and s < 0.45: + label = 'BROWN' + else: + if v > 0.77: + label = 'ORANGE' + else: + label = 'RED' + else: + label = 'BLACK' + return label + + def get_distance(self, unit: str = 'cm') -> (float | None, float | None, float | None, float | None, float | None): + """ + Returns the distance readout of the TOF sensor + :param unit: distance output unit + :return: left_tof, center_left_tof, center_tof, center_right_tof, right_tof + """ + + return (convert_distance(self._left_tof, 'mm', unit), + convert_distance(self._center_left_tof, 'mm', unit), + convert_distance(self._center_tof, 'mm', unit), + convert_distance(self._center_right_tof, 'mm', unit), + convert_distance(self._right_tof, 'mm', unit)) + + def get_distance_top(self, unit: str = 'cm') -> float | None: + """ + Returns the obstacle top distance readout + :param unit: + :return: + """ + return convert_distance(self._top_tof, 'mm', unit) + + def get_distance_bottom(self, unit: str = 'cm') -> float | None: + """ + Returns the obstacle bottom distance readout + :param unit: + :return: + """ + return convert_distance(self._bottom_tof, 'mm', unit) + + def get_version(self) -> str: + """ + Returns the firmware version of the Alvik + :return: + """ + return f'{self._version[0]}.{self._version[1]}.{self._version[2]}' + + def print_status(self): + """ + Prints the Alvik status + :return: + """ + for a in vars(self): + if str(a).startswith('_'): + continue + print(f'{str(a).upper()} = {getattr(self, str(a))}') + + def on_touch_ok_pressed(self, callback: callable, args: tuple = ()) -> None: + """ + Register callback when touch button OK is pressed + :param callback: + :param args: + :return: + """ + self._touch_events.register_callback('on_ok_pressed', callback, args) + + def on_touch_cancel_pressed(self, callback: callable, args: tuple = ()) -> None: + """ + Register callback when touch button CANCEL is pressed + :param callback: + :param args: + :return: + """ + self._touch_events.register_callback('on_cancel_pressed', callback, args) + + def on_touch_center_pressed(self, callback: callable, args: tuple = ()) -> None: + """ + Register callback when touch button CENTER is pressed + :param callback: + :param args: + :return: + """ + self._touch_events.register_callback('on_center_pressed', callback, args) + + def on_touch_up_pressed(self, callback: callable, args: tuple = ()) -> None: + """ + Register callback when touch button UP is pressed + :param callback: + :param args: + :return: + """ + self._touch_events.register_callback('on_up_pressed', callback, args) + + def on_touch_left_pressed(self, callback: callable, args: tuple = ()) -> None: + """ + Register callback when touch button LEFT is pressed + :param callback: + :param args: + :return: + """ + self._touch_events.register_callback('on_left_pressed', callback, args) + + def on_touch_down_pressed(self, callback: callable, args: tuple = ()) -> None: + """ + Register callback when touch button DOWN is pressed + :param callback: + :param args: + :return: + """ + self._touch_events.register_callback('on_down_pressed', callback, args) + + def on_touch_right_pressed(self, callback: callable, args: tuple = ()) -> None: + """ + Register callback when touch button RIGHT is pressed + :param callback: + :param args: + :return: + """ + self._touch_events.register_callback('on_right_pressed', callback, args) + + def _start_touch_events_thread(self) -> None: + """ + Starts the touch events thread + :return: + """ + if not self.__class__._touch_events_thread_running: + self.__class__._touch_events_thread_running = True + self.__class__._touch_events_thread_id = _thread.start_new_thread(self._update_touch_events, (50,)) + + def _update_touch_events(self, delay_: int = 100): + """ + Updates the touch state so that touch events can be generated + :param delay_: + :return: + """ + while True: + if self.is_on() and self._touch_byte is not None: + self._touch_events.update_touch_state(self._touch_byte) + if not ArduinoAlvik._touch_events_thread_running: + break + sleep_ms(delay_) + + @classmethod + def _stop_touch_events_thread(cls): + """ + Stops the touch events thread + :return: + """ + cls._touch_events_thread_running = False + + +class _ArduinoAlvikWheel: + + def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEEL_DIAMETER_MM): + self._packeter = packeter + self._label = label + self._wheel_diameter_mm = wheel_diameter_mm + self._speed = None + self._position = None + + def reset(self, initial_position: float = 0.0, unit: str = 'deg'): + """ + Resets the wheel reference position + :param initial_position: + :param unit: reference position unit (defaults to deg) + :return: + """ + initial_position = convert_angle(initial_position, unit, 'deg') + self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('Z'), initial_position) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + def set_pid_gains(self, kp: float = MOTOR_KP_DEFAULT, ki: float = MOTOR_KI_DEFAULT, kd: float = MOTOR_KD_DEFAULT): + """ + Set PID gains for Alvik wheels + :param kp: proportional gain + :param ki: integration gain + :param kd: derivative gain + :return: + """ + + self._packeter.packetC1B3F(ord('P'), self._label & 0xFF, kp, ki, kd) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + def stop(self): + """ + Stop Alvik wheel + :return: + """ + self.set_speed(0) + + def set_speed(self, velocity: float, unit: str = 'rpm'): + """ + Sets the motor speed + :param velocity: the speed of the motor + :param unit: the unit of measurement + :return: + """ + + if unit == '%': + velocity = (velocity / 100) * MOTOR_MAX_RPM + else: + velocity = convert_rotational_speed(velocity, unit, 'rpm') + + self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('V'), velocity) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + def get_speed(self, unit: str = 'rpm') -> float | None: + """ + Returns the current RPM speed of the wheel + :param unit: the unit of the output speed + :return: + """ + if unit == '%': + speed = (self._speed / MOTOR_MAX_RPM) * 100 if self._speed is not None else None + else: + speed = convert_rotational_speed(self._speed, 'rpm', unit) + return speed + + def get_position(self, unit: str = 'deg') -> float | None: + """ + Returns the wheel position (angle with respect to the reference) + :param unit: the unit of the output position + :return: + """ + return convert_angle(self._position, 'deg', unit) + + def set_position(self, position: float, unit: str = 'deg'): + """ + Sets left/right motor speed + :param position: the speed of the motor + :param unit: the unit of measurement + :return: + """ + self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'), + convert_angle(position, unit, 'deg')) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + +class _ArduinoAlvikRgbLed: + def __init__(self, packeter: ucPack, label: str, led_state: list[int | None], rgb_mask: list[int]): + self._packeter = packeter + self.label = label + self._rgb_mask = rgb_mask + self._led_state = led_state + + def set_color(self, red: bool, green: bool, blue: bool): + """ + Sets the LED's r,g,b state + :param red: + :param green: + :param blue: + :return: + """ + led_status = self._led_state[0] + if led_status is None: + return + led_status = led_status | self._rgb_mask[0] if red else led_status & (0b11111111 - self._rgb_mask[0]) + led_status = led_status | self._rgb_mask[1] if green else led_status & (0b11111111 - self._rgb_mask[1]) + led_status = led_status | self._rgb_mask[2] if blue else led_status & (0b11111111 - self._rgb_mask[2]) + self._led_state[0] = led_status + self._packeter.packetC1B(ord('L'), led_status & 0xFF) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + +class _ArduinoAlvikEvents: + """ + This is a generic events class + """ + + def __init__(self): + self._callbacks = dict() + + def register_callback(self, event_name: str, callback: callable, args: tuple = None): + """ + Registers a callback to execute on an event + :param event_name: + :param callback: the callable + :param args: arguments tuple to pass to the callable. remember the comma! (value,) + :return: + """ + self._callbacks[event_name] = (callback, args,) + + def has_callbacks(self) -> bool: + """ + True if the _callbacks dictionary has any callback registered + :return: + """ + return bool(self._callbacks) + + def execute_callback(self, event_name: str): + """ + Executes the callback associated to the event_name + :param event_name: + :return: + """ + if event_name not in self._callbacks.keys(): + return + self._callbacks[event_name][0](*self._callbacks[event_name][1]) + + +class _ArduinoAlvikTouchEvents(_ArduinoAlvikEvents): + """ + This is the event class to handle touch button events + """ + + available_events = ['on_ok_pressed', 'on_cancel_pressed', + 'on_center_pressed', 'on_left_pressed', + 'on_right_pressed', 'on_up_pressed', + 'on_down_pressed'] + + def __init__(self): + self._current_touch_state = 0 + super().__init__() + + @staticmethod + def _is_ok_pressed(current_state, new_state) -> bool: + """ + True if OK was pressed + :param current_state: + :param new_state: + :return: + """ + return not bool(current_state & 0b00000010) and bool(new_state & 0b00000010) + + @staticmethod + def _is_cancel_pressed(current_state, new_state) -> bool: + """ + True if CANCEL was pressed + :param current_state: + :param new_state: + :return: + """ + return not bool(current_state & 0b00000100) and bool(new_state & 0b00000100) + + @staticmethod + def _is_center_pressed(current_state, new_state) -> bool: + """ + True if CENTER was pressed + :param current_state: + :param new_state: + :return: + """ + return not bool(current_state & 0b00001000) and bool(new_state & 0b00001000) + + @staticmethod + def _is_up_pressed(current_state, new_state) -> bool: + """ + True if UP was pressed + :param current_state: + :param new_state: + :return: + """ + return not bool(current_state & 0b00010000) and bool(new_state & 0b00010000) + + @staticmethod + def _is_left_pressed(current_state, new_state) -> bool: + """ + True if LEFT was pressed + :param current_state: + :param new_state: + :return: + """ + return not bool(current_state & 0b00100000) and bool(new_state & 0b00100000) + + @staticmethod + def _is_down_pressed(current_state, new_state) -> bool: + """ + True if DOWN was pressed + :param current_state: + :param new_state: + :return: + """ + return not bool(current_state & 0b01000000) and bool(new_state & 0b01000000) + + @staticmethod + def _is_right_pressed(current_state, new_state) -> bool: + """ + True if RIGHT was pressed + :param current_state: + :param new_state: + :return: + """ + return not bool(current_state & 0b10000000) and bool(new_state & 0b10000000) + + def update_touch_state(self, touch_state: int): + """ + Updates the internal touch state and executes any possible callback + :param touch_state: + :return: + """ + + if self._is_ok_pressed(self._current_touch_state, touch_state): + self.execute_callback('on_ok_pressed') + + if self._is_cancel_pressed(self._current_touch_state, touch_state): + self.execute_callback('on_cancel_pressed') + + if self._is_center_pressed(self._current_touch_state, touch_state): + self.execute_callback('on_center_pressed') + + if self._is_up_pressed(self._current_touch_state, touch_state): + self.execute_callback('on_up_pressed') + + if self._is_left_pressed(self._current_touch_state, touch_state): + self.execute_callback('on_left_pressed') + + if self._is_down_pressed(self._current_touch_state, touch_state): + self.execute_callback('on_down_pressed') + + if self._is_right_pressed(self._current_touch_state, touch_state): + self.execute_callback('on_right_pressed') + + self._current_touch_state = touch_state + + def register_callback(self, event_name: str, callback: callable, args: tuple = None): + if event_name not in self.__class__.available_events: + return + super().register_callback(event_name, callback, args) diff --git a/constants.py b/arduino_alvik/constants.py similarity index 100% rename from constants.py rename to arduino_alvik/constants.py diff --git a/conversions.py b/arduino_alvik/conversions.py similarity index 93% rename from conversions.py rename to arduino_alvik/conversions.py index d2cc96c..36486ce 100644 --- a/conversions.py +++ b/arduino_alvik/conversions.py @@ -9,6 +9,10 @@ def wrapper(*args, **kwargs): return func(*args, **kwargs) except KeyError: raise ConversionError(f'Cannot {func.__name__} from {args[1]} to {args[2]}') + except TypeError: + return None + except Exception as e: + raise ConversionError(f'Unexpected error: {e}') return wrapper diff --git a/utilities/firmware_updater.py b/arduino_alvik/firmware_updater.py similarity index 62% rename from utilities/firmware_updater.py rename to arduino_alvik/firmware_updater.py index 6a98ad2..0367704 100644 --- a/utilities/firmware_updater.py +++ b/arduino_alvik/firmware_updater.py @@ -1,9 +1,14 @@ from sys import exit from stm32_flash import * +if CHECK_STM32.value() is not 1: + print("Turn on your Alvik to continue...") + while CHECK_STM32.value() is not 1: + sleep_ms(500) + ans = STM32_startCommunication() if ans == STM32_NACK: - print("Cannot etablish connection with STM32") + print("Cannot establish connection with STM32") exit(-1) print('\nSTM32 FOUND') diff --git a/pinout_definitions.py b/arduino_alvik/pinout_definitions.py similarity index 71% rename from pinout_definitions.py rename to arduino_alvik/pinout_definitions.py index 5551eb1..06d24a3 100644 --- a/pinout_definitions.py +++ b/arduino_alvik/pinout_definitions.py @@ -11,7 +11,12 @@ BOOT0_STM32 = Pin(D2, Pin.OUT) # nano D2 -> STM32 Boot0 RESET_STM32 = Pin(D3, Pin.OUT) # nano D3 -> STM32 NRST -NANO_CHK = Pin(D4, Pin.OUT) # nano D3 -> STM32 NANO_CHK -CHECK_STM32 = Pin(A6, Pin.IN, Pin.PULL_UP) # nano A6/D23 -> STM32 ROBOT_CHK +NANO_CHK = Pin(D4, Pin.OUT) # nano D4 -> STM32 NANO_CHK +CHECK_STM32 = Pin(A6, Pin.IN, Pin.PULL_DOWN) # nano A6/D23 -> STM32 ROBOT_CHK ESP32_SDA = Pin(A4, Pin.OUT) # ESP32_SDA ESP32_SCL = Pin(A5, Pin.OUT) # ESP32_SCL + +# LEDS +LEDR = Pin(46, Pin.OUT) #RED ESP32 LEDR +LEDG = Pin(0, Pin.OUT) #GREEN ESP32 LEDG +LEDB = Pin(45, Pin.OUT) #BLUE ESP32 LEDB diff --git a/robot_definitions.py b/arduino_alvik/robot_definitions.py similarity index 100% rename from robot_definitions.py rename to arduino_alvik/robot_definitions.py diff --git a/utilities/stm32_flash.py b/arduino_alvik/stm32_flash.py similarity index 98% rename from utilities/stm32_flash.py rename to arduino_alvik/stm32_flash.py index 200af76..89522c6 100644 --- a/utilities/stm32_flash.py +++ b/arduino_alvik/stm32_flash.py @@ -3,6 +3,9 @@ from time import sleep_ms from machine import UART, Pin +A6 = 13 # ESP32 pin13 -> nano A6/D23 +CHECK_STM32 = Pin(A6, Pin.IN) # nano A6/D23 -> TO CHECK STM32 IS ON + STM32_INIT = b'\x7F' STM32_NACK = b'\x1F' STM32_ACK = b'\x79' diff --git a/uart.py b/arduino_alvik/uart.py similarity index 100% rename from uart.py rename to arduino_alvik/uart.py diff --git a/examples/line_follower.py b/examples/line_follower.py new file mode 100644 index 0000000..e50136f --- /dev/null +++ b/examples/line_follower.py @@ -0,0 +1,61 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep_ms +import sys + + +def calculate_center(left: int, center: int, right: int): + centroid = 0 + sum_weight = left + center + right + sum_values = left + 2 * center + 3 * right + if sum_weight != 0: + centroid = sum_values / sum_weight + centroid = 2 - centroid + return centroid + + +alvik = ArduinoAlvik() +alvik.begin() + +error = 0 +control = 0 +kp = 50.0 + +alvik.left_led.set_color(0, 0, 1) +alvik.right_led.set_color(0, 0, 1) + +while not alvik.get_touch_ok(): + sleep_ms(50) + +try: + while True: + while not alvik.get_touch_cancel(): + + line_sensors = alvik.get_line_sensors() + print(f' {line_sensors}') + + error = calculate_center(*line_sensors) + control = error * kp + + if control > 0.2: + alvik.left_led.set_color(1, 0, 0) + alvik.right_led.set_color(0, 0, 0) + elif control < -0.2: + alvik.left_led.set_color(1, 0, 0) + alvik.right_led.set_color(0, 0, 0) + else: + alvik.left_led.set_color(0, 1, 0) + alvik.right_led.set_color(0, 1, 0) + + alvik.set_wheels_speed(30 - control, 30 + control) + sleep_ms(100) + + while not alvik.get_touch_ok(): + alvik.left_led.set_color(0, 0, 1) + alvik.right_led.set_color(0, 0, 1) + alvik.brake() + sleep_ms(100) + +except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() diff --git a/examples/message_reader.py b/examples/message_reader.py index 57b3dd0..e03707c 100644 --- a/examples/message_reader.py +++ b/examples/message_reader.py @@ -3,19 +3,21 @@ import sys alvik = ArduinoAlvik() -if alvik.begin() < 0: - sys.exit() +alvik.begin() + +speed = 0 while True: try: - print(f'VER: {alvik.version}') + print(f'VER: {alvik.get_version()}') print(f'LSP: {alvik.left_wheel.get_speed()}') print(f'RSP: {alvik.right_wheel.get_speed()}') print(f'LPOS: {alvik.left_wheel.get_position()}') print(f'RPOS: {alvik.right_wheel.get_position()}') - print(f'TOUCH: {alvik.touch_bits}') - print(f'RGB: {alvik.red} {alvik.green} {alvik.blue}') - print(f'LINE: {alvik.left_line} {alvik.center_line} {alvik.right_line}') + print(f'TOUCH (UP): {alvik.get_touch_up()}') + print(f'RGB: {alvik.get_color()}') + print(f'LINE: {alvik.get_line_sensors()}') + print(f'SOC: {alvik.get_battery_charge()}%') alvik.set_wheels_speed(speed, speed) speed = (speed + 1) % 60 diff --git a/examples/pose_example.py b/examples/pose_example.py index 3004d4d..ab248e9 100644 --- a/examples/pose_example.py +++ b/examples/pose_example.py @@ -34,15 +34,39 @@ alvik.move(50.0, 'mm', blocking=False) print("on target after move") + while not alvik.is_target_reached(): + alvik.left_led.set_color(1, 0, 0) + sleep_ms(500) + alvik.left_led.set_color(0, 0, 0) + sleep_ms(500) + alvik.rotate(45.0, 'deg', blocking=False) print("on target after rotation") + while not alvik.is_target_reached(): + alvik.left_led.set_color(1, 0, 0) + sleep_ms(500) + alvik.left_led.set_color(0, 0, 0) + sleep_ms(500) + alvik.move(100.0, 'mm', blocking=False) print("on target after move") + while not alvik.is_target_reached(): + alvik.left_led.set_color(1, 0, 0) + sleep_ms(500) + alvik.left_led.set_color(0, 0, 0) + sleep_ms(500) + alvik.rotate(-90.00, 'deg', blocking=False) print("on target after rotation") + while not alvik.is_target_reached(): + alvik.left_led.set_color(1, 0, 0) + sleep_ms(500) + alvik.left_led.set_color(0, 0, 0) + sleep_ms(500) + x, y, theta = alvik.get_pose() print(f'Current pose is x(cm)={x}, y(cm)={y}, theta(deg)={theta}') diff --git a/examples/read_color_sensor.py b/examples/read_color_sensor.py index 994dbca..462826c 100644 --- a/examples/read_color_sensor.py +++ b/examples/read_color_sensor.py @@ -10,7 +10,7 @@ r, g, b = alvik.get_color() h, s, v = alvik.get_color('hsv') print(f'RED: {r}, Green: {g}, Blue: {b}, HUE: {h}, SAT: {s}, VAL: {v}') - print(f'COLOR LABEL: {alvik.get_color_label(h, s, v)}') + print(f'COLOR LABEL: {alvik.get_color_label()}') sleep_ms(100) except KeyboardInterrupt as e: print('over') diff --git a/examples/test_idle.py b/examples/test_idle.py new file mode 100644 index 0000000..f65625a --- /dev/null +++ b/examples/test_idle.py @@ -0,0 +1,23 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep_ms +import sys + +alvik = ArduinoAlvik() +alvik.begin() + +speed = 0 + +while True: + try: + + if alvik.is_on(): + print(f'VER: {alvik.get_version()}') + print(f'LSP: {alvik.left_wheel.get_speed()}') + alvik.set_wheels_speed(speed, speed) + speed = (speed + 1) % 30 + sleep_ms(1000) + except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() + diff --git a/examples/touch_events.py b/examples/touch_events.py new file mode 100644 index 0000000..db2d64f --- /dev/null +++ b/examples/touch_events.py @@ -0,0 +1,57 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep +import sys + +value = 0 + + +def toggle_left_led(custom_text: str = '') -> None: + global value + value = (value + 1) % 2 + alvik.left_led.set_color(value, 0, 0) + print(f"RED BLINKS! {custom_text}") + + +def simple_print(custom_text: str = '') -> None: + print(custom_text) + +alvik = ArduinoAlvik() +alvik.on_touch_ok_pressed(toggle_left_led, ("OK WAS PRESSED... THAT'S COOL", )) +alvik.on_touch_center_pressed(simple_print, ("CENTER PRESSED",)) +alvik.on_touch_cancel_pressed(simple_print, ("CANCEL PRESSED",)) +alvik.on_touch_up_pressed(simple_print, ("UP PRESSED",)) +alvik.on_touch_left_pressed(simple_print, ("LEFT PRESSED",)) +alvik.on_touch_down_pressed(simple_print, ("DOWN PRESSED",)) +alvik.on_touch_right_pressed(simple_print, ("RIGHT PRESSED",)) + +alvik.begin() + +alvik.left_wheel.reset() +alvik.right_wheel.reset() + +while True: + try: + alvik.left_wheel.set_position(30) + sleep(2) + print(f'Left wheel degs: {alvik.left_wheel.get_position()}') + print(f'Right wheel degs: {alvik.right_wheel.get_position()}') + + alvik.right_wheel.set_position(10) + sleep(2) + print(f'Left wheel degs: {alvik.left_wheel.get_position()}') + print(f'Right wheel degs: {alvik.right_wheel.get_position()}') + + alvik.left_wheel.set_position(180) + sleep(2) + print(f'Left wheel degs: {alvik.left_wheel.get_position()}') + print(f'Right wheel degs: {alvik.right_wheel.get_position()}') + + alvik.right_wheel.set_position(270) + sleep(2) + print(f'Left wheel degs: {alvik.left_wheel.get_position()}') + print(f'Right wheel degs: {alvik.right_wheel.get_position()}') + + except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() diff --git a/examples/wheels_servo.py b/examples/wheels_position.py similarity index 100% rename from examples/wheels_servo.py rename to examples/wheels_position.py diff --git a/install.bat b/install.bat index 2e81c7c..4e17379 100644 --- a/install.bat +++ b/install.bat @@ -16,19 +16,15 @@ if /i "%1"=="-h" ( ) :install -python -m mpremote %port_string% fs rm :arduino_alvik.py -python -m mpremote %port_string% fs rm :constants.py -python -m mpremote %port_string% fs rm :conversions.py -python -m mpremote %port_string% fs rm :pinout_definitions.py -python -m mpremote %port_string% fs rm :robot_definitions.py -python -m mpremote %port_string% fs rm :uart.py -python -m mpremote %port_string% fs cp arduino_alvik.py :arduino_alvik.py -python -m mpremote %port_string% fs cp constants.py :constants.py -python -m mpremote %port_string% fs cp conversions.py :conversions.py -python -m mpremote %port_string% fs cp pinout_definitions.py :pinout_definitions.py -python -m mpremote %port_string% fs cp robot_definitions.py :robot_definitions.py -python -m mpremote %port_string% fs cp uart.py :uart.py +python -m mpremote %port_string% fs mkdir lib/arduino_alvik +python -m mpremote %port_string% fs cp arduino_alvik/__init__.py :lib/arduino_alvik/__init__.py +python -m mpremote %port_string% fs cp arduino_alvik/arduino_alvik.py :lib/arduino_alvik/arduino_alvik.py +python -m mpremote %port_string% fs cp arduino_alvik/constants.py :lib/arduino_alvik/constants.py +python -m mpremote %port_string% fs cp arduino_alvik/conversions.py :lib/arduino_alvik/conversions.py +python -m mpremote %port_string% fs cp arduino_alvik/pinout_definitions.py :lib/arduino_alvik/pinout_definitions.py +python -m mpremote %port_string% fs cp arduino_alvik/robot_definitions.py :lib/arduino_alvik/robot_definitions.py +python -m mpremote %port_string% fs cp arduino_alvik/uart.py :lib/arduino_alvik/uart.py echo Installing dependencies python -m mpremote %port_string% mip install github:arduino/ucPack-mpy diff --git a/install.sh b/install.sh index c5950d4..736166c 100644 --- a/install.sh +++ b/install.sh @@ -42,19 +42,14 @@ fi # Uncomment the following line on windows machines # python_command="python" -$python_command -m mpremote $connect_string fs rm :arduino_alvik.py -$python_command -m mpremote $connect_string fs rm :constants.py -$python_command -m mpremote $connect_string fs rm :conversions.py -$python_command -m mpremote $connect_string fs rm :pinout_definitions.py -$python_command -m mpremote $connect_string fs rm :robot_definitions.py -$python_command -m mpremote $connect_string fs rm :uart.py - -$python_command -m mpremote $connect_string fs cp arduino_alvik.py :arduino_alvik.py -$python_command -m mpremote $connect_string fs cp constants.py :constants.py -$python_command -m mpremote $connect_string fs cp conversions.py :conversions.py -$python_command -m mpremote $connect_string fs cp pinout_definitions.py :pinout_definitions.py -$python_command -m mpremote $connect_string fs cp robot_definitions.py :robot_definitions.py -$python_command -m mpremote $connect_string fs cp uart.py :uart.py +$python_command -m mpremote $connect_string fs mkdir lib/arduino_alvik +$python_command -m mpremote $connect_string fs cp arduino_alvik/__init__.py :lib/arduino_alvik/__init__.py +$python_command -m mpremote $connect_string fs cp arduino_alvik/arduino_alvik.py :lib/arduino_alvik/arduino_alvik.py +$python_command -m mpremote $connect_string fs cp arduino_alvik/constants.py :lib/arduino_alvik/constants.py +$python_command -m mpremote $connect_string fs cp arduino_alvik/conversions.py :lib/arduino_alvik/conversions.py +$python_command -m mpremote $connect_string fs cp arduino_alvik/pinout_definitions.py :lib/arduino_alvik/pinout_definitions.py +$python_command -m mpremote $connect_string fs cp arduino_alvik/robot_definitions.py :lib/arduino_alvik/robot_definitions.py +$python_command -m mpremote $connect_string fs cp arduino_alvik/uart.py :lib/arduino_alvik/uart.py echo "Installing dependencies" $python_command -m mpremote $connect_string mip install github:arduino/ucPack-mpy diff --git a/package.json b/package.json index 72a44a7..90b6c77 100644 --- a/package.json +++ b/package.json @@ -1,7 +1,17 @@ { "urls": [ + ["arduino_alvik/__init__.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/__init__.py"], + ["arduino_alvik/arduino_alvik.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/arduino_alvik.py"], + ["arduino_alvik/constants.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/constants.py"], + ["arduino_alvik/conversions.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/conversions.py"], + ["arduino_alvik/pinout_definitions.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/pinout_definitions.py"], + ["arduino_alvik/robot_definitions.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/robot_definitions.py"], + ["arduino_alvik/uart.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/uart.py"], + ["arduino_alvik/firmware_updater.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/firmware_updater.py"], + ["arduino_alvik/stm32_flash.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/stm32_flash.py"] ], "deps": [ + ["github:arduino/ucPack-mpy", "0.1.5"] ], - "version": "0.2.0" + "version": "0.3.0" } \ No newline at end of file diff --git a/utilities/flash_firmware.bat b/utilities/flash_firmware.bat index ee8b7b7..f883fee 100644 --- a/utilities/flash_firmware.bat +++ b/utilities/flash_firmware.bat @@ -26,23 +26,19 @@ if "%1"=="" ( echo Installing flash firmware utilities... -python -m mpremote %port_string% fs rm :firmware_updater.py -python -m mpremote %port_string% fs rm :stm32_flash.py - -python -m mpremote %port_string% fs cp firmware_updater.py :firmware_updater.py -python -m mpremote %port_string% fs cp stm32_flash.py :stm32_flash.py +python -m mpremote %port_string% fs cp ../arduino_alvik/firmware_updater.py :firmware_updater.py +python -m mpremote %port_string% fs cp ../arduino_alvik/stm32_flash.py :stm32_flash.py echo Uploading %filename% -python -m mpremote %port_string% fs rm :firmware.bin python -m mpremote %port_string% fs cp %filename% :firmware.bin set /p userInput=Do you want to flash the firmware right now? (y/N): if /i "%userInput%"=="y" ( - python -m mpremote %port_string% run firmware_updater.py + python -m mpremote %port_string% run ../arduino_alvik/firmware_updater.py ) else ( - echo Firmware was not flashed on the remote device. + echo The firmware will not be written to the device. ) python -m mpremote %port_string% reset @@ -53,4 +49,4 @@ echo Usage: %~nx0 [-p PORT] FILENAME echo Options: echo -p PORT Specify the device port echo -h Display this help message -exit /b 0 \ No newline at end of file +exit /b 0 diff --git a/utilities/flash_firmware.sh b/utilities/flash_firmware.sh index b9bfd1c..c973ad8 100644 --- a/utilities/flash_firmware.sh +++ b/utilities/flash_firmware.sh @@ -53,24 +53,20 @@ fi echo "Installing flash firmware utilities..." -$python_command -m mpremote $connect_string fs rm :firmware_updater.py -$python_command -m mpremote $connect_string fs rm :stm32_flash.py - -$python_command -m mpremote $connect_string fs cp firmware_updater.py :firmware_updater.py -$python_command -m mpremote $connect_string fs cp stm32_flash.py :stm32_flash.py +$python_command -m mpremote $connect_string fs cp ../arduino_alvik/firmware_updater.py :firmware_updater.py +$python_command -m mpremote $connect_string fs cp ../arduino_alvik/stm32_flash.py :stm32_flash.py echo "Uploading $filename..." -$python_command -m mpremote $connect_string fs rm :firmware.bin $python_command -m mpremote $connect_string fs cp $filename :firmware.bin -echo "Do want to flash the firmware right now? (y/N)" +echo "Do you want to flash the firmware right now? (y/N)" read do_flash if [ "$do_flash" == "y" ] || [ "$do_flash" == "Y" ]; then - $python_command -m mpremote $connect_string run firmware_updater.py + $python_command -m mpremote $connect_string run ../arduino_alvik/firmware_updater.py else - echo "Firmware was not flashed on the remote device." + echo "The firmware will not be written to the device." fi $python_command -m mpremote $connect_string reset