Line-Follower Robot
Line Follower Robot (LFR) is a simple robot concept that is autonomously guided to follow a line, that is drawn over white surface as dark lines or over a considerably dark surface as white lines. These lines are detected via the Reflectance Sensor Module on the robot. The code uses a PID algorithm in order to actuate motors, deciding whether turn left or right. The robot smoothly follows the line with these decisions of slight turns.
We used an Arduino board on the robot as the microcontroller for running the SMD Arduino Library codes.
Here is the codes of the project:
from smd.red import *
import time
import tkinter as tk
import math
from serial.tools.list_ports import comports
from platform import system
class LineFollower:
def __init__(self, port):
# Initialize master and motors
self.m = Master(port)
self.redLeft = self.m.attach(Red(1))
self.redRight = self.m.attach(Red(0))
# PID parameters
self.kp = 0.13
self.kd = 0.38
self.ki = 0.0
# Control variables
self.error = 0
self.prev_error = 0
self.integral = 0
self.main_speed = 50
# Create GUI
self.root = tk.Tk()
self.root.title("Robot Movement")
# Create canvas
self.canvas = tk.Canvas(self.root, width=1200, height=800, bg='white')
self.canvas.pack(pady=20)
# Robot position
self.robot_x = 600
self.robot_y = 400
self.robot_angle = 0
self.path_points = []
# Robot size
self.robot_size = 15
# Robot shape
self.robot = self.canvas.create_rectangle(
self.robot_x - self.robot_size,
self.robot_y - self.robot_size,
self.robot_x + self.robot_size,
self.robot_y + self.robot_size,
fill='red'
)
# Direction indicator
self.direction = self.canvas.create_line(
self.robot_x,
self.robot_y,
self.robot_x + self.robot_size * math.cos(self.robot_angle),
self.robot_y - self.robot_size * math.sin(self.robot_angle),
fill='black',
width=2
)
# Control buttons
self.button_frame = tk.Frame(self.root)
self.button_frame.pack(pady=10)
self.start_button = tk.Button(self.button_frame, text="Start", command=self.start)
self.start_button.pack(side=tk.LEFT, padx=5)
self.stop_button = tk.Button(self.button_frame, text="Stop", command=self.stop)
self.stop_button.pack(side=tk.LEFT, padx=5)
self.clear_button = tk.Button(self.button_frame, text="Clear", command=self.clear_path)
self.clear_button.pack(side=tk.LEFT, padx=5)
self.running = False
def setup(self):
"""Initial robot setup"""
try:
# Set to PWM mode
self.m.set_operation_mode(1, OperationMode.PWM)
self.m.set_operation_mode(0, OperationMode.PWM)
# Enable motors
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 read_line(self):
"""Read line position from QTR sensors"""
try:
qtr_values = self.m.get_qtr(0, 1) # Get QTR values
if qtr_values is not None:
left = qtr_values[0]
middle = qtr_values[1]
right = qtr_values[2]
# Calculate weighted sum
weighted_sum = (left * 0) + (middle * 1000) + (right * 2000)
sensor_sum = left + middle + right
if sensor_sum > 0:
return weighted_sum / sensor_sum
return None
except Exception as e:
print(f"Sensor read error: {e}")
return None
def calculate_pid(self, position):
"""PID calculation"""
self.error = position - 1000 # Middle point is 1000
self.integral += self.error
derivative = self.error - self.prev_error
# Limit integral
self.integral = max(min(self.integral, 500), -500)
# Calculate PID output
output = (self.kp * self.error) + \
(self.ki * self.integral) + \
(self.kd * derivative)
self.prev_error = self.error
return output
def update_robot_position(self, left_speed, right_speed):
"""Update robot position"""
# Normalize speeds (-100 to 100 -> -1 to 1)
left_speed = left_speed / 100.0
right_speed = right_speed / 100.0
# Calculate angular velocity
angular_velocity = (right_speed - left_speed) * 0.1
self.robot_angle += angular_velocity
# Calculate forward speed
forward_speed = (right_speed + left_speed) * 0.5
# Calculate new position
self.robot_x += forward_speed * math.cos(self.robot_angle) * 5
self.robot_y -= forward_speed * math.sin(self.robot_angle) * 5
# Update robot shape
self.canvas.coords(
self.robot,
self.robot_x - self.robot_size,
self.robot_y - self.robot_size,
self.robot_x + self.robot_size,
self.robot_y + self.robot_size
)
# Update direction line
self.canvas.coords(
self.direction,
self.robot_x,
self.robot_y,
self.robot_x + self.robot_size * math.cos(self.robot_angle),
self.robot_y - self.robot_size * math.sin(self.robot_angle)
)
# Draw path
self.path_points.append((self.robot_x, self.robot_y))
if len(self.path_points) > 1:
self.canvas.create_line(
self.path_points[-2][0],
self.path_points[-2][1],
self.path_points[-1][0],
self.path_points[-1][1],
fill='blue',
width=2
)
# Update canvas
self.root.update()
def set_motors(self, pid):
"""Set motor speeds"""
base_speed = self.main_speed
error_abs = abs(self.error)
if error_abs > 400:
base_speed = self.main_speed * 0.3
elif error_abs > 200:
base_speed = self.main_speed * 0.5
elif error_abs > 100:
base_speed = self.main_speed * 0.7
if pid < 0:
right_speed = base_speed + abs(pid)
left_speed = base_speed * (1 - abs(pid)/100)
else:
right_speed = base_speed * (1 - abs(pid)/100)
left_speed = base_speed + abs(pid)
right_speed = min(100, max(-100, right_speed))
left_speed = min(100, max(-100, left_speed))
self.m.set_duty_cycle(0, -left_speed)
self.m.set_duty_cycle(1, right_speed)
# Update GUI
self.update_robot_position(left_speed, right_speed)
def clear_path(self):
"""Clear path"""
self.canvas.delete("all")
self.path_points = []
self.robot_x = 600
self.robot_y = 400
self.robot_angle = 0
# Redraw robot and direction line
self.robot = self.canvas.create_rectangle(
self.robot_x - self.robot_size,
self.robot_y - self.robot_size,
self.robot_x + self.robot_size,
self.robot_y + self.robot_size,
fill='red'
)
self.direction = self.canvas.create_line(
self.robot_x,
self.robot_y,
self.robot_x + self.robot_size * math.cos(self.robot_angle),
self.robot_y - self.robot_size * math.sin(self.robot_angle),
fill='black',
width=2
)
def start(self):
"""Start the robot"""
self.running = True
self.run()
def stop(self):
"""Stop the robot"""
self.running = False
self.m.set_duty_cycle(0, 0)
self.m.set_duty_cycle(1, 0)
def run(self):
"""Main operation loop"""
if not self.setup():
print("Setup failed!")
return
print("Starting robot...")
try:
while self.running:
position = self.read_line()
if position is not None:
pid = self.calculate_pid(position)
self.set_motors(pid)
self.root.update()
time.sleep(0.001)
except KeyboardInterrupt:
print("\nStopping program...")
finally:
self.m.set_duty_cycle(0, 0)
self.m.set_duty_cycle(1, 0)
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("Connect")
return port
# Eğer uygun port bulunmazsa mevcut portları yazdır
print("Mevcut portlar:")
for port, desc, hwid in ports:
print(f"Port: {port}, Açıklama: {desc}, HWID: {hwid}")
else:
print("Port not found!")
return None
def main():
port = USB_Port()
if port:
print(f"Port found: {port}")
robot = LineFollower(port)
robot.root.mainloop()
else:
print("Port not found!")
if __name__ == "__main__":
main()#include <Acrome-SMD.h>
#define BAUDRATE 115200
Red redLeft(1, Serial, BAUDRATE);
Red redRight(0, Serial, BAUDRATE);
uint8_t Sensor[1]={iQTR_1};
int16_t X[3];
int Error,DeltaError,IterationP,IterationD,PrevError,PD;
float kp=0.13;
float kd=0.38;
short MainSpeed=100;
void setup() {
redLeft.begin();
redRight.begin();
redLeft.setOperationMode(PWMControl);
redRight.setOperationMode(PWMControl);
redLeft.torqueEnable(1);
redRight.torqueEnable(1);
redRight.setConnectedModules(Sensor, 1);
}
void loop() {
LinePos();
PD2PWM();
}
void LinePos() {
QTRValues qtrValues = redRight.getQTR(1);
uint32_t WeightedSum=0,SensorSum=0;
float PosValue=0;
WeightedSum=(int32_t)(qtrValues.MiddleValue)*1000+(int32_t)(qtrValues.RightValue)*2000;
SensorSum= qtrValues.LeftValue+qtrValues.MiddleValue+qtrValues.RightValue;
PosValue=(float)(WeightedSum)/(float)(SensorSum);
fPD(PosValue);
}
void fPD(int32_t PosValue) {
Error= PosValue-1000;
DeltaError=Error-PrevError;
IterationP=kp*Error;
IterationD=kd*DeltaError;
PD=IterationP+IterationD;
PrevError=Error;
}
void PD2PWM() {
float RightPWM=(PD<0)?MainSpeed+PD:MainSpeed;
float LeftPWM=(PD<0)?MainSpeed:MainSpeed-PD;
RightPWM=(RightPWM>100)?100:RightPWM; RightPWM=(RightPWM<-MainSpeed)?-MainSpeed:RightPWM;
LeftPWM=(LeftPWM>100)?100:LeftPWM; LeftPWM=(LeftPWM<-MainSpeed)?-MainSpeed:LeftPWM;
redRight.setpoint(0,RightPWM);
redLeft.setpoint(0,(-1)*LeftPWM);
}Last updated