rafaelcaricio/Open_Duck_Mini_Runtime
1
0
Fork 0
mirror of https://github.com/apirrone/Open_Duck_Mini_Runtime.git synced 2025-09-03 03:33:54 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

update

Browse source
This commit is contained in:
apirrone 2025-03-10 17:01:58 +01:00
parent 5e930a2f9a
commit 1b415c1b68
3 changed files with 57 additions and 42 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

68
mini_bdx_runtime/mini_bdx_runtime/antennas.py
View file

@ -1,50 +1,46 @@
import RPi.GPIO as GPIO
import time
LEFT_EAR_PIN = 12
RIGHT_EAR_PIN = 13
LEFT_ANTENNA_PIN = 13
RIGHT_ANTENNA_PIN = 12
LEFT_SIGN = 1
RIGHT_SIGN = -1
class EarServos:
class Antennas:
def __init__(self):
GPIO.setmode(GPIO.BCM)
GPIO.setup(LEFT_EAR_PIN, GPIO.OUT)
GPIO.setup(RIGHT_EAR_PIN, GPIO.OUT)
GPIO.setup(LEFT_ANTENNA_PIN, GPIO.OUT)
GPIO.setup(RIGHT_ANTENNA_PIN, GPIO.OUT)
# Create PWM objects for both servos (50Hz frequency)
self.pwm1 = GPIO.PWM(LEFT_EAR_PIN, 50)
self.pwm2 = GPIO.PWM(RIGHT_EAR_PIN, 50)
self.pwm1 = GPIO.PWM(LEFT_ANTENNA_PIN, 50)
self.pwm2 = GPIO.PWM(RIGHT_ANTENNA_PIN, 50)
self.pwm1.start(0)
self.pwm2.start(0)
def map_input_to_angle(self, value):
"""
Maps an input range of [-1, 1] to an angle range of [0°, 180°].
"""
return 90 + (value * 90) # Maps -1 to 0°, 0 to 90°, and 1 to 180°
return 90 + (value * 90)
def set_position_left(self):
"""
Moves the left ear servo to the left.
"""
self.set_position(1, -1)
def set_position_left(self, position):
self.set_position(1, position, LEFT_SIGN)
def set_position_right(self):
"""
Moves the right ear servo to the right.
"""
self.set_position(2, 1)
def set_position_right(self, position):
self.set_position(2, position, RIGHT_SIGN)
def set_position(self, servo, value):
def set_position(self, servo, value, sign=1):
"""
Moves the servo based on an input value in the range [-1, 1].
:param servo: 1 for the first servo, 2 for the second servo
:param value: A float between -1 and 1
"""
# if value == 0:
# return
if -1 <= value <= 1:
angle = self.map_input_to_angle(value)
angle = self.map_input_to_angle(value * sign)
duty = 2 + (angle / 18) # Convert angle to duty cycle (1ms-2ms)
if servo == 1:
self.pwm1.ChangeDutyCycle(duty)
@ -52,26 +48,24 @@ class EarServos:
self.pwm2.ChangeDutyCycle(duty)
else:
print("Invalid servo number!")
# time.sleep(0.3) # Allow time for movement
# time.sleep(0.01) # Allow time for movement
else:
print("Invalid input! Enter a value between -1 and 1.")
def stop(self):
"""Stops PWM and cleans up GPIO."""
self.pwm1.stop()
self.pwm2.stop()
GPIO.cleanup()
if __name__ == "__main__":
# Initialize the ServoController for GPIO12 and GPIO13
servo_control = EarServos()
# if __name__ == "__main__":
# servo_control = Antennas()
try:
while True:
servo_num = int(input("Enter servo number (1 or 2): "))
value = float(input("Enter position (-1 to 1): "))
servo_control.set_position(servo_num, value)
# try:
# while True:
# servo_num = int(input("Enter servo number (1 or 2): "))
# value = float(input("Enter position (-1 to 1): "))
# servo_control.set_position(servo_num, value)
except KeyboardInterrupt:
print("Stopping servos...")
servo_control.stop()
# except KeyboardInterrupt:
# print("Stopping servos...")
# servo_control.stop()

20
mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py
View file

@ -24,6 +24,8 @@ class XBoxController:
self.head_control_mode = self.standing
self.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.last_left_trigger = 0
self.last_right_trigger = 0
pygame.init()
self.p1 = pygame.joystick.Joystick(0)
self.p1.init()
@ -41,12 +43,23 @@ class XBoxController:
A_pressed = False
X_pressed = False
last_commands = self.last_commands
left_trigger = self.last_left_trigger
right_trigger = self.last_right_trigger
for event in pygame.event.get():
l_x = -1 * self.p1.get_axis(0)
l_y = -1 * self.p1.get_axis(1)
r_x = -1 * self.p1.get_axis(2)
r_y = -1 * self.p1.get_axis(3)
right_trigger = (self.p1.get_axis(4) + 1) / 2
left_trigger = (self.p1.get_axis(5) + 1) / 2
if left_trigger < 0.1:
left_trigger = 0
if right_trigger < 0.1:
right_trigger = 0
# print(f"Right trigger: {right_trigger}"
# f"Left trigger: {left_trigger}")
if not self.head_control_mode:
lin_vel_y = l_x
lin_vel_x = l_y
@ -114,17 +127,18 @@ class XBoxController:
pygame.event.pump() # process event queue
return np.around(last_commands, 3), A_pressed, X_pressed
return np.around(last_commands, 3), A_pressed, X_pressed, left_trigger, right_trigger
def get_last_command(self):
A_pressed = False
X_pressed = False
try:
self.last_commands, A_pressed, X_pressed = self.cmd_queue.get(False) # non blocking
self.last_commands, A_pressed, X_pressed, self.last_left_trigger, self.last_right_trigger = self.cmd_queue.get(False) # non blocking
except Exception:
pass
return self.last_commands, A_pressed, X_pressed
return self.last_commands, A_pressed, X_pressed, self.last_left_trigger, self.last_right_trigger
if __name__ == "__main__":

11
scripts/v2_rl_walk_mujoco.py
View file

@ -17,6 +17,7 @@ from mini_bdx_runtime.xbox_controller import XBoxController
from mini_bdx_runtime.feet_contacts import FeetContacts
from mini_bdx_runtime.eyes import Eyes
from mini_bdx_runtime.sounds import Sounds
from mini_bdx_runtime.antennas import Antennas
joints_order = [
"left_hip_yaw",
@ -118,8 +119,9 @@ class RLWalk:
self.paused = False
self.sounds = Sounds(volume=1.0, sound_directory="../mini_bdx_runtime/assets/")
self.antennas = Antennas()
self.command_freq = 10 # hz
self.command_freq = 20 # hz
if self.commands:
self.xbox_controller = XBoxController(self.command_freq, self.standing)
@ -219,16 +221,21 @@ class RLWalk:
while True:
A_pressed = False
X_pressed = False
left_trigger = 0
right_trigger = 0
t = time.time()
if self.commands:
self.last_commands, A_pressed, X_pressed = (
self.last_commands, A_pressed, X_pressed, left_trigger, right_trigger = (
self.xbox_controller.get_last_command()
)
if X_pressed:
self.sounds.play_random_sound()
self.antennas.set_position_left(right_trigger)
self.antennas.set_position_right(left_trigger)
if A_pressed and not self.paused:
self.paused = True
print("PAUSE")