Merge pull request #9 from arduino/dev

gbr1 · web-flow · commit 04f498a466a9 · 2024-02-16T16:44:47.000+01:00
0.2.0
diff --git a/arduino_alvik.py b/arduino_alvik.py
diff --git a/constants.py b/constants.py
@@ -1,3 +1,5 @@
 
 # COLOR SENSOR
 COLOR_FULL_SCALE = 4097
+WHITE_CAL = [444, 342, 345]
+BLACK_CAL = [153, 135, 123]
diff --git a/conversions.py b/conversions.py
@@ -0,0 +1,68 @@
+# MEASUREMENT UNITS CONVERSION #
+
+from math import pi
+
+
+def conversion_method(func):
+    def wrapper(*args, **kwargs):
+        try:
+            return func(*args, **kwargs)
+        except KeyError:
+            raise ConversionError(f'Cannot {func.__name__} from {args[1]} to {args[2]}')
+    return wrapper
+
+
+@conversion_method
+def convert_rotational_speed(value: float, from_unit: str, to_unit: str) -> float:
+    """
+    Converts a rotational speed value from one unit to another
+    :param value:
+    :param from_unit: unit of input value
+    :param to_unit: unit of output value
+    :return:
+    """
+    speeds = {'rpm': 1.0, 'deg/s': 1/6, 'rad/s': 60/(2*pi), 'rev/s': 60}
+    return value * speeds[from_unit.lower()] / speeds[to_unit.lower()]
+
+
+@conversion_method
+def convert_angle(value: float, from_unit: str, to_unit: str) -> float:
+    """
+    Converts an angle value from one unit to another
+    :param value:
+    :param from_unit: unit of input value
+    :param to_unit: unit of output value
+    :return:
+    """
+    angles = {'deg': 1.0, 'rad': 180/pi, 'rev': 360, 'revolution': 360, '%': 3.6, 'perc': 3.6}
+    return value * angles[from_unit.lower()] / angles[to_unit.lower()]
+
+
+@conversion_method
+def convert_distance(value: float, from_unit: str, to_unit: str) -> float:
+    """
+    Converts a distance value from one unit to another
+    :param value:
+    :param from_unit: unit of input value
+    :param to_unit: unit of output value
+    :return:
+    """
+    distances = {'cm': 1.0, 'mm': 0.1, 'm': 100, 'inch': 2.54, 'in': 2.54}
+    return value * distances[from_unit.lower()] / distances[to_unit.lower()]
+
+
+@conversion_method
+def convert_speed(value: float, from_unit: str, to_unit: str) -> float:
+    """
+    Converts a distance value from one unit to another
+    :param value:
+    :param from_unit: unit of input value
+    :param to_unit: unit of output value
+    :return:
+    """
+    distances = {'cm/s': 1.0, 'mm/s': 0.1, 'm/s': 100, 'inch/s': 2.54, 'in/s': 2.54}
+    return value * distances[from_unit.lower()] / distances[to_unit.lower()]
+
+
+class ConversionError(Exception):
+    pass
diff --git a/examples/hand_follower.py b/examples/hand_follower.py
@@ -4,15 +4,17 @@
 
 alvik = ArduinoAlvik()
 alvik.begin()
-speed = 0
+
+reference = 10.0
 
 while True:
     try:
-        alvik.left_wheel.set_pid_gains(10.0, 1.3, 4.2)
-        sleep_ms(100)
-        alvik.right_wheel.set_pid_gains(4.0, 13, 1.9)
+        L, CL, C, CR, R = alvik.get_distance()
+        print(f'C: {C}')
+        error = C - reference
+        alvik.set_wheels_speed(error*10, error*10)
         sleep_ms(100)
     except KeyboardInterrupt as e:
         print('over')
         alvik.stop()
-        sys.exit()
+        sys.exit()
diff --git a/examples/message_reader.py b/examples/message_reader.py
@@ -6,8 +6,6 @@
 if alvik.begin() < 0:
     sys.exit()
 
-speed = 0
-
 while True:
     try:
         print(f'VER: {alvik.version}')
diff --git a/examples/pose_example.py b/examples/pose_example.py
@@ -8,56 +8,48 @@
 while True:
     try:
 
-        alvik.move(100.0)
+        alvik.move(100.0, 'mm')
         print("on target after move")
 
