Skip to content

Commit 02529aa

Browse files
gbr1gbr1
authored
Merge pull request #25 from arduino/wheels-pos-ack
Wheels pos ack
2 parents d06cd42 + 06fa130 commit 02529aa

File tree

3 files changed

+66
-22
lines changed

3 files changed

+66
-22
lines changed

‎arduino_alvik/arduino_alvik.py‎

Lines changed: 28 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,8 @@ def __new__(cls):
2828
def __init__(self):
2929
self.i2c = _ArduinoAlvikI2C(A4, A5)
3030
self._packeter = ucPack(200)
31-
self.left_wheel = _ArduinoAlvikWheel(self._packeter, ord('L'))
32-
self.right_wheel = _ArduinoAlvikWheel(self._packeter, ord('R'))
31+
self.left_wheel = _ArduinoAlvikWheel(self._packeter, ord('L'), alvik=self)
32+
self.right_wheel = _ArduinoAlvikWheel(self._packeter, ord('R'), alvik=self)
3333
self._servo_positions = list((None, None,))
3434
self.servo_A = _ArduinoAlvikServo(self._packeter, 'A', 0, self._servo_positions)
3535
self.servo_B = _ArduinoAlvikServo(self._packeter, 'B', 1, self._servo_positions)
@@ -394,17 +394,22 @@ def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'r
394394
self._packeter.packetC2F(ord('J'), left_speed, right_speed)
395395
uart.write(self._packeter.msg[0:self._packeter.msg_size])
396396

397-
def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = 'deg'):
397+
def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = 'deg', blocking: bool = True):
398398
"""
399399
Sets left/right motor angle
400400
:param left_angle:
401401
:param right_angle:
402402
:param unit: the speed unit of measurement (default: 'rpm')
403+
:param blocking:
403404
:return:
404405
"""
405-
self._packeter.packetC2F(ord('A'), convert_angle(left_angle, unit, 'deg'),
406-
convert_angle(right_angle, unit, 'deg'))
406+
left_angle = convert_angle(left_angle, unit, 'deg')
407+
right_angle = convert_angle(right_angle, unit, 'deg')
408+
self._packeter.packetC2F(ord('A'), left_angle, right_angle)
407409
uart.write(self._packeter.msg[0:self._packeter.msg_size])
410+
self._waiting_ack = ord('P')
411+
if blocking:
412+
self._wait_for_target(idle_time=(max(left_angle, right_angle) / MOTOR_CONTROL_DEG_S))
408413

