Waypoint tracker robot
This Python application enables robot navigation using a graphical user interface (GUI). The GUI allows for interaction, including path planning with waypoints, real-time navigation, and control over the robot's movement.
Features:
USB Port Detection: Automatically detects connected USB devices based on the operating system.
GUI:
Developed with PyQt5.
Includes start/stop controls, speed adjustment, grid display, and waypoint management.
Dynamic visualization of the robot's path and current position.
Robot Motion:
Supports real-time forward and reverse movement.
Implements waypoint navigation with automatic switching to the next target.
Uses path-following algorithms with visual indicators.
Distance Measurement:
Calculates total distance traveled using encoder values and wheel dimensions.
Distance is updated dynamically during robot movement.
Platform Specific Adjustments:
Dynamically adjusts control update intervals for Windows and non-Windows platforms.
Custom speed scaling to accommodate Raspberry Pi's reduced processing power.
Waypoint Management:
Allows users to add, clear, and navigate through waypoints.
Provides real-time feedback when all waypoints are reached.
Key Classes and Methods:
RobotCanvas (QWidget):
Handles graphical rendering of the robot, path, and waypoints.
Tracks robot state (position, angle) and mouse interactions for navigation.
LineFollower (QMainWindow):
Manages the main application logic and GUI controls.
Communicates with hardware through an abstracted
Masterinterface.Includes speed adjustment, waypoint addition, and robot control logic.
USB Port Management:
Detects and lists available USB ports compatible with the robot hardware.
Workflow:
Initialization:
Detect USB ports and establish communication with the robot hardware.
GUI Interaction:
Users interact with controls to define movement speed, toggle grid display, or add waypoints.
Path Navigation:
Real-time adjustments to robot speed and direction based on the current target (mouse or waypoint).
Visual updates to the robot's path and position are reflected on the canvas.
Libraries:
PyQt5: For GUI creation and event handling.
Math and Time: For calculations and timing adjustments.
Serial Tools: For USB port detection and communication.
Platform Module: To identify the operating system.
SMD Red Module: To control the movement of the robot.
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()Last updated