WIP bus servo management

config/chatgpt.yml

@@ -5,8 +5,10 @@ chatgpt:
     model: gpt-4o-mini
     persona: "You are a helpful assistant robot. You respond with short phrases where possible.
     Alternatively, you can respond with the following commands instead of text if you feel they are appropriate:
-    - animate:head_nod
-    - animate:head_shake"
+    - animate/head_nod
+    - animate/head_shake
+    - gpio/laser/on
+    - gpio/laser/off"
   dependencies:
     python:
       - openai

config/servo_waveshare.yml

@@ -0,0 +1,38 @@
+servos:
+  enabled: true
+  path: "modules.actuators.bus_servo.servo.Servo" # Include class name here
+  instances:
+    - name: "leg_r_hip"
+      model: 'ST3215'
+      id: 3
+      range: [744, 3136]
+      start: 1269
+      poses:
+        - stand: 824
+        - sit: 1269
+        - centre: 2048
+    - name: "leg_r_knee"
+      model: 'ST3215'
+      id: 2
+      range: [702, 3260]
+      start: 723
+      poses:
+        - stand: 2111
+        - sit: 723
+        - centre: 2048
+      # configure_on_boot: true
+    - name: "leg_r_ankle"
+      model: 'ST3215'
+      id: 1
+      range: [0, 4095]
+      start: 2048
+    - name: "neck_pan"
+      model: 'SC09'
+      id: 4
+      range: [0, 1023]
+      start: 511
+    - name: "neck_tilt"
+      model: 'SC09'
+      id: 5
+      range: [0, 1023]
+      start: 511

docs/BusServo.md

@@ -0,0 +1,42 @@
+# BaseModule Documentation
+
+## Overview
+
+Serial bus servos allow for efficient communication and control of multiple servos over a single bus. The `Servo` class provides an interface to manage these servos, including setting positions, speeds, and handling configurations.
+
+## Configuration
+
+The `config/servo_waveshare.yml` file contains the configuration for each bus servo. The `instances` section defines the servos, their IDs, and their initial positions. The `poses` section within each instance defines various poses that can be used to set servo positions.
+
+## Getting Started
+
+To calibrate the servos, each must have their ID set individually. To achieve this, connect one servo to the driver board and run `modules/actuators/bus_servo/change_id.py`. This script will prompt you to enter the ID for the connected servo, which will then be saved permanently on the servo.
+
+Once this has been done for all servos, you can use the `Servo` class to control them by enabling it in the above yaml file.
+
+To calibrate the servo positions, set the flag `configure_on_boot` to `true` in the configuration file for each instance (servo). This will cause the servo to output it's current position in the debug log, which can then be copied into the start position, or range. Servos can be manually moved to any position to identify their range or certain poses.
+
+Once the start position and range are set, move the flag to the next servo and repeat the process until all servos are configured.
+
+Finally, remove the `configure_on_boot` flag from the configuration file and re-run the program to start using the servos with their configured positions.
+
+## Subsriptions
+
+The `Servo` class subscribes to the following topics:
+`servo:<identifier>:mv` - to move the servo to a specific position relative to it's current position.
+`servo:<identifier>:mvabs` - to move the servo to a specific position with absolute values.
+
+## Smooth initialization
+
+Because the `Servo` class gets the current position of the servo on initialization, there is no danger of a servo jumping from an unknown position to the start position. This is especially useful when the servos are powered on in a random position and is an advantage over hobby servos used in previous versions of the modular biped.
+
+## SC vs ST servos
+
+The `Servo` class supports both ST and SC series servos from Waveshare. The type of servo is determined by the `model` variable in the configuration file. The class will automatically use the appropriate SDK for the specified servo type.
+
+There are some limitations to the SC servos as they do not support continuous rotation and have a lower rotational range compared to the ST servos. 
+
+## References
+https://www.waveshare.com/wiki/ST3215_Servo
+https://www.waveshare.com/wiki/SC09_Servo
+https://www.waveshare.com/wiki/Bus_Servo_Adapter_(A)

