diff --git a/arduino_alvik.py b/arduino_alvik.py
index 01c6fa6..07329a8 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -21,53 +21,53 @@ class ArduinoAlvik:
_update_thread_id = None
def __new__(cls):
- if not hasattr(cls, 'instance'):
- cls.instance = super(ArduinoAlvik, cls).__new__(cls)
- return cls.instance
+ 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,
+ 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,
+ 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._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._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]
@staticmethod
def is_on() -> bool:
@@ -191,7 +191,7 @@ def _wait_for_ack(self) -> None:
Waits until receives 0x00 ack from robot
:return:
"""
- while self.last_ack != 0x00:
+ while self._last_ack != 0x00:
sleep_ms(20)
@staticmethod
@@ -231,12 +231,12 @@ def is_target_reached(self) -> bool:
It also responds with an ack received message
:return:
"""
- if self.last_ack != ord('M') and self.last_ack != ord('R'):
+ 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])
+ self._packeter.packetC1B(ord('X'), ord('K'))
+ uart.write(self._packeter.msg[0:self._packeter.msg_size])
sleep_ms(200)
return True
@@ -246,8 +246,8 @@ def set_behaviour(self, behaviour: int):
:param behaviour: behaviour code
:return:
"""
- self.packeter.packetC1B(ord('B'), behaviour & 0xFF)
- uart.write(self.packeter.msg[0:self.packeter.msg_size])
+ 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):
"""
@@ -259,8 +259,8 @@ def rotate(self, angle: float, unit: str = 'deg', blocking: bool = True):
"""
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])
+ self._packeter.packetC1F(ord('R'), angle)
+ uart.write(self._packeter.msg[0:self._packeter.msg_size])
if blocking:
self._wait_for_target()
@@ -274,8 +274,8 @@ def move(self, distance: float, unit: str = 'cm', blocking: bool = True):
"""
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])
+ self._packeter.packetC1F(ord('G'), distance)
+ uart.write(self._packeter.msg[0:self._packeter.msg_size])
if blocking:
self._wait_for_target()
@@ -293,8 +293,8 @@ def stop(self):
# stop the update thrad
self._stop_update_thread()
- # delete instance
- del self.__class__.instance
+ # delete _instance
+ del self.__class__._instance
gc.collect()
@staticmethod
@@ -333,8 +333,8 @@ def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'r
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])
+ 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'):
"""
@@ -344,9 +344,9 @@ def set_wheels_position(self, left_angle: float, right_angle: float, unit: str =
:param unit: the speed unit of measurement (default: 'rpm')
:return:
"""
- self.packeter.packetC2F(ord('A'), convert_angle(left_angle, unit, 'deg'),
+ 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])
+ uart.write(self._packeter.msg[0:self._packeter.msg_size])
def get_wheels_position(self, unit: str = 'deg') -> (float, float):
"""
@@ -363,28 +363,28 @@ def get_orientation(self) -> (float, float, float):
:return: roll, pitch, yaw
"""
- return self.roll, self.pitch, self.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
+ 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
+ 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
+ return self._ax, self._ay, self._az, self._gx, self._gy, self._gz
def get_line_sensors(self) -> (int, int, int):
"""
@@ -392,7 +392,7 @@ def get_line_sensors(self) -> (int, int, int):
:return: left_line, center_line, right_line
"""
- return self.left_line, self.center_line, self.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'):
@@ -409,8 +409,8 @@ def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: st
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])
+ self._packeter.packetC2F(ord('V'), linear_velocity, angular_velocity)
+ uart.write(self._packeter.msg[0:self._packeter.msg_size])
def brake(self):
"""
@@ -427,11 +427,11 @@ def get_drive_speed(self, linear_unit: str = 'cm/s', angular_unit: str = 'deg/s'
:return: linear_velocity, angular_velocity
"""
if angular_unit == '%':
- angular_velocity = (self.angular_velocity/ROBOT_MAX_DEG_S)*100
+ angular_velocity = (self._angular_velocity/ROBOT_MAX_DEG_S)*100
else:
- angular_velocity = convert_rotational_speed(self.angular_velocity, 'deg/s', angular_unit)
+ angular_velocity = convert_rotational_speed(self._angular_velocity, 'deg/s', angular_unit)
- return convert_speed(self.linear_velocity, 'mm/s', linear_unit), angular_velocity
+ 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'):
"""
@@ -446,8 +446,8 @@ def reset_pose(self, x: float, y: float, theta: float, distance_unit: str = 'cm'
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])
+ 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):
@@ -457,9 +457,9 @@ def get_pose(self, distance_unit: str = 'cm', angle_unit: str = 'deg') -> (float
: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))
+ 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):
"""
@@ -468,19 +468,19 @@ def set_servo_positions(self, a_position: int, b_position: int):
: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])
+ 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):
"""
Returns last acknowledgement
:return:
"""
- return self.last_ack
+ return self._last_ack
# def send_ack(self):
- # self.packeter.packetC1B(ord('X'), ACK_)
- # uart.write(self.packeter.msg[0:self.packeter.msg_size])
+ # self._packeter.packetC1B(ord('X'), ACK_)
+ # uart.write(self._packeter.msg[0:self._packeter.msg_size])
def _set_leds(self, led_state: int):
"""
@@ -489,9 +489,9 @@ def _set_leds(self, led_state: int):
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])
+ 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):
"""
@@ -499,10 +499,10 @@ def set_builtin_led(self, value: bool):
:param value:
:return:
"""
- if self.led_state[0] is None:
+ 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])
+ 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):
"""
@@ -510,10 +510,10 @@ def set_illuminator(self, value: bool):
:param value:
:return:
"""
- if self.led_state[0] is None:
+ 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])
+ 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):
"""
@@ -547,8 +547,8 @@ def _read_message(self) -> bool:
"""
while uart.any():
b = uart.read(1)[0]
- self.packeter.buffer.push(b)
- if b == self.packeter.end_index and self.packeter.checkPayload():
+ self._packeter.buffer.push(b)
+ if b == self._packeter.end_index and self._packeter.checkPayload():
return True
return False
@@ -557,53 +557,53 @@ def _parse_message(self) -> int:
Parse a received message
:return: -1 if parse error 0 if ok
"""
- code = self.packeter.payloadTop()
+ code = self._packeter.payloadTop()
if code == ord('j'):
# joint speed
- _, self.left_wheel._speed, self.right_wheel._speed = self.packeter.unpacketC2F()
+ _, 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()
+ _, 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()
+ _, 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()
+ _, 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()
+ _, self._battery_perc = self._packeter.unpacketC1F()
elif code == ord('d'):
# distance sensor
- _, self.left_tof, self.center_tof, self.right_tof = self.packeter.unpacketC3I()
+ _, self._left_tof, self._center_tof, self._right_tof = self._packeter.unpacketC3I()
elif code == ord('t'):
# touch input
- _, self.touch_bits = self.packeter.unpacketC1B()
+ _, self._touch_byte = self._packeter.unpacketC1B()
elif code == ord('b'):
# behaviour
- _, self.behaviour = self.packeter.unpacketC1B()
+ _, 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()
+ (_, 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()
+ _, 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()
+ _, 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()
+ _, self._linear_velocity, self._angular_velocity = self._packeter.unpacketC2F()
elif code == ord('x'):
# robot ack
- _, self.last_ack = self.packeter.unpacketC1B()
+ _, self._last_ack = self._packeter.unpacketC1B()
elif code == ord('z'):
# robot ack
- _, self.x, self.y, self.theta = self.packeter.unpacketC3F()
+ _, self._x, self._y, self._theta = self._packeter.unpacketC3F()
elif code == 0x7E:
# firmware version
- _, *self.version = self.packeter.unpacketC3B()
+ _, *self._version = self._packeter.unpacketC3B()
else:
return -1
@@ -614,72 +614,73 @@ def get_battery_charge(self) -> int:
Returns the battery SOC
:return:
"""
- if self.battery_perc > 100:
+ if self._battery_perc > 100:
return 100
- return round(self.battery_perc)
+ return round(self._battery_perc)
- def _get_touch(self) -> int:
+ @property
+ def _touch_bits(self) -> int:
"""
Returns the touch sensor's state
:return: touch_bits
"""
- return self.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)
+ 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)
+ 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)
+ 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)
+ 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)
+ 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)
+ 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)
+ 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)
+ return bool(self._touch_bits & 0b10000000)
@staticmethod
def _limit(value: float, lower: float, upper: float) -> float:
@@ -768,7 +769,7 @@ def get_color_raw(self) -> (int, int, int):
:return: red, green, blue
"""
- return self.red, self.green, self.blue
+ return self._red, self._green, self._blue
def _normalize_color(self, r: float, g: float, b: float) -> (float, float, float):
"""
@@ -836,6 +837,9 @@ def get_color(self, color_format: str = 'rgb') -> (float, float, float):
"""
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':
@@ -858,6 +862,9 @@ def hsv2label(h, s, v) -> str:
:return:
"""
+ if None in [h, s, v]:
+ return 'UNDEFINED'
+
if s < 0.1:
if v < 0.05:
label = 'BLACK'
@@ -899,11 +906,15 @@ def get_distance(self, unit: str = 'cm') -> (float, float, float, float, float):
: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))
+
+ if None in [self._left_tof, self._center_left_tof, self._center_tof, self._center_right_tof, self._right_tof]:
+ return None, None, None, None, None
+
+ 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:
"""
@@ -911,7 +922,7 @@ def get_distance_top(self, unit: str = 'cm') -> float:
:param unit:
:return:
"""
- return convert_distance(self.top_tof, 'mm', unit)
+ return convert_distance(self._top_tof, 'mm', unit)
def get_distance_bottom(self, unit: str = 'cm') -> float:
"""
@@ -919,14 +930,14 @@ def get_distance_bottom(self, unit: str = 'cm') -> float:
:param unit:
:return:
"""
- return convert_distance(self.bottom_tof, 'mm', unit)
+ 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]}'
+ return f'{self._version[0]}.{self._version[1]}.{self._version[2]}'
def print_status(self):
"""