Skip to content

Wheels pos ack #25

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 4 commits into from
Aug 12, 2024
Merged

Wheels pos ack #25

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4192462
feat: alvik wheels set position acknowledgment
eigen-value Aug 12, 2024
43ceb09
fix: wheels_position.py
eigen-value Aug 12, 2024
5b91e51
ex: wheels_position.py
eigen-value Aug 12, 2024
06fa130
Merge remote-tracking branch 'refs/remotes/origin/dev' into wheels-po…
eigen-value Aug 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
38 changes: 28 additions & 10 deletions arduino_alvik/arduino_alvik.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,8 @@ def __new__(cls):
def __init__(self):
self.i2c = _ArduinoAlvikI2C(A4, A5)
self._packeter = ucPack(200)
self.left_wheel = _ArduinoAlvikWheel(self._packeter, ord('L'))
self.right_wheel = _ArduinoAlvikWheel(self._packeter, ord('R'))
self.left_wheel = _ArduinoAlvikWheel(self._packeter, ord('L'), alvik=self)
self.right_wheel = _ArduinoAlvikWheel(self._packeter, ord('R'), alvik=self)
self._servo_positions = list((None, None,))
self.servo_A = _ArduinoAlvikServo(self._packeter, 'A', 0, self._servo_positions)
self.servo_B = _ArduinoAlvikServo(self._packeter, 'B', 1, self._servo_positions)
Expand Down Expand Up @@ -394,17 +394,22 @@ def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'r
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'):
def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = 'deg', blocking: bool = True):
"""
Sets left/right motor angle
:param left_angle:
:param right_angle:
:param unit: the speed unit of measurement (default: 'rpm')
:param blocking:
:return:
"""
self._packeter.packetC2F(ord('A'), convert_angle(left_angle, unit, 'deg'),
convert_angle(right_angle, unit, 'deg'))
left_angle = convert_angle(left_angle, unit, 'deg')
right_angle = convert_angle(right_angle, unit, 'deg')
self._packeter.packetC2F(ord('A'), left_angle, right_angle)
uart.write(self._packeter.msg[0:self._packeter.msg_size])
self._waiting_ack = ord('P')
if blocking:
self._wait_for_target(idle_time=(max(left_angle, right_angle) / MOTOR_CONTROL_DEG_S))

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

class _ArduinoAlvikWheel:

def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEEL_DIAMETER_MM):
def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEEL_DIAMETER_MM,
alvik: ArduinoAlvik = None):
self._packeter = packeter
self._label = label
self._wheel_diameter_mm = wheel_diameter_mm
self._speed = None
self._position = None
self._alvik = alvik

def reset(self, initial_position: float = 0.0, unit: str = 'deg'):
"""
Expand Down Expand Up @@ -1538,16 +1545,27 @@ def get_position(self, unit: str = 'deg') -> float | None:
"""
return convert_angle(self._position, 'deg', unit)

def set_position(self, position: float, unit: str = 'deg'):
def set_position(self, position: float, unit: str = 'deg', blocking: bool = True):
"""
Sets left/right motor speed
:param position: the speed of the motor
:param position: the position of the motor
:param unit: the unit of measurement
:param blocking:
:return:
"""
self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'),
convert_angle(position, unit, 'deg'))
position = convert_angle(position, unit, 'deg')
self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'), position)
uart.write(self._packeter.msg[0:self._packeter.msg_size])
self._alvik._waiting_ack = ord('P')
if blocking:
self._alvik._wait_for_target(idle_time=(position / MOTOR_CONTROL_DEG_S))

def is_target_reached(self):
"""
Checks if the target position is reached
:return:
"""
return self._alvik.is_target_reached()


class _ArduinoAlvikRgbLed:
Expand Down
11 changes: 4 additions & 7 deletions examples/pose_example.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,40 +32,37 @@
print("___________NON-BLOCKING__________________")

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)
print("on target after move")

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)
print("on target after rotation")

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)
print("on target after move")

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)
print("on target after rotation")

x, y, theta = alvik.get_pose()
print(f'Current pose is x(cm)={x}, y(cm)={y}, theta(deg)={theta}')
Expand Down
39 changes: 34 additions & 5 deletions examples/wheels_position.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
from arduino_alvik import ArduinoAlvik
from time import sleep
from time import sleep, sleep_ms
import sys

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

while True:
try:

alvik.set_wheels_position(45, 45)
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(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()}')

print("___________NON-BLOCKING__________________")

alvik.set_wheels_position(90, 90, blocking=False)
while not alvik.is_target_reached():
alvik.left_led.set_color(1, 0, 0)
alvik.right_led.set_color(1, 0, 0)
sleep_ms(200)
alvik.left_led.set_color(0, 0, 0)
alvik.right_led.set_color(0, 0, 0)
sleep_ms(200)
print(f'Wheels position reached: R:{alvik.right_wheel.get_position()} L:{alvik.left_wheel.get_position()}')

alvik.left_wheel.set_position(180, blocking=False)
while not alvik.left_wheel.is_target_reached():
alvik.left_led.set_color(1, 0, 0)
sleep_ms(200)
alvik.left_led.set_color(0, 0, 0)
sleep_ms(200)
print(f'Left wheel position reached: {alvik.left_wheel.get_position()}')

alvik.right_wheel.set_position(180, blocking=False)
while not alvik.right_wheel.is_target_reached():
alvik.right_led.set_color(1, 0, 0)
sleep_ms(200)
alvik.right_led.set_color(0, 0, 0)
sleep_ms(200)
print(f'Left wheel position reached: {alvik.right_wheel.get_position()}')

except KeyboardInterrupt as e:
print('over')
alvik.stop()
Expand Down