modules/actuators/bus_servo/SCServo_examples/ping.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+#
+# *********     Ping Example      *********
+#
+#
+# Available SCServo model on this example : All models using Protocol SCS
+# This example is tested with a SCServo(STS/SMS/SCS), and an URT
+# Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000)
+#
+
+import os
+
+if os.name == 'nt':
+    import msvcrt
+    def getch():
+        return msvcrt.getch().decode()
+else:
+    import sys, tty, termios
+    fd = sys.stdin.fileno()
+    old_settings = termios.tcgetattr(fd)
+    def getch():
+        try:
+            tty.setraw(sys.stdin.fileno())
+            ch = sys.stdin.read(1)
+        finally:
+            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
+        return ch
+
+from scservo_sdk import *                 # Uses SCServo SDK library
+
+# Default setting
+SCS_ID                  = 1                 # SCServo ID : 1
+BAUDRATE                = 115200            # Driver board default baudrate : 115200
+DEVICENAME              = '/dev/ttyUSB0'    # Check which port is being used on your controller
+                                            # ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
+
+protocol_end            = 1                 # SCServo bit end(STS/SMS=0, SCS=1)
+
+# Initialize PortHandler instance
+# Set the port path
+# Get methods and members of PortHandlerLinux or PortHandlerWindows
+portHandler = PortHandler(DEVICENAME)
+
+# Initialize PacketHandler instance
+# Get methods and members of Protocol
+packetHandler = PacketHandler(protocol_end)
+
+# Open port
+if portHandler.openPort():
+    print("Succeeded to open the port")
+else:
+    print("Failed to open the port")
+    print("Press any key to terminate...")
+    getch()
+    quit()
+
+
+# Set port baudrate
+if portHandler.setBaudRate(BAUDRATE):
+    print("Succeeded to change the baudrate")
+else:
+    print("Failed to change the baudrate")
+    print("Press any key to terminate...")
+    getch()
+    quit()
+
+# Try to ping the SCServo
+# Get SCServo model number
+scs_model_number, scs_comm_result, scs_error = packetHandler.ping(portHandler, SCS_ID)
+if scs_comm_result != COMM_SUCCESS:
+    print("%s" % packetHandler.getTxRxResult(scs_comm_result))
+elif scs_error != 0:
+    print("%s" % packetHandler.getRxPacketError(scs_error))
+else:
+    print("[ID:%03d] ping Succeeded. SCServo model number : %d" % (SCS_ID, scs_model_number))
+
+# Close port
+portHandler.closePort()

modules/actuators/bus_servo/SCServo_examples/read_write.py