-        alvik.move(50.0)
+        alvik.move(50.0, 'mm')
         print("on target after move")
 
-        alvik.rotate(90.0)
+        alvik.rotate(90.0, 'deg')
         print("on target after rotation")
 
-        alvik.rotate(-45.00)
+        alvik.rotate(-45.00, 'deg')
         print("on target after rotation")
 
         x, y, theta = alvik.get_pose()
-        print(f'Current pose is x={x}, y={y} ,theta={theta}')
+        print(f'Current pose is x(cm)={x}, y(cm)={y}, theta(deg)={theta}')
 
         alvik.reset_pose(0, 0, 0)
 
         x, y, theta = alvik.get_pose()
-        print(f'Updated pose is x={x}, y={y} ,theta={theta}')
+        print(f'Updated pose is x(cm)={x}, y(cm)={y}, theta(deg)={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}")
+        alvik.move(50.0, 'mm', blocking=False)
         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}")
+        alvik.rotate(45.0, 'deg', blocking=False)
         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}")
+        alvik.move(100.0, 'mm', blocking=False)
         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}")
+        alvik.rotate(-90.00, 'deg', blocking=False)
         print("on target after rotation")
 
         x, y, theta = alvik.get_pose()
-        print(f'Current pose is x={x}, y={y} ,theta={theta}')
+        print(f'Current pose is x(cm)={x}, y(cm)={y}, theta(deg)={theta}')
 
         alvik.reset_pose(0, 0, 0)
 
         x, y, theta = alvik.get_pose()
-        print(f'Updated pose is x={x}, y={y} ,theta={theta}')
+        print(f'Updated pose is x={x}, y={y}, theta(deg)={theta}')
         sleep_ms(500)
 
         alvik.stop()
diff --git a/examples/read_color_sensor.py b/examples/read_color_sensor.py
@@ -4,12 +4,13 @@
 
 alvik = ArduinoAlvik()
 alvik.begin()
-speed = 0
 
 while True:
     try:
-        r, g, b = alvik.get_color_raw()
-        print(f'RED: {r}, Green: {g}, Blue: {b}')
+        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)}')
         sleep_ms(100)
     except KeyboardInterrupt as e:
         print('over')
diff --git a/examples/read_imu.py b/examples/read_imu.py
@@ -4,7 +4,6 @@
 
 alvik = ArduinoAlvik()
 alvik.begin()
-speed = 0
 
 while True:
     try:
diff --git a/examples/read_tof.py b/examples/read_tof.py
@@ -0,0 +1,18 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep_ms
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+
+while True:
+    try:
+        L, CL, C, CR, R = alvik.get_distance()
+        T = alvik.get_distance_top()
+        B = alvik.get_distance_bottom()
+        print(f'T: {T} | B: {B} | L: {L} | CL: {CL} | C: {C} | CR: {CR} | R: {R}')
+        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
@@ -4,7 +4,6 @@
 
 alvik = ArduinoAlvik()
 alvik.begin()
-speed = 0
 
 while True:
     try:
