diff --git a/arduino_alvik.py b/arduino_alvik.py
index f10dbe5..633ec49 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -1,3 +1,7 @@
+import math
+
+import gc
+
from uart import uart
import _thread
from time import sleep_ms
@@ -5,21 +9,32 @@
from ucPack import ucPack
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._update_thread_running = False
- self._update_thread_id = None
- self.l_speed = None
- self.r_speed = None
+ 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.led_state = None
self.red = None
self.green = None
self.blue = None
@@ -29,6 +44,15 @@ def __init__(self):
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
@@ -36,25 +60,108 @@ def __init__(self):
self.right_tof = None
self.top_tof = None
self.bottom_tof = None
+ self.linear_velocity = None
+ self.angular_velocity = None
+ self.last_ack = ''
self.version = [None, None, None]
- def run(self):
+ 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)
+ self.set_illuminator(True)
+ return 0
+
+ def _begin_update_thread(self):
"""
Runs robot background operations (e.g. threaded update)
:return:
"""
- self._update_thread_running = True
- self._update_thread_id = _thread.start_new_thread(self._update, (1,))
- def stop(self):
+ 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:
"""
- self._update_thread_running = False
+ 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 rotate(self, angle: float, blocking: bool = True):
+ """
+ Rotates the robot by given angle
+ :param angle:
+ :param blocking:
+ :return:
+ """
+ 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, blocking: bool = True):
+ """
+ Moves the robot by given distance
+ :param distance:
+ :param blocking:
+ :return:
+ """
+ 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():
+ def _reset_hw():
"""
Resets the STM32
:return:
@@ -65,48 +172,104 @@ def reset_hw():
RESET_STM32.value(1)
sleep_ms(100)
- def get_speeds(self) -> (float, float):
- return self.l_speed, self.r_speed
+ def get_wheels_speed(self) -> (float, float):
+ """
+ Returns the speed of the wheels
+ :return: left_wheel_speed, right_wheel_speed
+ """
+ return self.left_wheel.get_speed(), self.right_wheel.get_speed()
- def set_speeds(self, left_speed: float, right_speed: float):
+ 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:
"""
- self.packeter.packetC2F(ord('J'), left_speed, right_speed)
- uart.write(self.packeter.msg[0:self.packeter.msg_size])
- def set_pid(self, side: str, kp: float, ki: float, kd: float):
- """
- Sets motor PID parameters. Side can be 'L' or 'R'
- :param side:
- :param kp:
- :param ki:
- :param kd:
- :return:
- """
+ if unit not in angular_velocity_units:
+ return
+ elif unit == '%':
+ left_speed = perc_to_rpm(left_speed)
+ right_speed = perc_to_rpm(right_speed)
- self.packeter.packetC1B3F(ord('P'), ord(side), kp, ki, kd)
+ self.packeter.packetC2F(ord('J'), left_speed, right_speed)
uart.write(self.packeter.msg[0:self.packeter.msg_size])
def get_orientation(self) -> (float, float, float):
"""
Returns the orientation of the IMU
- :return:
+ :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:
+ :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):
+ """
+ Drives the robot by linear and angular velocity
+ :param linear_velocity:
+ :param angular_velocity:
+ :return:
+ """
+ self.packeter.packetC2F(ord('V'), linear_velocity, angular_velocity)
+ uart.write(self.packeter.msg[0:self.packeter.msg_size])
+
+ def get_drive_speed(self) -> (float, float):
+ """
+ Returns linear and angular velocity of the robot
+ :return: linear_velocity, angular_velocity
+ """
+ return self.linear_velocity, self.angular_velocity
+
+ def reset_pose(self, x: float, y: float, theta: float):
+ """
+ Resets the robot pose
+ :param x: x coordinate of the robot
+ :param y: y coordinate of the robot
+ :param theta: angle of the robot
+ :return:
+ """
+ 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) -> (float, float, float):
+ """
+ Returns the current pose of the robot
+ :return: x, y, theta
+ """
+ return self.x, self.y, self.theta
+
def set_servo_positions(self, a_position: int, b_position: int):
"""
Sets A/B servomotor angle
@@ -117,6 +280,13 @@ def set_servo_positions(self, a_position: int, b_position: int):
self.packeter.packetC2B(ord('S'), a_position & 0xFF, b_position & 0xFF)
uart.write(self.packeter.msg[0:self.packeter.msg_size])
+ def get_ack(self):
+ """
+ Resets and returns last acknowledgement
+ :return:
+ """
+ return self.last_ack
+
# def send_ack(self):
# self.packeter.packetC1B(ord('X'), ACK_)
# uart.write(self.packeter.msg[0:self.packeter.msg_size])
@@ -128,37 +298,31 @@ def _set_leds(self, led_state: int):
5->right_red 6->right_green 7->right_blue
:return:
"""
- self.led_state = led_state & 0xFF
- self.packeter.packetC1B(ord('L'), self.led_state)
+ 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):
- if self.led_state is None:
+ """
+ Turns on/off the builtin led
+ :param value:
+ :return:
+ """
+ if self.led_state[0] is None:
self._set_leds(0x00)
- self.led_state = self.led_state | 0b00000001 if value else self.led_state & 0b11111110
- self._set_leds(self.led_state)
+ 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):
- if self.led_state is None:
- self._set_leds(0x00)
- self.led_state = self.led_state | 0b00000010 if value else self.led_state & 0b11111101
- self._set_leds(self.led_state)
-
- def set_left_led_color(self, red: bool, green: bool, blue: bool):
- if self.led_state is None:
- self._set_leds(0x00)
- self.led_state = self.led_state | 0b00000100 if red else self.led_state & 0b11111011
- self.led_state = self.led_state | 0b00001000 if green else self.led_state & 0b11110111
- self.led_state = self.led_state | 0b00010000 if blue else self.led_state & 0b11101111
- self._set_leds(self.led_state)
-
- def set_right_led_color(self, red: bool, green: bool, blue: bool):
- if self.led_state is None:
+ """
+ Turns on/off the illuminator led
+ :param value:
+ :return:
+ """
+ if self.led_state[0] is None:
self._set_leds(0x00)
- self.led_state = self.led_state | 0b00100000 if red else self.led_state & 0b11011111
- self.led_state = self.led_state | 0b01000000 if green else self.led_state & 0b10111111
- self.led_state = self.led_state | 0b10000000 if blue else self.led_state & 0b01111111
- self._set_leds(self.led_state)
+ 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):
"""
@@ -169,7 +333,7 @@ def _update(self, delay_=1):
:return:
"""
while True:
- if not self._update_thread_running:
+ if not ArduinoAlvik._update_thread_running:
break
if self._read_message():
self._parse_message()
@@ -196,7 +360,7 @@ def _parse_message(self) -> int:
code = self.packeter.payload[0]
if code == ord('j'):
# joint speed
- _, self.l_speed, self.r_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()
@@ -205,7 +369,7 @@ def _parse_message(self) -> int:
_, self.red, self.green, self.blue = self.packeter.unpacketC3I()
elif code == ord('i'):
# imu
- _, ax, ay, az, gx, gy, 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()
@@ -225,6 +389,18 @@ def _parse_message(self) -> int:
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()
@@ -234,36 +410,72 @@ def _parse_message(self) -> int:
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)
- def get_color(self) -> (int, int, int):
+ def get_color_raw(self) -> (int, int, int):
"""
- Returns the RGB color readout
- :return:
+ Returns the color sensor's raw readout
+ :return: red, green, blue
"""
return self.red, self.green, self.blue
@@ -272,13 +484,160 @@ def get_color(self) -> (int, int, int):
# int((self.blue/COLOR_FULL_SCALE)*255))
def get_distance(self) -> (int, int, int, int, int, int):
+ """
+ Returns the distance readout of the TOF sensor
+ :return: left_tof, center_left_tof, center_tof, center_right_tof, right_tof
+ """
return self.left_tof, self.center_left_tof, self.center_tof, self.center_right_tof, self.right_tof
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):
+ """
+ Resets the wheel reference position
+ :param initial_position:
+ :return:
+ """
+ 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 not in angular_velocity_units:
+ return
+ elif unit == '%':
+ velocity = perc_to_rpm(velocity)
+
+ 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) -> float:
+ """
+ Returns the current RPM speed of the wheel
+ :return:
+ """
+ return self._speed
+
+ def get_position(self) -> float:
+ """
+ Returns the wheel position (angle with respect to the reference)
+ :return:
+ """
+ return self._position
+
+ 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:
+ """
+
+ if unit not in angle_units:
+ return
+ elif unit == 'rad':
+ position = rad_to_deg(position)
+
+ self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'), position)
+ 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])
+
+
+# Units and unit conversion methods
+
+angular_velocity_units = ['rpm', '%']
+angle_units = ['deg', 'rad']
+distance_units = ['mm', 'cm']
+
+
+def perc_to_rpm(percent: float) -> float:
+ """
+ Converts percent of max_rpm to rpm
+ :param percent:
+ :return:
+ """
+ return (percent / 100.0)*MOTOR_MAX_RPM
+
+
+def rad_to_deg(rad: float) -> float:
+ """
+ Converts radians to degrees
+ :param rad:
+ :return:
+ """
+ return rad*180/math.pi
diff --git a/examples/led_setting.py b/examples/leds_setting.py
similarity index 57%
rename from examples/led_setting.py
rename to examples/leds_setting.py
index cc2c005..cceb6c4 100644
--- a/examples/led_setting.py
+++ b/examples/leds_setting.py
@@ -3,17 +3,10 @@
import sys
alvik = ArduinoAlvik()
-
-alvik.run()
-sleep_ms(100)
-alvik.reset_hw()
+alvik.begin()
while True:
try:
- #alvik._set_leds(0xff)
- #sleep_ms(1000)
- #alvik._set_leds(0x00)
- #sleep_ms(1000)
alvik.set_builtin_led(1)
sleep_ms(1000)
alvik.set_illuminator(1)
@@ -22,18 +15,23 @@
sleep_ms(1000)
alvik.set_illuminator(0)
sleep_ms(1000)
- alvik.set_left_led_color(0,0,1)
+ alvik.left_led.set_color(0, 0, 1)
+ sleep_ms(1000)
+ alvik.left_led.set_color(0, 1, 0)
+ sleep_ms(1000)
+ alvik.left_led.set_color(1, 0, 0)
sleep_ms(1000)
- alvik.set_right_led_color(0,0,1)
+ alvik.left_led.set_color(1, 1, 1)
sleep_ms(1000)
- alvik.set_left_led_color(0,1,0)
+ alvik.right_led.set_color(0, 0, 1)
sleep_ms(1000)
- alvik.set_right_led_color(0,1,0)
+ alvik.right_led.set_color(0, 1, 0)
sleep_ms(1000)
- alvik.set_left_led_color(1,0,0)
+ alvik.right_led.set_color(1, 0, 0)
sleep_ms(1000)
- alvik.set_right_led_color(1,0,0)
+ alvik.right_led.set_color(1, 1, 1)
sleep_ms(1000)
except KeyboardInterrupt as e:
print('over')
+ alvik.stop()
sys.exit()
diff --git a/examples/message_reader.py b/examples/message_reader.py
index 89086f8..9c0a582 100644
--- a/examples/message_reader.py
+++ b/examples/message_reader.py
@@ -3,26 +3,27 @@
import sys
alvik = ArduinoAlvik()
+if alvik.begin() < 0:
+ sys.exit()
-alvik.run()
-sleep_ms(100)
-alvik.reset_hw()
speed = 0
while True:
try:
print(f'VER: {alvik.version}')
- print(f'LSP: {alvik.l_speed}')
- print(f'RSP: {alvik.r_speed}')
+ 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}')
- alvik.set_speeds(speed, speed)
+ alvik.set_wheels_speed(speed, speed)
speed = (speed + 1) % 60
sleep_ms(1000)
except KeyboardInterrupt as e:
print('over')
- alvik.set_speeds(0, 0)
- break
-sys.exit()
+ alvik.stop()
+ sys.exit()
+
diff --git a/examples/move_wheels.py b/examples/move_wheels.py
new file mode 100644
index 0000000..d6f6e7e
--- /dev/null
+++ b/examples/move_wheels.py
@@ -0,0 +1,36 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep_ms
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+
+while True:
+ try:
+ alvik.left_wheel.set_speed(10)
+ sleep_ms(1000)
+ print(f'LSP: {alvik.left_wheel.get_speed()}')
+ print(f'RSP: {alvik.right_wheel.get_speed()}')
+
+ alvik.right_wheel.set_speed(10)
+ sleep_ms(1000)
+
+ print(f'LSP: {alvik.left_wheel.get_speed()}')
+ print(f'RSP: {alvik.right_wheel.get_speed()}')
+
+ alvik.left_wheel.set_speed(20)
+ sleep_ms(1000)
+
+ print(f'LSP: {alvik.left_wheel.get_speed()}')
+ print(f'RSP: {alvik.right_wheel.get_speed()}')
+
+ alvik.right_wheel.set_speed(20)
+ sleep_ms(1000)
+
+ print(f'LSP: {alvik.left_wheel.get_speed()}')
+ print(f'RSP: {alvik.right_wheel.get_speed()}')
+
+ except KeyboardInterrupt as e:
+ print('over')
+ alvik.stop()
+ sys.exit()
diff --git a/examples/pose_example.py b/examples/pose_example.py
new file mode 100644
index 0000000..3f4f40e
--- /dev/null
+++ b/examples/pose_example.py
@@ -0,0 +1,69 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep_ms
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+
+while True:
+ try:
+
+ alvik.move(100.0)
+ print("on target after move")
+
+ alvik.move(50.0)
+ print("on target after move")
+
+ alvik.rotate(90.0)
+ print("on target after rotation")
+
+ alvik.rotate(-45.00)
+ print("on target after rotation")
+
+ x, y, theta = alvik.get_pose()
+ print(f'Current pose is x={x}, y={y} ,theta={theta}')
+
+ alvik.reset_pose(0, 0, 0)
+
+ x, y, theta = alvik.get_pose()
+ print(f'Updated pose is x={x}, y={y} ,theta={theta}')
+ sleep_ms(500)
+
+ print("___________NON-BLOCKING__________________")
+
+ alvik.move(50.0, blocking=False)
+ while not alvik.is_target_reached():
+ print(f"Not yet on target received:{alvik.last_ack}")
+ print("on target after move")
+
+ alvik.rotate(45.0, blocking=False)
+ while not alvik.is_target_reached():
+ print(f"Not yet on target received:{alvik.last_ack}")
+ print("on target after rotation")
+
+ alvik.move(100.0, blocking=False)
+ while not alvik.is_target_reached():
+ print(f"Not yet on target received:{alvik.last_ack}")
+ print("on target after move")
+
+ alvik.rotate(-90.00, blocking=False)
+ while not alvik.is_target_reached():
+ print(f"Not yet on target received:{alvik.last_ack}")
+ print("on target after rotation")
+
+ x, y, theta = alvik.get_pose()
+ print(f'Current pose is x={x}, y={y} ,theta={theta}')
+
+ alvik.reset_pose(0, 0, 0)
+
+ x, y, theta = alvik.get_pose()
+ print(f'Updated pose is x={x}, y={y} ,theta={theta}')
+ sleep_ms(500)
+
+ alvik.stop()
+ sys.exit()
+
+ except KeyboardInterrupt as e:
+ print('over')
+ alvik.stop()
+ sys.exit()
diff --git a/examples/read_color_sensor.py b/examples/read_color_sensor.py
index aa4d1a4..27636b2 100644
--- a/examples/read_color_sensor.py
+++ b/examples/read_color_sensor.py
@@ -3,18 +3,15 @@
import sys
alvik = ArduinoAlvik()
-
-alvik.run()
-sleep_ms(1000)
-alvik.reset_hw()
+alvik.begin()
speed = 0
while True:
try:
- r, g, b = alvik.get_color()
+ r, g, b = alvik.get_color_raw()
print(f'RED: {r}, Green: {g}, Blue: {b}')
sleep_ms(100)
except KeyboardInterrupt as e:
print('over')
- break
-sys.exit()
+ alvik.stop()
+ sys.exit()
diff --git a/examples/read_imu.py b/examples/read_imu.py
new file mode 100644
index 0000000..9e82380
--- /dev/null
+++ b/examples/read_imu.py
@@ -0,0 +1,18 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep_ms
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+speed = 0
+
+while True:
+ try:
+ ax, ay, az = alvik.get_accelerations()
+ gx, gy, gz = alvik.get_gyros()
+ print(f'ax: {ax}, ay: {ay}, az: {az}, gx: {gx}, gy: {gy}, gz: {gz}')
+ sleep_ms(100)
+ except KeyboardInterrupt as e:
+ print('over')
+ alvik.stop()
+ sys.exit()
diff --git a/examples/read_touch.py b/examples/read_touch.py
index 8ebf4a2..9f7e183 100644
--- a/examples/read_touch.py
+++ b/examples/read_touch.py
@@ -3,10 +3,7 @@
import sys
alvik = ArduinoAlvik()
-
-alvik.run()
-sleep_ms(1000)
-alvik.reset_hw()
+alvik.begin()
speed = 0
while True:
@@ -30,5 +27,5 @@
sleep_ms(100)
except KeyboardInterrupt as e:
print('over')
- break
-sys.exit()
+ alvik.stop()
+ sys.exit()
diff --git a/examples/set_pid.py b/examples/set_pid.py
index a3d21b1..77909f9 100644
--- a/examples/set_pid.py
+++ b/examples/set_pid.py
@@ -3,19 +3,16 @@
import sys
alvik = ArduinoAlvik()
-
-alvik.run()
-sleep_ms(1000)
-alvik.reset_hw()
+alvik.begin()
speed = 0
while True:
try:
- alvik.set_pid('L', 10.0, 1.3, 4.2)
+ alvik.left_wheel.set_pid_gains(10.0, 1.3, 4.2)
sleep_ms(100)
- alvik.set_pid('R', 4.0, 13, 1.9)
+ alvik.right_wheel.set_pid_gains(4.0, 13, 1.9)
sleep_ms(100)
except KeyboardInterrupt as e:
print('over')
- break
-sys.exit()
+ alvik.stop()
+ sys.exit()
diff --git a/examples/wheels_servo.py b/examples/wheels_servo.py
new file mode 100644
index 0000000..b4220dd
--- /dev/null
+++ b/examples/wheels_servo.py
@@ -0,0 +1,36 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep
+import sys
+
+alvik = ArduinoAlvik()
+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/move_example.py b/examples/wheels_speed.py
similarity index 60%
rename from examples/move_example.py
rename to examples/wheels_speed.py
index ffc674b..18c9e46 100644
--- a/examples/move_example.py
+++ b/examples/wheels_speed.py
@@ -3,23 +3,19 @@
import sys
alvik = ArduinoAlvik()
-
-alvik.run()
-sleep_ms(100)
-alvik.reset_hw()
-
+alvik.begin()
while True:
try:
- alvik.set_speeds(10, 10)
+ alvik.set_wheels_speed(10, 10)
sleep_ms(1000)
- alvik.set_speeds(30, 60)
+ alvik.set_wheels_speed(30, 60)
sleep_ms(1000)
- alvik.set_speeds(60, 30)
+ alvik.set_wheels_speed(60, 30)
sleep_ms(1000)
except KeyboardInterrupt as e:
print('over')
- alvik.set_speeds(0, 0)
+ alvik.stop()
sys.exit()
diff --git a/install.bat b/install.bat
index e4b41d1..7871cc1 100644
--- a/install.bat
+++ b/install.bat
@@ -19,11 +19,13 @@ if /i "%1"=="-h" (
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 :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 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
echo Installing dependencies
diff --git a/install.sh b/install.sh
index 3e1821e..8129e2c 100644
--- a/install.sh
+++ b/install.sh
@@ -45,11 +45,13 @@ fi
$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 :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 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
echo "Installing dependencies"
diff --git a/package.json b/package.json
index 3944440..556f12b 100644
--- a/package.json
+++ b/package.json
@@ -3,5 +3,5 @@
],
"deps": [
],
- "version": "0.0.7"
+ "version": "0.1.0"
}
\ No newline at end of file
diff --git a/pinout_definitions.py b/pinout_definitions.py
index b10a06a..a24e3d9 100644
--- a/pinout_definitions.py
+++ b/pinout_definitions.py
@@ -1,7 +1,17 @@
from machine import Pin
# NANO to STM32 PINS
-D2 = 5 # ESP32 pin5 -> nano D2
-D3 = 6 # ESP32 pin6 -> nano D3
-BOOT0_STM32 = Pin(D2, Pin.OUT) # nano D2 -> STM32 Boot0
-RESET_STM32 = Pin(D3, Pin.OUT) # nano D2 -> STM32 NRST
+D2 = 5 # ESP32 pin5 -> nano D2
+D3 = 6 # ESP32 pin6 -> nano D3
+D4 = 7 # ESP32 pin7 -> nano D4
+
+A4 = 11 # ESP32 pin11 SDA -> nano A4
+A5 = 12 # ESP32 pin12 SCL -> nano A5
+A6 = 13 # ESP32 pin13 -> nano A6/D23
+
+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 NRST
+CHECK_STM32 = Pin(A6, Pin.IN, Pin.PULL_UP) # nano A6/D23 -> STM32 ROBOT_CHK
+ESP32_SDA = Pin(A4, Pin.OUT)
+ESP32_SCL = Pin(A5, Pin.OUT)
diff --git a/robot_definitions.py b/robot_definitions.py
new file mode 100644
index 0000000..610378e
--- /dev/null
+++ b/robot_definitions.py
@@ -0,0 +1,8 @@
+# Motor control and mechanical parameters
+MOTOR_KP_DEFAULT = 32.0
+MOTOR_KI_DEFAULT = 450.0
+MOTOR_KD_DEFAULT = 0.0
+MOTOR_MAX_RPM = 70
+
+# Wheels parameters
+WHEEL_DIAMETER_MM = 34