409414
def get_wheels_position(self, unit: str = 'deg') -> (float | None, float | None):
410415
"""
@@ -1465,12 +1470,14 @@ def get_position(self) -> int:
14651470

14661471
class _ArduinoAlvikWheel:
14671472

1468-
def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEEL_DIAMETER_MM):
1473+
def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEEL_DIAMETER_MM,
1474+
alvik: ArduinoAlvik = None):
14691475
self._packeter = packeter
14701476
self._label = label
14711477
self._wheel_diameter_mm = wheel_diameter_mm
14721478
self._speed = None
14731479
self._position = None
1480+
self._alvik = alvik
14741481

14751482
def reset(self, initial_position: float = 0.0, unit: str = 'deg'):
14761483
"""
@@ -1538,16 +1545,27 @@ def get_position(self, unit: str = 'deg') -> float | None:
15381545
"""
15391546
return convert_angle(self._position, 'deg', unit)
15401547

1541-
def set_position(self, position: float, unit: str = 'deg'):
1548+
def set_position(self, position: float, unit: str = 'deg', blocking: bool = True):
15421549
"""
15431550
Sets left/right motor speed
1544-
:param position: the speed of the motor
1551+
:param position: the position of the motor
15451552
:param unit: the unit of measurement
1553+
:param blocking:
15461554
:return:
15471555
"""
1548-
self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'),
1549-
convert_angle(position, unit, 'deg'))
1556+
position = convert_angle(position, unit, 'deg')
1557+
self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'), position)
15501558
uart.write(self._packeter.msg[0:self._packeter.msg_size])
1559+
self._alvik._waiting_ack = ord('P')
1560+
if blocking:
1561+
self._alvik._wait_for_target(idle_time=(position / MOTOR_CONTROL_DEG_S))
1562+
1563+
def is_target_reached(self):
1564+
"""
1565+
Checks if the target position is reached
1566+
:return:
1567+
"""
1568+
return self._alvik.is_target_reached()
15511569

15521570

15531571
class _ArduinoAlvikRgbLed:

‎examples/pose_example.py‎

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -32,40 +32,37 @@
3232
print("___________NON-BLOCKING__________________")
3333

3434
alvik.move(50.0, 'mm', blocking=False)
35-
print("on target after move")
3635

3736
while not alvik.is_target_reached():
3837
alvik.left_led.set_color(1, 0, 0)
3938
sleep_ms(500)
4039
alvik.left_led.set_color(0, 0, 0)
4140
sleep_ms(500)
41+
print("on target after move")
4242

4343
alvik.rotate(45.0, 'deg', blocking=False)
44-
print("on target after rotation")
45-
4644
while not alvik.is_target_reached():
4745
alvik.left_led.set_color(1, 0, 0)
4846
sleep_ms(500)
4947
alvik.left_led.set_color(0, 0, 0)
5048
sleep_ms(500)
49+
print("on target after rotation")
5150

5251
alvik.move(100.0, 'mm', blocking=False)
53-
print("on target after move")
54-
5552
while not alvik.is_target_reached():
5653
alvik.left_led.set_color(1, 0, 0)
5754
sleep_ms(500)
5855
alvik.left_led.set_color(0, 0, 0)
5956
sleep_ms(500)
57+
print("on target after move")
6058

6159
alvik.rotate(-90.00, 'deg', blocking=False)
62-
print("on target after rotation")
63-
6460
while not alvik.is_target_reached():
6561
alvik.left_led.set_color(1, 0, 0)
6662
sleep_ms(500)
6763
alvik.left_led.set_color(0, 0, 0)
6864
sleep_ms(500)
65+
print("on target after rotation")
6966

7067
x, y, theta = alvik.get_pose()
7168
print(f'Current pose is x(cm)={x}, y(cm)={y}, theta(deg)={theta}')

‎examples/wheels_position.py‎

Lines changed: 34 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
from arduino_alvik import ArduinoAlvik
2-
from time import sleep
2+
from time import sleep, sleep_ms
33
import sys
44

55
alvik = ArduinoAlvik()
@@ -10,26 +10,55 @@
1010

1111
while True:
1212
try:
13+
14+
alvik.set_wheels_position(45, 45)
15+
print(f'Left wheel degs: {alvik.left_wheel.get_position()}')
16+
print(f'Right wheel degs: {alvik.right_wheel.get_position()}')
17+
1318
alvik.left_wheel.set_position(30)
14-
sleep(2)
1519
print(f'Left wheel degs: {alvik.left_wheel.get_position()}')
1620
print(f'Right wheel degs: {alvik.right_wheel.get_position()}')
1721

1822
alvik.right_wheel.set_position(10)
19-
sleep(2)
2023
print(f'Left wheel degs: {alvik.left_wheel.get_position()}')
2124
print(f'Right wheel degs: {alvik.right_wheel.get_position()}')
2225

2326
alvik.left_wheel.set_position(180)
24-
sleep(2)
2527
print(f'Left wheel degs: {alvik.left_wheel.get_position()}')
2628
print(f'Right wheel degs: {alvik.right_wheel.get_position()}')
2729

2830
alvik.right_wheel.set_position(270)
29-
sleep(2)
3031
print(f'Left wheel degs: {alvik.left_wheel.get_position()}')
3132
print(f'Right wheel degs: {alvik.right_wheel.get_position()}')
3233

34+
print("___________NON-BLOCKING__________________")
35+
36+
alvik.set_wheels_position(90, 90, blocking=False)
37+
while not alvik.is_target_reached():
38+
alvik.left_led.set_color(1, 0, 0)
39+
alvik.right_led.set_color(1, 0, 0)
40+
sleep_ms(200)
41+
alvik.left_led.set_color(0, 0, 0)
42+
alvik.right_led.set_color(0, 0, 0)
43+
sleep_ms(200)
44+
print(f'Wheels position reached: R:{alvik.right_wheel.get_position()} L:{alvik.left_wheel.get_position()}')
45+
46+
alvik.left_wheel.set_position(180, blocking=False)
47+
while not alvik.left_wheel.is_target_reached():
48+
alvik.left_led.set_color(1, 0, 0)
49+
sleep_ms(200)
50+
alvik.left_led.set_color(0, 0, 0)
51+
sleep_ms(200)
52+
print(f'Left wheel position reached: {alvik.left_wheel.get_position()}')
53+
54+
alvik.right_wheel.set_position(180, blocking=False)
55+
while not alvik.right_wheel.is_target_reached():
56+
alvik.right_led.set_color(1, 0, 0)
57+
sleep_ms(200)
58+
alvik.right_led.set_color(0, 0, 0)
59+
sleep_ms(200)
60+
print(f'Left wheel position reached: {alvik.right_wheel.get_position()}')
61+
3362
except KeyboardInterrupt as e:
3463
print('over')
3564
alvik.stop()

0 commit comments

Comments
 (0)