@@ -0,0 +1,138 @@
+#!/usr/bin/env python
+#
+# *********     Gen Write Example      *********
+#
+#
+# Available SCServo model on this example : All models using Protocol SCS
+# This example is tested with a SCServo(STS/SMS/SCS), and an URT
+# Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000)
+#
+
+import os
+
+if os.name == 'nt':
+    import msvcrt
+    def getch():
+        return msvcrt.getch().decode()
+
+else:
+    import sys, tty, termios
+    fd = sys.stdin.fileno()
+    old_settings = termios.tcgetattr(fd)
+    def getch():
+        try:
+            tty.setraw(sys.stdin.fileno())
+            ch = sys.stdin.read(1)
+        finally:
+            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
+        return ch
+
+from scservo_sdk import *                    # Uses SCServo SDK library
+
+# Control table address
+ADDR_SCS_TORQUE_ENABLE     = 40
+ADDR_SCS_GOAL_ACC          = 41
+ADDR_SCS_GOAL_POSITION     = 42
+ADDR_SCS_GOAL_SPEED        = 46
+ADDR_SCS_PRESENT_POSITION  = 56
+
+# Default setting
+SCS_ID                      = 1                 # SCServo ID : 1
+BAUDRATE                    = 115200            # Driver board default baudrate : 115200
+DEVICENAME                  = '/dev/ttyUSB0'    # Check which port is being used on your controller
+                                                # ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
+
+SCS_MINIMUM_POSITION_VALUE  = 100         # SCServo will rotate between this value
+SCS_MAXIMUM_POSITION_VALUE  = 4000        # and this value (note that the SCServo would not move when the position value is out of movable range. Check e-manual about the range of the SCServo you use.)
+SCS_MOVING_STATUS_THRESHOLD = 20          # SCServo moving status threshold
+SCS_MOVING_SPEED            = 0           # SCServo moving speed
+SCS_MOVING_ACC              = 0           # SCServo moving acc
+protocol_end                = 1           # SCServo bit end(STS/SMS=0, SCS=1)
+
+index = 0
+scs_goal_position = [SCS_MINIMUM_POSITION_VALUE, SCS_MAXIMUM_POSITION_VALUE]         # Goal position
+
+
+# Initialize PortHandler instance
+# Set the port path
+# Get methods and members of PortHandlerLinux or PortHandlerWindows
+portHandler = PortHandler(DEVICENAME)
+
+# Initialize PacketHandler instance
+# Get methods and members of Protocol
+packetHandler = PacketHandler(protocol_end)
+
+# Open port
+if portHandler.openPort():
+    print("Succeeded to open the port")
+else:
+    print("Failed to open the port")
+    print("Press any key to terminate...")
+    getch()
+    quit()
+
+# Set port baudrate
+if portHandler.setBaudRate(BAUDRATE):
+    print("Succeeded to change the baudrate")
+else:
+    print("Failed to change the baudrate")
+    print("Press any key to terminate...")
+    getch()
+    quit()
+
+# Write SCServo acc
+scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_ACC, SCS_MOVING_ACC)
+if scs_comm_result != COMM_SUCCESS:
+    print("%s" % packetHandler.getTxRxResult(scs_comm_result))
+elif scs_error != 0:
+    print("%s" % packetHandler.getRxPacketError(scs_error))
+
+# Write SCServo speed
+scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_SPEED, SCS_MOVING_SPEED)
+if scs_comm_result != COMM_SUCCESS:
+    print("%s" % packetHandler.getTxRxResult(scs_comm_result))
+elif scs_error != 0:
+    print("%s" % packetHandler.getRxPacketError(scs_error))
+
+while 1:
+    print("Press any key to continue! (or press ESC to quit!)")
+    if getch() == chr(0x1b):
+        break
+
+    # Write SCServo goal position
+    scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_POSITION, scs_goal_position[index])
+    if scs_comm_result != COMM_SUCCESS:
+        print("%s" % packetHandler.getTxRxResult(scs_comm_result))
+    elif scs_error != 0:
+        print("%s" % packetHandler.getRxPacketError(scs_error))
+
+    while 1:
+        # Read SCServo present position
+        scs_present_position_speed, scs_comm_result, scs_error = packetHandler.read4ByteTxRx(portHandler, SCS_ID, ADDR_SCS_PRESENT_POSITION)
+        if scs_comm_result != COMM_SUCCESS:
+            print(packetHandler.getTxRxResult(scs_comm_result))
+        elif scs_error != 0:
+            print(packetHandler.getRxPacketError(scs_error))
+
+        scs_present_position = SCS_LOWORD(scs_present_position_speed)
+        scs_present_speed = SCS_HIWORD(scs_present_position_speed)
+        print("[ID:%03d] GoalPos:%03d PresPos:%03d PresSpd:%03d" 
+              % (SCS_ID, scs_goal_position[index], scs_present_position, SCS_TOHOST(scs_present_speed, 15)))
+
+        if not (abs(scs_goal_position[index] - scs_present_position_speed) > SCS_MOVING_STATUS_THRESHOLD):
+            break
+
+
+    # Change goal position
+    if index == 0:
+        index = 1
+    else:
+        index = 0    
+
+scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS_ID, ADDR_SCS_TORQUE_ENABLE, 0)
+if scs_comm_result != COMM_SUCCESS:
+    print("%s" % packetHandler.getTxRxResult(scs_comm_result))
+elif scs_error != 0:
+    print("%s" % packetHandler.getRxPacketError(scs_error))
+# Close port
+portHandler.closePort()

modules/actuators/bus_servo/SCServo_examples/scservo_sdk/__init__.py

@@ -0,0 +1,6 @@
+#!/usr/bin/env python
+
+from .port_handler import *
+from .packet_handler import *
+from .group_sync_read import *
+from .group_sync_write import *