Teleoperation Robot

The Teleoperation Robot is a differential drive robotic system that allows remote control via a keyboard interface. This project provides a Python-based teleoperation system, enabling users to control the robot’s movement in real-time. The robot is equipped with two independent motors and utilizes a PID controller for smooth and precise motion control.

About Tools and Materials:

2x SMD Red (Purchase Here)

SMD USB Gateway (Purchase Here)

Arduino Gateway Module (Purchase Here)

2x BDC Motor (Purchase Here)

Step 1: Hardware & Software Overview

Project Key Components

1. SMD

   The SMD acts as a bridge between the script and the modules. It is responsible for interpreting the commands sent by the script and translating them into actions that read input from the Ultrasonic Distance Sensor Module and meanwhile, actuate the motor for the continuous reading of the script.

2. BDC Motor

   The 100 RPM BDC Motor with Encoder is used to rotate the radar mechanism in a full circle. The user can precisely control the motor and get the position through the built-in encoder.

Key Features:

• Real-time teleoperation via keyboard inputs.

• PID-based motor control for smooth and stable movement.

• Cross-platform support (Windows, Linux, macOS).

• USB serial communication for motor control.

• Differential drive mechanism for precise navigation.

• Emergency stop function to halt movement immediately.

Step 2: Assemble

1. Hardware Setup

   • Connect the SMD to the PC or Arduino board using USB Gateway Module or Arduino Gateway Module.
   • Connect the 100 RPM BDC Motor with Encoder to the motor ports of the SMD using an RJ-45 cable.
   • Make sure that the SMD is powered and all connections are correct.

Project Wiring diagram

Step 3: Run & Test

1. Connect the Robot • Ensure that the robot is powered on and all connections are secure. • The USB connection should be properly established before running the script.

2. Run the Teleoperation Script • Open a terminal or command prompt. • Navigate to the directory where the script is located. •Run the Code.

3. Control the Robot Once the script is running, use the following keyboard controls to operate the robot: Key Action: W: Move forward S: Move backward A: Turn left D: Turn right Q: Quit the program • The script initializes a PID controller for both motors to ensure smooth operation. • It automatically detects the correct USB port for communication with the SMD module.

4. Verify Motion and Response • When pressing the movement keys, observe the robot’s response. • Check if the movements are smooth and consistent. • If any delays or unexpected behaviors occur, review the USB connection and the PID parameters.

5. Check for Errors • If the script fails to detect the USB port, it will display No suitable port found. Exiting... • Any communication or motor control issues will be printed as error messages in the terminal.

6. Stopping and Exiting • When pressing Q, the script will stop the motors and disable torque before exiting safely. • If the robot does not stop as expected, manually disconnect the power or reset the system.

Codes

from pynput import keyboard
import time
from smd.red import *
from serial.tools.list_ports import comports
from platform import system
class PIDController:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.previous_error = 0
        self.integral = 0

    def calculate(self, error, delta_time):
        self.integral += error * delta_time
        derivative = (error - self.previous_error) / delta_time if delta_time > 0 else 0
        output = (self.kp * error) + (self.ki * self.integral) + (self.kd * derivative)
        self.previous_error = error
        return max(min(output, 100), -100)  
    
    
def USB_Port():
    ports = list(comports())
    usb_names = {
        "Windows": ["USB Serial Port"],
        "Linux": ["/dev/ttyUSB"],
        "Darwin": [
            "/dev/tty.usbserial",
            "/dev/tty.usbmodem",
            "/dev/tty.SLAB_USBtoUART",
            "/dev/tty.wchusbserial",
            "/dev/cu.usbserial",
        ]
    }
    os_name = system()
    if ports:
        for port, desc, hwid in sorted(ports):
            if any(name in port or name in desc for name in usb_names.get(os_name, [])):
                print("Connected!")
                return port
        print("Available ports:")
        for port, desc, hwid in ports:
            print(f"Port: {port}, Description: {desc}, HWID: {hwid}")
    else:
        print("No ports detected!")
    return None


def teleoperate_smd():
    print("Use W/A/S/D to control the robot. Press Q to quit.")

    port = USB_Port()
    if not port:
        print("No suitable port found. Exiting...")
        return

    try:
        smd = Master(port)
        smd.attach(Red(0))  # Left motor (ID 0)
        smd.attach(Red(1))  # Right motor (ID 1)

        smd.enable_torque(0, 1)
        smd.enable_torque(1, 1)

        left_pid = PIDController(kp=24.96, ki=0.00, kd=19.10)
        right_pid = PIDController(kp=46.97, ki=0.00, kd=18.96)

        base_speed = 100  
        turning_speed = 100  # Speed for turning
        last_time = time.time()

        def on_press(key):
            nonlocal last_time

            try:
                # Calculate time difference
                current_time = time.time()
                delta_time = current_time - last_time
                last_time = current_time

                if hasattr(key, 'char'):
                    if key.char == 'w':  # Move forward
                        print("Move Forward")
                        error = base_speed
                        left_speed = left_pid.calculate(error, delta_time)
                        right_speed = right_pid.calculate(error, delta_time)
                        # Send commands to both motors simultaneously
                        smd.set_duty_cycle(0, -left_speed)  # Left motor forward
                        smd.set_duty_cycle(1, right_speed)  # Right motor forward
                    elif key.char == 's':  # Move backward
                        print("Move Backward")
                        error = -base_speed
                        left_speed = left_pid.calculate(error, delta_time)
                        right_speed = right_pid.calculate(error, delta_time)
                        # Send commands to both motors simultaneously
                        smd.set_duty_cycle(0, -left_speed)  # Left motor backward
                        smd.set_duty_cycle(1, right_speed)  # Right motor backward
                    elif key.char == 'a':  # Turn left
                        print("Turn Left")
                        # Send commands to both motors simultaneously
                        smd.set_duty_cycle(0, 0)  # Left motor stopped
                        smd.set_duty_cycle(1, turning_speed)  # Right motor forward
                    elif key.char == 'd':  # Turn right
                        print("Turn Right")
                        # Send commands to both motors simultaneously
                        smd.set_duty_cycle(0, -turning_speed)  # Left motor forward
                        smd.set_duty_cycle(1, 0)  # Right motor stopped
                    elif key.char == 'q':  # Quit
                        print("Exiting...")
                        return False
            except AttributeError:
                pass

        def on_release(key):
            # Stop both motors simultaneously
            smd.set_duty_cycle(0, 0)
            smd.set_duty_cycle(1, 0)

        # Start keyboard listener
        with keyboard.Listener(on_press=on_press, on_release=on_release) as listener:
            listener.join()

    except Exception as e:
        print(f"Error: {e}")
    finally:
        # Stop both motors simultaneously during cleanup
        smd.set_duty_cycle(0, 0)
        smd.set_duty_cycle(1, 0)
        smd.enable_torque(0, 0)
        smd.enable_torque(1, 0)
        smd.close()
        print("SMD connection closed.")

teleoperate_smd()