Waypoint tracker robot

The Waypoint Tracker Robot is designed to autonomously navigate to predefined waypoints. Using Python for control logic and serial communication, the robot follows a set of coordinates while adjusting its path in real-time. It utilizes motor controllers to manage movement and ensures smooth navigation with precise speed adjustments. The system can operate in both manual and autonomous modes, allowing for flexible control. Ideal for robotics simulations, path planning, and autonomous navigation applications, this project provides an efficient way to track and reach specific locations with minimal human intervention.

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:

• Autonomous Navigation: The robot follows predefined waypoints and adjusts its path in real-time.

• Mouse-Controlled Movement: Users can manually control the robot using the mouse cursor.

• Waypoint Mode: Allows setting multiple waypoints for the robot to navigate autonomously.

• Grid Display: A visual grid helps in positioning and planning movement paths.

• Path Visualization: Tracks and displays the movement history of the robot.

• Reverse Mode: Enables the robot to move in the opposite direction.

• Speed Control: Adjustable speed settings for fine-tuned movement.

• Real-Time Distance Tracking: Calculates and displays the total distance traveled using motor encoder data.

• Serial Communication: Interfaces with motor controllers via USB for movement execution.

Step 2: Assemble

Getting Started

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 Hardware – Ensure the SMD Red Module and motors are connected, then plug in the USB.

2. Test Controls – Move the mouse to control the robot, toggle Reverse Mode, and adjust speed.

3. Waypoint Navigation – Click to set waypoints, enable Waypoint Mode, and let the robot navigate.

4. Verify Tracking – Check total distance traveled and movement accuracy.

5. Troubleshoot – If issues arise, verify the USB port, adjust speed settings, or recalibrate encoders.

Codes

Python Code

from smd.red import *
import time
import math
import sys
from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QPushButton, QVBoxLayout, QHBoxLayout, QSlider, QGroupBox, QLabel, QMessageBox
from PyQt5.QtGui import QPainter, QColor, QPen, QRadialGradient
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",port)
                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')
        
        self.grid_size = 50
        self.show_grid = True
        
        self.path_thickness = 2
        
        # Total distance traveled (cm)
        self.total_distance = 0
        
        self.cpr = 6533  # Encoder tiks değeri
        self.left_motor_steps = 0
        self.right_motor_steps = 0

        # New waypoint-related attributes
        self.waypoints = []
        self.current_waypoint_index = 0
        self.waypoint_mode = False
        # Platform specific update interval
        if system() == 'Windows':
            self.update_interval = 1
        else:
            self.update_interval = 3

    def update_total_distance(self):
        # Motor shaft encoder ticks per revolution
        encoder_ticks_per_revolution = self.cpr  # 6533 ticks

        # Wheel diameter and circumference
        wheel_diameter = 75  # mm
        wheel_circumference = math.pi * wheel_diameter / 10  # convert to cm

        # Motor shaft rotations
        left_shaft_rotations = self.left_motor_steps / encoder_ticks_per_revolution
        right_shaft_rotations = self.right_motor_steps / encoder_ticks_per_revolution

        # Wheel rotations 
        left_wheel_distance = left_shaft_rotations * wheel_circumference
        right_wheel_distance = right_shaft_rotations * wheel_circumference

        # Average distance traveled
        self.total_distance = (left_wheel_distance + right_wheel_distance) / 2

        # Debug print (you can comment this out later)
        print(f"Left Motor Steps: {self.left_motor_steps}")
        print(f"Right Motor Steps: {self.right_motor_steps}")
        print(f"Left Shaft Rotations: {left_shaft_rotations:.4f}")
        print(f"Right Shaft Rotations: {right_shaft_rotations:.4f}")
        print(f"Left Wheel Distance: {left_wheel_distance:.4f} cm")
        print(f"Right Wheel Distance: {right_wheel_distance:.4f} cm")
        print(f"Total Distance: {self.total_distance:.4f} cm")

        self.update()

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

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

    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)
        
        # Draw grid
        if self.show_grid:
            painter.setPen(QPen(QColor(200, 200, 200), 1, Qt.DashLine))
            for x in range(0, self.width(), self.grid_size):
                painter.drawLine(x, 0, x, self.height())
            for y in range(0, self.height(), self.grid_size):
                painter.drawLine(0, y, self.width(), y)
        
        # Draw path
        if len(self.path_points) > 1:
            pen = QPen(QColor('blue'), self.path_thickness)
            if self.is_reverse_mode:
                pen = QPen(QColor('purple'), self.path_thickness)
            pen.setCapStyle(Qt.RoundCap)
            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))
        gradient = QRadialGradient(self.robot_x, self.robot_y, self.robot_size)
        gradient.setColorAt(0, QColor('red' if not self.is_reverse_mode else 'green'))
        gradient.setColorAt(1, QColor('darkRed' if not self.is_reverse_mode else 'darkGreen'))
        painter.setBrush(gradient)
        painter.drawRect(
            int(self.robot_x - self.robot_size),
            int(self.robot_y - self.robot_size),
            self.robot_size * 2,
            self.robot_size * 2
        )
        
        # Draw robot orientation line
        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))
        )

        # Draw waypoints
        if self.waypoints:
            for i, (x, y) in enumerate(self.waypoints):
                painter.setPen(QPen(QColor('green'), 2))
                painter.drawEllipse(int(x) - 5, int(y) - 5, 10, 10)
                
                # Draw waypoint number
                painter.drawText(int(x) + 10, int(y) + 10, str(i + 1))

        # Total distance
        painter.setPen(QPen(QColor('black'), 1))
        painter.drawText(self.width() - 150, self.height() - 10, f"Total Distance: {self.total_distance:.2f} cm")