diff --git a/examples/test_meas_units.py b/examples/test_meas_units.py
@@ -0,0 +1,124 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep_ms
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+
+while True:
+    try:
+
+        # -- LINEAR MOVEMENTS --
+
+        print("Move fw 0.05 m")
+        alvik.move(0.05, unit='m')
+        sleep_ms(2000)
+
+        print("Move fw 10 cm")
+        alvik.move(5, unit='cm')
+        sleep_ms(2000)
+
+        print("Move bw 100 mm")
+        alvik.move(-100, unit='mm')
+        sleep_ms(2000)
+
+        print("Move fw 1 inch")
+        alvik.move(1, unit='in')
+        sleep_ms(2000)
+
+        print(f"Current position: {alvik.get_pose()}")
+        alvik.reset_pose(0, 0, theta=3.1415, angle_unit='rad')
+        sleep_ms(2000)
+
+        print(f"Current position: {alvik.get_pose()}")
+
+        # -- WHEEL ROTATIONS --
+        alvik.right_wheel.reset()
+        sleep_ms(2000)
+        curr_pos = alvik.right_wheel.get_position()
+        print(f'R wheel pos: {curr_pos}')
+        sleep_ms(2000)
+
+        print("Rotate right wheel 25% fw")
+        alvik.right_wheel.set_position(25, unit='%')
+        sleep_ms(2000)
+        curr_pos = alvik.right_wheel.get_position()
+        print(f'R wheel pos: {curr_pos}')
+
+        print("Rotate right wheel 90 deg bw")
+        alvik.right_wheel.set_position(-90, unit='deg')
+        sleep_ms(2000)
+        curr_pos = alvik.right_wheel.get_position()
+        print(f'R wheel pos: {curr_pos}')
+
+        print("Rotate right wheel pi rad fw")
+        alvik.right_wheel.set_position(3.14, unit='rad')
+        sleep_ms(2000)
+        curr_pos = alvik.right_wheel.get_position()
+        print(f'R wheel pos: {curr_pos}')
+
+        print("Rotate right wheel a quarter revolution bw")
+        alvik.right_wheel.set_position(-0.25, unit='rev')
+        sleep_ms(2000)
+        curr_pos = alvik.right_wheel.get_position()
+        print(f'R wheel pos: {curr_pos}')
+
+        # -- WHEELS SPEED --
+        print("Set speed 50% max_rpm (35.0 rpm)")
+        alvik.set_wheels_speed(50, 50, '%')
+        sleep_ms(1000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        print("Set speed 12 rpm (1 rev in 5 sec)")
+        alvik.set_wheels_speed(12, 12, 'rpm')
+        sleep_ms(1000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        print("Set speed -pi rad/s (1 back rev in 2 sec)")
+        alvik.set_wheels_speed(-3.1415, -3.1415, 'rad/s')
+        sleep_ms(1000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        print("Set speed 180 deg/s (1 back rev in 2 sec)")
+        alvik.set_wheels_speed(180, 180, 'deg/s')
+        sleep_ms(1000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        # -- DRIVE --
+        print("Driving at 10 mm/s (expecting approx 5.6 rpm 64 deg/s)")
+        alvik.drive(10, 20, linear_unit='mm/s', angular_unit='%')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_drive_speed()} (mm/s, deg/s))")
+
+        print("Driving at 10 mm/s (expecting approx 5.6 rpm)")
+        alvik.drive(10, 0, linear_unit='mm/s')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        print("Driving at 2 cm/s (expecting approx 11.2 rpm)")
+        alvik.drive(2, 0, linear_unit='cm/s')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        print("Driving at 1 in/s (expecting approx 14 rpm)")
+        alvik.drive(1, 0, linear_unit='in/s')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        print("Driving at 5 mm/s (expecting approx 5.6 rpm) pi/8 rad/s (22.5 deg/s)")
+        alvik.drive(5, 3.1415/8, linear_unit='mm/s', angular_unit='rad/s')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_drive_speed()} (mm/s) (rpm)")
+
+        print("Driving at 5 mm/s (expecting approx 5.6 rpm) 1/8 rev/s (45 deg/s)")
+        alvik.drive(5, 1/8, linear_unit='mm/s', angular_unit='rev/s')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_drive_speed()} (mm/s) (rpm)")
+
+        alvik.stop()
+        sys.exit()
+
+    except KeyboardInterrupt as e:
+        print('over')
+        alvik.stop()
+        sys.exit()
diff --git a/examples/wheels_speed.py b/examples/wheels_speed.py
@@ -8,12 +8,15 @@
 while True:
     try:
         alvik.set_wheels_speed(10, 10)
+        print(f'Wheels speed: {alvik.get_wheels_speed()}')
         sleep_ms(1000)
 
         alvik.set_wheels_speed(30, 60)
+        print(f'Wheels speed: {alvik.get_wheels_speed()}')
         sleep_ms(1000)
 
         alvik.set_wheels_speed(60, 30)
+        print(f'Wheels speed: {alvik.get_wheels_speed()}')
         sleep_ms(1000)
     except KeyboardInterrupt as e:
         print('over')
diff --git a/install.bat b/install.bat
@@ -18,12 +18,14 @@ 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
diff --git a/install.sh b/install.sh
@@ -44,12 +44,14 @@ 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 :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
diff --git a/package.json b/package.json
diff --git a/pinout_definitions.py b/pinout_definitions.py
diff --git a/robot_definitions.py b/robot_definitions.py
diff --git a/uart.py b/uart.py
