Manual Controller System for Motor Control

Introduction

This section explains the implementation of a manual control system for the boat using a Bluetooth-connected controller. For this project, a PS5 controller was used to provide input commands to the motors. It is important to note that during manual control, the boat cannot create maps due to robot localization issues, particularly during turning. The manual control system is primarily intended for retrieving the boat in scenarios where it cannot be reached manually.

---

Overview

The manual control system consists of the following components:

1. Controller Input: Receives joystick input from the connected PS5 controller via the joy topic.
2. Motor Commands: Converts joystick input into motor power values and publishes these to the ESC motors via the esc_topic.
3. ROS 2 Node: Implements the JoyToEsc node to handle input processing and message publishing.

---

Code Walkthrough

Node Initialization

The node JoyToEsc initializes two key components:

1. Subscriber: Subscribes to the joy topic to receive joystick input in the form of sensor_msgs/Joy messages.
2. Publisher: Publishes motor commands to the esc_topic using the custom orientation_msg/Esc message type.

---

Joystick Input Processing

1. Joystick Axes:

   • Vertical Axis (msg.axes[1]): Controls forward and backward motion (-1 for full reverse, 1 for full forward).
   • Horizontal Axis (msg.axes[0]): Controls left and right turns (-1 for full left, 1 for full right).

2. Vector Math for Motor Control:

   • The left and right motor powers are calculated using vector math:

     left_motor_power = (left_vertical - left_horizontal) / 2 * max_power
     right_motor_power = (left_vertical + left_horizontal) / 2 * max_power
     

     This ensures that:
     • Forward motion applies equal power to both motors.
     • Turning applies opposite power to the motors to create a pivot effect.

3. Normalization:

   • Motor power values are limited to the range [-max_power, max_power] to prevent exceeding the allowed range.

4. Conversion:

   • Motor power values are converted to integers to match the expected message type.

---

Publishing Motor Commands

1. Custom Esc Message:

   • The motor commands are published as two separate messages:
     • One for the left motor (motor_selection = 'Left').
     • One for the right motor (motor_selection = 'Right').

2. ESC Communication:

   • Each motor command is sent to the esc_topic where the ESC nodes process them to control the motors.

---

Challenges and Considerations

1. Localization Issues:

   • During manual control, turning movements disrupt robot localization, making it unsuitable for mapping.

2. Power Limitation:

   • The max_power parameter can be adjusted to limit motor power, ensuring safe operation during manual control.

3. Joystick Compatibility:

   • The system is designed for a PS5 controller but can be adapted to other controllers with similar button and axis mappings.

---

Code Implementation

#!/usr/bin/env python

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from orientation_msg.msg import Esc

class JoyToEsc(Node):

    def __init__(self):
        super().__init__('joy_to_esc')

        # Create subscriber to the 'joy' topic
        self.subscription = self.create_subscription(
            Joy,
            'joy',
            self.joy_callback,
            10)

        # Create publisher to send motor commands
        self.publisher = self.create_publisher(Esc, 'esc_topic', 10)

    def joy_callback(self, msg):
        
        # Joystick's vertical axis is axis[1] and horizontal axis is axis[0]
        left_vertical = msg.axes[1]  # Forward/backward control (-1 to 1)
        left_horizontal = msg.axes[0]  # Left/right control (-1 to 1)

        # Max motor power 
        max_power = 100  # Maximum motor power as a percentage (e.g., 100%)

        # Vector Math for controller to power 
        left_motor_power = (left_vertical - left_horizontal) / 2 * max_power
        right_motor_power = (left_vertical + left_horizontal) / 2 * max_power

        # Normalize the motor values if they exceed max range (-100 to 100)
        left_motor_power = max(min(left_motor_power, max_power), -max_power)
        right_motor_power = max(min(right_motor_power, max_power), -max_power)

        # Convert motor powers to integers to match the expected message type
        left_motor_power = int(left_motor_power)
        right_motor_power = int(right_motor_power)

        # Create and publish the message to control ESCs
        esc_msg = Esc()
        esc_msg.motor_selection = 'Left'
        esc_msg.power_percentage = left_motor_power
        self.publisher.publish(esc_msg)

        esc_msg.motor_selection = 'Right'
        esc_msg.power_percentage = right_motor_power
        self.publisher.publish(esc_msg)
        
        #self.get_logger().info(f'Published left motor: {left_motor_power}, right motor: {right_motor_power}')


def main(args=None):
    rclpy.init(args=args)
    node = JoyToEsc()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()