Mouse Cursor Tracker Motion Robot

Key Features

• Robot Control: The robot's movement is controlled through a graphical interface using mouse clicks. The robot's position and orientation are updated based on user input.

• Motor Control: Two motors (left and right) drive the robot's forward, backward, and rotational movements.

• Path Tracking: The path followed by the robot during movement is displayed on the screen. Users can clear the path if needed.

• Target Movement: The robot moves towards a target point specified by the user with the mouse.

Technical Details

• USB Connection: The USB_Port() function detects a suitable USB device connected to the platform and establishes a connection.

• Qt Interface: A graphical user interface (GUI) built with PyQt5 enables users to visually control the robot.

• Dynamic Motor Speed Adjustment: Motor speeds are dynamically adjusted based on the distance to the target and angular differences.

• Reverse Mode: Users can enable reverse mode to make the robot move in the opposite direction.

Usage

1. When the application runs, it searches for a device connected to the USB port.

2. In the graphical interface, the robot is represented as a red rectangle (or green in reverse mode).

3. Users can set the robot's target by clicking on the desired position in the interface, and the robot will move towards it.

4. Control buttons like "Start," "Stop," "Clear," and "Reverse" allow users to manage the robot's movement.

Control Buttons

1. Start

   • Function: Initializes the robot's motors and begins the movement logic.

   • Use Case: Click this button to start the robot. The robot will move towards the target point set by clicking on the interface.

2. Stop

   • Function: Stops the robot's motors and halts movement.

   • Use Case: Use this button to pause the robot's movement at any time. The robot will remain stationary until "Start" is pressed again.

3. Clear

   • Function: Resets the robot's path and position.

   • Use Case: Click this button to clear the path drawn by the robot on the interface and reset its position to the center of the canvas. Useful for starting a new simulation.

4. Reverse

   • Function: Toggles the robot's movement direction between forward and reverse.

   • Use Case: Press this button to enable or disable reverse mode. When enabled, the robot will move backward relative to its target. The button text changes dynamically to indicate the current mode:

     • "Reverse": Forward movement mode is active.

     • "Forward": Reverse movement mode is active.

Code:

from smd.red import *
import time
import math
import sys
from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QPushButton, QVBoxLayout, QHBoxLayout
from PyQt5.QtGui import QPainter, QColor, QPen
from PyQt5.QtCore import Qt, QTimer
from serial.tools.list_ports import comports
from platform import system

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",
            "/dev/cu.usbmodem",
            "/dev/cu.SLAB_USBtoUART",
            "/dev/cu.wchusbserial",
        ]
    }
    
    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

class RobotCanvas(QWidget):
    def __init__(self):
        super().__init__()
        self.setMinimumSize(1200, 800)
        
        self.robot_x = 600
        self.robot_y = 400
        self.robot_angle = 0
        self.robot_size = 15
        self.path_points = []
        
        self.mouse_x = 600
        self.mouse_y = 400
        self.is_mouse_pressed = False
        self.is_reverse_mode = False
        
        self.mouse_target_distance = float('inf')

    def mousePressEvent(self, event):
        self.is_mouse_pressed = True
        self.mouse_x = event.x()
        self.mouse_y = event.y()

    def mouseReleaseEvent(self, event):
        self.is_mouse_pressed = False
        self.mouse_target_distance = float('inf')

    def mouseMoveEvent(self, event):
        if self.is_mouse_pressed:
            self.mouse_x = event.x()
            self.mouse_y = event.y()

    def paintEvent(self, event):
        painter = QPainter(self)
        painter.setRenderHint(QPainter.Antialiasing)
        
        if len(self.path_points) > 1:
            pen = QPen(QColor('blue'), 2)
            if self.is_reverse_mode:
                pen = QPen(QColor('purple'), 2)
            painter.setPen(pen)
            for i in range(len(self.path_points) - 1):
                x1, y1 = self.path_points[i]
                x2, y2 = self.path_points[i + 1]
                painter.drawLine(int(x1), int(y1), int(x2), int(y2))
        
        painter.setPen(QPen(QColor('black'), 2))
        painter.setBrush(QColor('red' if not self.is_reverse_mode else 'green'))
        painter.drawRect(
            int(self.robot_x - self.robot_size),
            int(self.robot_y - self.robot_size),
            self.robot_size * 2,
            self.robot_size * 2
        )
        
        painter.setPen(QPen(QColor('black'), 2))
        painter.drawLine(
            int(self.robot_x),
            int(self.robot_y),
            int(self.robot_x + self.robot_size * math.cos(self.robot_angle)),
            int(self.robot_y - self.robot_size * math.sin(self.robot_angle))
        )

        if self.is_mouse_pressed:
            painter.setPen(QPen(QColor('green'), 2))
            painter.drawEllipse(self.mouse_x - 5, self.mouse_y - 5, 10, 10)