class LineFollower(QMainWindow):
    def __init__(self, port):
        super().__init__()
        
        self.port = port
        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
        
        # Set window properties
        self.setWindowTitle("Robot Waypoint Navigation")
        self.setStyleSheet("""
            QMainWindow {
                background-color: #f0f0f0;
            }
            QPushButton {
                background-color: #4a90e2;
                color: white;
                border: none;
                padding: 8px 16px;
                border-radius: 4px;
                min-width: 100px;
            }
            QPushButton:hover {
                background-color: #357abd;
            }
            QPushButton:pressed {
                background-color: #2d6da3;
            }
            QPushButton:checked {
                background-color: #45a049;
            }
            QSlider::groove:horizontal {
                border: 1px solid #999999;
                height: 8px;
                background: #ffffff;
                margin: 2px 0;
                border-radius: 4px;
            }
            QSlider::handle:horizontal {
                background: #4a90e2;
                border: 1px solid #5c5c5c;
                width: 18px;
                margin: -2px 0;
                border-radius: 9px;
            }
            QGroupBox {
                border: 2px solid #cccccc;
                border-radius: 6px;
                margin-top: 12px;
                padding-top: 10px;
            }
            QLabel {
                color: #333333;
                font-size: 14px;
            }
        """)
        
        # Create main widget and layout
        main_widget = QWidget()
        self.setCentralWidget(main_widget)
        main_layout = QVBoxLayout(main_widget)
        main_layout.setSpacing(10)
        main_layout.setContentsMargins(10, 10, 10, 10)
        
        # Create top control panel
        top_panel = QGroupBox("Control Panel")
        top_layout = QVBoxLayout(top_panel)
        
        # Create button layout
        button_layout = QHBoxLayout()
        button_layout.setSpacing(10)
        
        # Create and style buttons
        self.start_button = QPushButton("▶ Start")
        self.stop_button = QPushButton("⬛ Stop")
        self.clear_button = QPushButton("🗑 Clear")
        self.grid_button = QPushButton("# Grid")
        self.waypoint_button = QPushButton("📍 Add Waypoint")
        self.waypoint_nav_button = QPushButton("🧭 Waypoint Nav")
        self.find_port_button = QPushButton("🔍 Connect Port")
        self.grid_button.setCheckable(True)
        self.grid_button.setChecked(True)
        self.waypoint_nav_button.setCheckable(True)
        
        # Connect button signals
        self.start_button.clicked.connect(self.start)
        self.stop_button.clicked.connect(self.stop)
        self.clear_button.clicked.connect(self.clear_path)
        self.grid_button.clicked.connect(self.toggle_grid)
        self.waypoint_button.clicked.connect(self.add_waypoint)
        self.waypoint_nav_button.clicked.connect(self.toggle_waypoint_nav)
        self.find_port_button.clicked.connect(self.find_new_port)
        
        # Add buttons to layout
        for button in [self.start_button, self.stop_button, self.clear_button, 
                       self.grid_button, self.waypoint_button, self.waypoint_nav_button, self.find_port_button]:
            button_layout.addWidget(button)
        
        # Create speed control layout
        speed_layout = QHBoxLayout()
        speed_label = QLabel("Speed Control:")
        speed_label.setMinimumWidth(100)
        
        self.speed_slider = QSlider(Qt.Horizontal)
        self.speed_slider.setMinimum(0)
        self.speed_slider.setMaximum(100)
        self.speed_slider.setValue(50)
        self.speed_slider.valueChanged.connect(self.update_speed)
        
        self.speed_value_label = QLabel("50%")
        self.speed_value_label.setMinimumWidth(50)
        self.speed_slider.valueChanged.connect(
            lambda v: self.speed_value_label.setText(f"{v}%"))
        
        speed_layout.addWidget(speed_label)
        speed_layout.addWidget(self.speed_slider)
        speed_layout.addWidget(self.speed_value_label)
        
        # Add layouts to top panel
        top_layout.addLayout(button_layout)
        top_layout.addLayout(speed_layout)
        
        # Add panels to main layout
        main_layout.addWidget(top_panel)
        
        # Add canvas
        self.canvas = RobotCanvas()
        main_layout.addWidget(self.canvas)
        
        # Setup timer with platform-specific interval
        self.timer = QTimer()
        self.timer.timeout.connect(self.update_robot)
        if system() == 'Windows':
            self.timer.setInterval(1) 
        else:
            self.timer.setInterval(3) 
            
        if system() == 'Windows':
            self.update_interval = 1  
        else:
            self.update_interval = 3  

    def add_waypoint(self):
        # Add a waypoint at the last mouse click position
        if not self.canvas.waypoint_mode:
            x, y = self.canvas.mouse_x, self.canvas.mouse_y
            self.canvas.waypoints.append((x, y))
            self.canvas.update()

    def toggle_waypoint_nav(self):
        # Toggle waypoint navigation mode
        self.canvas.waypoint_mode = self.waypoint_nav_button.isChecked()
        if self.canvas.waypoint_mode:
            self.canvas.current_waypoint_index = 0
            if self.canvas.waypoints:
                # Set first waypoint as target
                self.canvas.mouse_x = self.canvas.waypoints[0][0]
                self.canvas.mouse_y = self.canvas.waypoints[0][1]
                
    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"Error: {e}")
            return False

    def update_robot_position(self, left_speed, right_speed):
        # Speed normalization
        left_speed = left_speed / 100.0
        right_speed = right_speed / 100.0
        
        # Platform specific speed adjustment
        if system() == 'Windows':
            speed_multiplier = 5  # Windows için normal hız
        else:
            speed_multiplier = 2  # Raspberry Pi için düşük hız
        
        # Angular velocity
        angular_velocity = (right_speed - left_speed) * 0.1
        self.canvas.robot_angle += angular_velocity
        
        # Forward velocity
        forward_speed = (right_speed + left_speed) * 0.5
        multiplier = -1 if self.canvas.is_reverse_mode else 1
        
        # Update position with platform-specific speed
        self.canvas.robot_x += multiplier * forward_speed * math.cos(self.canvas.robot_angle) * speed_multiplier
        self.canvas.robot_y -= multiplier * forward_speed * math.sin(self.canvas.robot_angle) * speed_multiplier
        
        self.canvas.path_points.append((self.canvas.robot_x, self.canvas.robot_y))
        
        # Motor steps update
        self.canvas.left_motor_steps += left_speed * self.canvas.cpr * 0.02
        self.canvas.right_motor_steps += right_speed * self.canvas.cpr * 0.02
        
        # Update total distance
        self.canvas.update_total_distance()
        
        self.canvas.update()

    def update_robot(self):
        if self.running:
            if self.canvas.waypoint_mode and self.canvas.waypoints:
                if not self.canvas.waypoints:
                    return
                
                current_wp_index = self.canvas.current_waypoint_index
                current_wp = self.canvas.waypoints[current_wp_index]
                
                dx = current_wp[0] - self.canvas.robot_x
                dy = current_wp[1] - self.canvas.robot_y
                current_distance = math.sqrt(dx**2 + dy**2)
                
                # Waypoint completion criteria
                waypoint_radius = 30  # Increase detection radius
                
                # Check if reached the last waypoint
                if current_wp_index == len(self.canvas.waypoints) - 1 and current_distance < waypoint_radius:
                    self.stop()
                    self.waypoint_nav_button.setChecked(False)
                    self.canvas.waypoint_mode = False
                    alert = QMessageBox()
                    alert.setIcon(QMessageBox.Information)
                    alert.setText("Waypoint Navigation Completed")
                    alert.setInformativeText("The robot has reached all waypoints and stopped.")
                    alert.setWindowTitle("Navigation Complete")
                    alert.exec_()
                    return
                
                # Move to next waypoint if close enough
                if current_distance < waypoint_radius:
                    self.canvas.current_waypoint_index = (current_wp_index + 1) % len(self.canvas.waypoints)
                    current_wp = self.canvas.waypoints[self.canvas.current_waypoint_index]
                
                self.canvas.mouse_x = current_wp[0]
                self.canvas.mouse_y = current_wp[1]
            
            # Existing navigation logic with some tweaks
            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.waypoint_mode or self.canvas.is_mouse_pressed:
                if current_distance < 10:
                    left_speed = 0
                    right_speed = 0
                else:
                    # Gelişmiş dönüş ve hız kontrolü
                    # PID benzeri yumuşak geçiş
                    Kp = 20  # Orantılı kazanç
                    Ki = 0.1  # İntegral kazancı
                    Kd = 5   # Türev kazancı
                    
                    # Açısal hız için yumuşatılmış kontrol
                    turn_factor = Kp * angle_diff + Ki * angle_diff + Kd * (angle_diff - self.last_angle_diff if hasattr(self, 'last_angle_diff') else 0)
                    
                    # Hız hesaplaması
                    left_speed = base_speed - turn_factor
                    right_speed = base_speed + turn_factor
                    
                    # Dinamik hız ölçeklendirmesi
                    max_look_ahead = 250  # Arttırılmış bakış mesafesi
                    speed_scale = min(1.0, current_distance / max_look_ahead)
                    
                    # Minimum speed enforcement
                    min_speed = 40
                    speed_scale = max(speed_scale, min_speed / base_speed)
                    
                    left_speed *= speed_scale
                    right_speed *= speed_scale
                    
                    # Son açı farkını kaydet
                    self.last_angle_diff = angle_diff
                
                # Hız sınırlaması
                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()
        time.sleep(self.update_interval / 1000)

    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.total_distance = 0
        self.canvas.left_motor_steps = 0
        self.canvas.right_motor_steps = 0
        self.canvas.waypoints = []
        self.canvas.current_waypoint_index = 0
        self.canvas.waypoint_mode = False
        self.waypoint_nav_button.setChecked(False)
        self.canvas.update()       

    def update_speed(self):
        self.main_speed = self.speed_slider.value()
        
    def toggle_grid(self):
        self.canvas.show_grid = self.grid_button.isChecked()
        self.canvas.update()

    def find_new_port(self):
        try:
            if hasattr(self, 'm') and self.m is not None:
                try:
                    if hasattr(self, 'redLeft'):
                        del self.redLeft
                    if hasattr(self, 'redRight'):
                        del self.redRight
                    if hasattr(self, 'm'):
                        del self.m
                except Exception as check_err:
                    print(f"Error: {check_err}")

            new_port = USB_Port()
            if new_port:
                try:
                    self.port = new_port
                    self.m = Master(new_port)
                    self.redLeft = self.m.attach(Red(1))
                    self.redRight = self.m.attach(Red(0))
                    
                    QMessageBox.information(self, "Port Found", f"New port found: {new_port}")
                except Exception as e:
                    QMessageBox.warning(self, "Connection Error", f"Could not establish port connection: {str(e)}")
            else:
                QMessageBox.warning(self, "Port Not Found", "No port found. Please ensure the device is connected.")
        
        except Exception as general_err:
            QMessageBox.warning(self, "General Error", f"An error occurred: {str(general_err)}")

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!")
        QMessageBox.warning(window, "Port Bulunamadı", "Hiçbir port bulunamadı. Lütfen cihazın bağlı olduğundan emin olun.")
    
    sys.exit(app.exec_())

if __name__ == "__main__":
    main()

Arduino Code

// Some code