Waypoint tracker robot
Last updated
Last updated
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.
2x ()
()
()
2x ()
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 and meanwhile, actuate the motor for the continuous reading of the script.
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.
• 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.
Hardware Setup
Make sure that the SMD is powered and all connections are correct.
Project Wiring diagram
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.
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()// Some codeConnect the SMD to the PC or Arduino board using or .
Connect the 100 RPM with Encoder to the motor ports of the SMD using an RJ-45 cable.