class LineFollower(QMainWindow):
    def __init__(self, port):
        super().__init__()
        
        self.m = Master(port)
        self.redLeft = self.m.attach(Red(1))
        self.redRight = self.m.attach(Red(0))
        
        self.main_speed = 50
        self.running = False
        
        self.setWindowTitle("Robot Mouse Control")
        
        main_widget = QWidget()
        self.setCentralWidget(main_widget)
        layout = QVBoxLayout(main_widget)
        
        self.canvas = RobotCanvas()
        layout.addWidget(self.canvas)
        
        button_layout = QHBoxLayout()
        
        self.start_button = QPushButton("Start")
        self.stop_button = QPushButton("Stop")
        self.clear_button = QPushButton("Clear")
        self.reverse_button = QPushButton("Reverse")
        
        self.start_button.clicked.connect(self.start)
        self.stop_button.clicked.connect(self.stop)
        self.clear_button.clicked.connect(self.clear_path)
        self.reverse_button.clicked.connect(self.toggle_reverse)
        
        button_layout.addWidget(self.start_button)
        button_layout.addWidget(self.stop_button)
        button_layout.addWidget(self.clear_button)
        button_layout.addWidget(self.reverse_button)
        
        layout.addLayout(button_layout)
        
        self.timer = QTimer()
        self.timer.timeout.connect(self.update_robot)
        self.timer.setInterval(1)

    def toggle_reverse(self):
        self.canvas.is_reverse_mode = not self.canvas.is_reverse_mode
        self.reverse_button.setText("Forward" if self.canvas.is_reverse_mode else "Reverse")
        self.canvas.mouse_target_distance = float('inf')

    def setup(self):
        try:
            self.m.set_operation_mode(1, OperationMode.PWM)
            self.m.set_operation_mode(0, OperationMode.PWM)
            
            self.m.enable_torque(1, True)
            self.m.enable_torque(0, True)
            return True
        except Exception as e:
            print(f"Hata: {e}")
            return False

    def update_robot_position(self, left_speed, right_speed):
        left_speed = left_speed / 100.0
        right_speed = right_speed / 100.0
        
        angular_velocity = (right_speed - left_speed) * 0.1
        self.canvas.robot_angle += angular_velocity
        
        forward_speed = (right_speed + left_speed) * 0.5
        
        multiplier = -1 if self.canvas.is_reverse_mode else 1
        
        self.canvas.robot_x += multiplier * forward_speed * math.cos(self.canvas.robot_angle) * 5
        self.canvas.robot_y -= multiplier * forward_speed * math.sin(self.canvas.robot_angle) * 5
        
        self.canvas.path_points.append((self.canvas.robot_x, self.canvas.robot_y))
        
        self.canvas.update()

    def update_robot(self):
        if self.running:
            dx = self.canvas.mouse_x - self.canvas.robot_x
            dy = self.canvas.mouse_y - self.canvas.robot_y
            
            current_distance = math.sqrt(dx**2 + dy**2)
            
            target_angle = math.atan2(-dy, dx)
            if self.canvas.is_reverse_mode:
                target_angle = target_angle + math.pi if target_angle < 0 else target_angle - math.pi
            
            angle_diff = target_angle - self.canvas.robot_angle
            
            while angle_diff > math.pi:
                angle_diff -= 2 * math.pi
            while angle_diff < -math.pi:
                angle_diff += 2 * math.pi
            
            base_speed = self.main_speed
            
            if self.canvas.is_mouse_pressed:
                if current_distance < 30:
                    left_speed = base_speed
                    right_speed = base_speed
                else:
                    left_speed = base_speed - (angle_diff * 30)
                    right_speed = base_speed + (angle_diff * 30)
                
                left_speed = min(100, max(-100, left_speed))
                right_speed = min(100, max(-100, right_speed))
                
                if self.canvas.is_reverse_mode:
                    self.m.set_duty_cycle(0, -left_speed)
                    self.m.set_duty_cycle(1, right_speed)
                else:
                    self.m.set_duty_cycle(0, -left_speed)
                    self.m.set_duty_cycle(1, right_speed)
                
                self.update_robot_position(left_speed, right_speed)
            else:
                self.m.set_duty_cycle(0, 0)
                self.m.set_duty_cycle(1, 0)

    def start(self):
        if not self.setup():
            print("Setup Failed!")
            return
        self.running = True
        self.timer.start()

    def stop(self):
        self.running = False
        self.timer.stop()
        self.m.set_duty_cycle(0, 0)
        self.m.set_duty_cycle(1, 0)

    def clear_path(self):
        self.canvas.path_points = []
        self.canvas.robot_x = 600
        self.canvas.robot_y = 400
        self.canvas.robot_angle = 0
        self.canvas.update()

def main():
    app = QApplication(sys.argv)
    port = USB_Port()
    window = LineFollower(port)
    window.show()
    if port:
        print(f"Port found: {port}")
    else:
        print("Port not found!")
    sys.exit(app.exec_())

if __name__ == "__main__":
    main()