Teleoperation Robot

Below is an example Python script for teleoperating a differential drive robot:

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()