Arduino MKR WiFi 1010

ROS2

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
import serial
from datetime import datetime

class XboxControllerNode(Node):
    def __init__(self):
        super().__init__('xbox_controller_node')
        self.subscription = self.create_subscription(
            Joy,
            'joy',
            self.joy_callback,
            10)
        self.subscription  # prevent unused variable warning

        # Initialize serial communication
        self.serial_port = serial.Serial('/dev/ttyACM0', 115200, timeout=0.1)
        self.get_logger().info('Serial port initialized')

    def joy_callback(self, msg):
        current_time = datetime.now().strftime("%H:%M:%S.%f")
        self.get_logger().info(f"Time: {current_time} | Axes: {msg.axes}, Buttons: {msg.buttons}")

        # Motor velocities dictionary
        motor_velocities = {
            1: self.convert_left_axis_to_velocity(msg.axes[1]),  # Motor 1 (Left joystick Y-axis)
            2: self.convert_right_axis_to_velocity(msg.axes[4]),  # Motor 2 (Right joystick Y-axis)
            3: self.convert_dpad_to_velocity(msg.axes[6]),  # Motor 3 (D-pad left/right)
            4: self.convert_buttons_to_velocity(msg.buttons[2], msg.buttons[1]),  # Motor 4 (X/B buttons)
            5: self.convert_buttons_to_velocity(msg.buttons[3], msg.buttons[0]),  # Motor 5 (Y/A buttons)
            6: self.convert_triggers_to_velocity(msg.buttons[6], msg.buttons[7])  # Motor 6 (LT/RT buttons)
        }

        # Send the motor velocities to Arduino
        for motor_id, velocity in motor_velocities.items():
            command = f"{motor_id},{velocity}\n"
            self.serial_port.write(command.encode())
            self.get_logger().info(f"Sent command: {command} to motor {motor_id}")

    def convert_left_axis_to_velocity(self, axis_value):
        # Convert the left joystick axis value to a fixed slow speed
        if axis_value > 0.1:
            return 30  # Reduced speed
        elif axis_value < -0.1:
            return -30  # Reduced speed
        else:
            return 0

    def convert_right_axis_to_velocity(self, axis_value):
        # Convert the right joystick axis value to a fixed slow speed
        if axis_value > 0.1:
            return 30  # Reduced speed
        elif axis_value < -0.1:
            return -30  # Reduced speed
        else:
            return 0

    def convert_dpad_to_velocity(self, axis_value):
        # Convert D-pad axis value to motor velocity
        if axis_value > 0.1:
            return 50  # Reduced speed
        elif axis_value < -0.1:
            return -50  # Reduced speed
        else:
            return 0

    def convert_buttons_to_velocity(self, button_forward, button_backward):
        # Convert button presses to motor velocity
        if button_forward:
            return 30  # Reduced speed
        elif button_backward:
            return -30  # Reduced speed
        else:
            return 0

    def convert_triggers_to_velocity(self, trigger_forward, trigger_backward):
        # Convert trigger presses to motor velocity
        if trigger_forward:
            return 30  # Reduced speed
        elif trigger_backward:
            return -30  # Reduced speed
        else:
            return 0

def main(args=None):
    rclpy.init(args=args)
    node = XboxControllerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main() 

Arudino

#include <Dynamixel2Arduino.h>

#if defined(ARDUINO_OpenRB)
  #define DXL_SERIAL Serial1
  #define DEBUG_SERIAL Serial
  const int DXL_DIR_PIN = -1;
#else
  #error "Please define the board you are using"
#endif

const uint8_t DXL_IDs[] = {1, 2, 3, 4, 5, 6};  // Motor IDs
const float DXL_PROTOCOL_VERSION = 2.0;
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

using namespace ControlTableItem;

void setup() {
  DEBUG_SERIAL.begin(115200);
  while(!DEBUG_SERIAL); //

  dxl.begin(57600);
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  
  for (uint8_t id : DXL_IDs) {
    dxl.ping(id);
    dxl.torqueOff(id);
    dxl.setOperatingMode(id, OP_VELOCITY);
    dxl.torqueOn(id);
  }

  DEBUG_SERIAL.println("Dynamixel initialized");
}

void loop() {
  if (DEBUG_SERIAL.available() > 0) {
    String command = DEBUG_SERIAL.readStringUntil('\n');
    DEBUG_SERIAL.println("Received command: " + command);

    int motor_id = command.substring(0, command.indexOf(',')).toInt();
    int motor_velocity = command.substring(command.indexOf(',') + 1).toInt();

    DEBUG_SERIAL.print("Parsed motor ID: ");
    DEBUG_SERIAL.println(motor_id);
    DEBUG_SERIAL.print("Parsed motor velocity: ");
    DEBUG_SERIAL.println(motor_velocity);

    if (motor_id >= 1 && motor_id <= 6 && motor_velocity >= -100 && motor_velocity <= 100) { // Reduced speed range
      dxl.setGoalVelocity(motor_id, motor_velocity);
    }
  }

  delay(3);  // Reduced delay to improve response time
}