Merge pull request #23 from mumuhhh/v2_gpio

feat: GPIO compatible

Author: apirrone

.gitignore

@@ -1,2 +1,4 @@
 __pycache__/
-mini_bdx_runtime.egg-info/
+mini_bdx_runtime.egg-info/
+build
+dist

README.md

@@ -116,6 +116,13 @@ git checkout v2
 pip install -e .
 ```
 
+In Raspberry Pi 5, you need to perform the following operations
+
+```bash
+pip uninstall -y RPi.GPIO
+pip install lgpio
+```
+
 
 ## Test the IMU
 

mini_bdx_runtime/mini_bdx_runtime/antennas.py

@@ -1,74 +1,64 @@
-import RPi.GPIO as GPIO
-import numpy as np
+import board
+import pwmio
+import math
 import time
 
-LEFT_ANTENNA_PIN = 13
-RIGHT_ANTENNA_PIN = 12
+LEFT_ANTENNA_PIN = board.D13
+RIGHT_ANTENNA_PIN = board.D12
 LEFT_SIGN = 1
 RIGHT_SIGN = -1
+MIN_UPDATE_INTERVAL = 1 / 50  # 20ms
 
 
-class Antennas:
-    def __init__(self):
-
-        GPIO.setmode(GPIO.BCM)
-        GPIO.setup(LEFT_ANTENNA_PIN, GPIO.OUT)
-        GPIO.setup(RIGHT_ANTENNA_PIN, GPIO.OUT)
-
-        self.pwm1 = GPIO.PWM(LEFT_ANTENNA_PIN, 50)
-        self.pwm2 = GPIO.PWM(RIGHT_ANTENNA_PIN, 50)
+def value_to_duty_cycle(v):
+    pulse_width_ms = 1.5 + (v * 0.5)  # 1ms to 2ms
+    duty_cycle = int((pulse_width_ms / 20) * 65535)
+    return min(max(duty_cycle, 3277), 6553)
 
-        self.pwm1.start(0)
-        self.pwm2.start(0)
 
-    def map_input_to_angle(self, value):
-        return 90 + (value * 90)
+class Antennas:
+    def __init__(self):
+        neutral_duty = value_to_duty_cycle(0)
+        self.pwm_left = pwmio.PWMOut(LEFT_ANTENNA_PIN, frequency=50, duty_cycle=neutral_duty)
+        self.pwm_right = pwmio.PWMOut(RIGHT_ANTENNA_PIN, frequency=50, duty_cycle=neutral_duty)
 
     def set_position_left(self, position):
-        self.set_position(1, position, LEFT_SIGN)
+        self.set_position(self.pwm_left, position, LEFT_SIGN)
 
     def set_position_right(self, position):
-        self.set_position(2, position, RIGHT_SIGN)
-
-    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
-        """
+        self.set_position(self.pwm_right, position, RIGHT_SIGN)
 
+    def set_position(self, pwm, value, sign=1):
         # if value == 0:
         #     return
         if -1 <= value <= 1:
-            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)
-            elif servo == 2:
-                self.pwm2.ChangeDutyCycle(duty)
-            else:
-                print("Invalid servo number!")
-            # time.sleep(0.01)  # Allow time for movement
+            duty_cycle = value_to_duty_cycle(value * sign) # Convert value to duty cycle (1ms-2ms)
+            pwm.duty_cycle = duty_cycle
         else:
             print("Invalid input! Enter a value between -1 and 1.")
 
     def stop(self):
-        self.pwm1.stop()
-        self.pwm2.stop()
-        GPIO.cleanup()
+        time.sleep(MIN_UPDATE_INTERVAL)
+        self.set_position_left(0)
+        self.set_position_right(0)
+        time.sleep(MIN_UPDATE_INTERVAL)
+        self.pwm_left.deinit()
+        self.pwm_right.deinit()
 
 
 if __name__ == "__main__":
     antennas = Antennas()
 
-    s = time.time()
-    while True:
-        antennas.set_position_left(np.sin(2 * np.pi * 1 * time.time()))
-        antennas.set_position_right(np.sin(2 * np.pi * 1 * time.time()))
+    try:
+        start_time = time.monotonic()
+        current_time = start_time
 
-        time.sleep(1 / 50)
+        while current_time - start_time < 5:
+            value = math.sin(2 * math.pi * 1 * current_time)
+            antennas.set_position_left(value)
+            antennas.set_position_right(value)
+            time.sleep(MIN_UPDATE_INTERVAL)
+            current_time = time.monotonic()
 
-        if time.time() - s > 5:
-            break
-        
+    finally:
+        antennas.stop()

mini_bdx_runtime/mini_bdx_runtime/eyes.py

@@ -1,40 +1,58 @@
-import RPi.GPIO as GPIO
-import numpy as np
+import board
+import digitalio
+import random
 import time
-from threading import Thread
+from threading import Thread, Event
 
-LEFT_EYE_GPIO = 24
-RIGHT_EYE_GPIO = 23
+LEFT_EYE_PIN = board.D24
+RIGHT_EYE_PIN = board.D23
 
 
 class Eyes:
-    def __init__(self):
-        GPIO.setmode(GPIO.BCM)
-        GPIO.setwarnings(False)
-        GPIO.setup(RIGHT_EYE_GPIO, GPIO.OUT)
-        GPIO.setup(LEFT_EYE_GPIO, GPIO.OUT)
+    def __init__(self, blink_duration=0.1, min_interval=1.0, max_interval=4.0):
+        self.left_eye = digitalio.DigitalInOut(LEFT_EYE_PIN)
+        self.left_eye.direction = digitalio.Direction.OUTPUT
 
-        GPIO.output(RIGHT_EYE_GPIO, GPIO.HIGH)
-        GPIO.output(LEFT_EYE_GPIO, GPIO.HIGH)
+        self.right_eye = digitalio.DigitalInOut(RIGHT_EYE_PIN)
+        self.right_eye.direction = digitalio.Direction.OUTPUT
 
-        self.blink_duration = 0.1
+        self.blink_duration = blink_duration
+        self.min_interval = min_interval
+        self.max_interval = max_interval
 
-        Thread(target=self.run, daemon=True).start()
+        self._stop_event = Event()
+        self._thread = Thread(target=self.run, daemon=True)
+        self._thread.start()
 
-    def run(self):
-        while True:
-            GPIO.output(RIGHT_EYE_GPIO, GPIO.LOW)
-            GPIO.output(LEFT_EYE_GPIO, GPIO.LOW)
-            time.sleep(self.blink_duration)
-            GPIO.output(RIGHT_EYE_GPIO, GPIO.HIGH)
-            GPIO.output(LEFT_EYE_GPIO, GPIO.HIGH)
-
-            next_blink = np.random.rand() * 4  # seconds
+    def _set_eyes(self, state):
+        self.left_eye.value = state
+        self.right_eye.value = state
 
-            time.sleep(next_blink)
+    def run(self):
+        try:
+            while not self._stop_event.is_set():
+                self._set_eyes(False)
+                time.sleep(self.blink_duration)
+                self._set_eyes(True)
+                next_blink = random.uniform(self.min_interval, self.max_interval)
+                time.sleep(next_blink)
+        except Exception as err:
+            print(f"Error in eye thread: {err}")
+            self._stop_event.set()
+
+    def stop(self):
+        self._stop_event.set()
+        self._thread.join()
+        self._set_eyes(False)
+        self.left_eye.deinit()
+        self.right_eye.deinit()
 
 
 if __name__ == "__main__":
-	e = Eyes()
-	while True:
-		time.sleep(1)
+    e = Eyes()
+    try:
+        while True:
+            time.sleep(1)
+    finally:
+        e.stop()
+

mini_bdx_runtime/mini_bdx_runtime/feet_contacts.py

@@ -1,32 +1,34 @@
-import RPi.GPIO as GPIO
-import numpy as np
-
-
-LEFT_FOOT_PIN = 22
-RIGHT_FOOT_PIN = 27
+import board
+import digitalio
+import time
 
+LEFT_FOOT_PIN = board.D22
+RIGHT_FOOT_PIN = board.D27
 
 class FeetContacts:
     def __init__(self):
-        GPIO.setwarnings(False)  # Ignore warning for now
-        GPIO.setmode(GPIO.BCM)  # Use physical pin numbering
-        GPIO.setup(LEFT_FOOT_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
-        GPIO.setup(RIGHT_FOOT_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
+        self.left_foot = digitalio.DigitalInOut(LEFT_FOOT_PIN)
+        self.left_foot.direction = digitalio.Direction.INPUT
+        self.left_foot.pull = digitalio.Pull.UP
+
+        self.right_foot = digitalio.DigitalInOut(RIGHT_FOOT_PIN)
+        self.right_foot.direction = digitalio.Direction.INPUT
+        self.right_foot.pull = digitalio.Pull.UP
 
     def get(self):
-        left = False
-        right = False
-        if GPIO.input(LEFT_FOOT_PIN) == GPIO.LOW:
-            left = True
-        if GPIO.input(RIGHT_FOOT_PIN) == GPIO.LOW:
-            right = True
-        return np.array([left, right])
+        left = not self.left_foot.value
+        right = not self.right_foot.value
+        return [left, right]
 
+    def stop(self):
+        self.left_foot.deinit()
+        self.right_foot.deinit()
 
 if __name__ == "__main__":
-    import time
-
     feet_contacts = FeetContacts()
-    while True:
-        print(feet_contacts.get())
-        time.sleep(0.05)
+    try:
+        while True:
+            print(feet_contacts.get())
+            time.sleep(0.05)
+    finally:
+        feet_contacts.stop()

mini_bdx_runtime/mini_bdx_runtime/projector.py

@@ -1,29 +1,31 @@
-import RPi.GPIO as GPIO
+import board
+import digitalio
 import time
 
-PROJECTOR_GPIO = 25
+PROJECTOR_GPIO = board.D25
+
 
 class Projector:
     def __init__(self):
-        GPIO.setmode(GPIO.BCM)
-        GPIO.setwarnings(False)
-        GPIO.setup(PROJECTOR_GPIO, GPIO.OUT)
-
-        GPIO.output(PROJECTOR_GPIO, GPIO.LOW)
+        self.project = digitalio.DigitalInOut(PROJECTOR_GPIO)
+        self.project.direction = digitalio.Direction.OUTPUT
         self.on = False
 
     def switch(self):
         self.on = not self.on
 
-        if self.on:
-            GPIO.output(PROJECTOR_GPIO, GPIO.HIGH)
-        else:
-            GPIO.output(PROJECTOR_GPIO, GPIO.LOW)
+        self.project.value = self.on
+
+    def stop(self):
+        self.project.value = False
+        self.project.deinit()
 
 
 if __name__ == "__main__":
     p = Projector()
-    while True:
-
-        p.switch()
-        time.sleep(1)
+    try:
+        while True:
+            p.switch()
+            time.sleep(1)
+    finally:
+        p.stop()

scripts/v2_rl_walk_mujoco.py

@@ -330,6 +330,11 @@ def run(self):
         except KeyboardInterrupt:
             if self.duck_config.antennas:
                 self.antennas.stop()
+            if self.duck_config.eyes:
+                self.eyes.stop()
+            if self.duck_config.projector:
+                self.projector.stop()
+            self.feet_contacts.stop()
 
         if self.save_obs:
             pickle.dump(self.saved_obs, open("robot_saved_obs.pkl", "wb"))