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.

About Tools and Materials:

2x SMD Red (Purchase Here)

SMD USB Gateway (Purchase Here)

Arduino Gateway Module (Purchase Here)

2x BDC Motor (Purchase Here)

Reflectance Sensor Module (Purchase Here)

Step 1: Hardware & Software Overview

Project Key Components

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.
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.
Reflectance Sensor Module The Reflectance Sensor Module is a tool that used to detect contrast on a surface. It works by detecting the reflectance of a surface using infrared light and differentiating the contrast by measuring the amount of reflected light to the ratio of emitted light.

Key Features

• PID-Controlled Line Following Uses QTR sensors and a PID algorithm for precise tracking.

• Dynamic Speed Adjustment Adjusts motor speed based on deviation from the line.

• Real-Time Visualization Graphical representation of robot movement using Tkinter Canvas.

• Smooth Path Tracking Implements differential drive control for accurate turns.

• Modular & Expandable Supports easy modifications for different sensor setups.

Step 2: Assemble

Connect the SMD Red to the PC or Arduino board using a USB Gateway Module or Arduino Gateway Module.

Connect the BDC Motors with encoders to the motor ports of the SMD.

Attach the Reflectance Sensor Module to the SMD using RJ-45 cables.

Ensure the SMD is powered and all connections are properly secured.

Project Wiring Diagram

Step 3: Run & Test

1. Connect the Line Follower Robot to your PC using the appropriate USB port. Ensure all connections are secure.

2. Run the Python script to initialize the SMD and set up the motor control.

3. Press the “Start” button in the GUI to begin the line-following operation.

4. The robot will use QTR sensors to detect the line and adjust its movement using a PID algorithm.

5. If needed, adjust the PID parameters (kp, ki, kd) to optimize the tracking accuracy.

6. Press “Stop” to halt the robot, or “Clear” to reset the path tracking.

7. Verify that the motors respond correctly to sensor input and that the robot follows the line smoothly.

Codes

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);
}

PreviousBraitenberg Robot NextTeleoperation Robot

Last updated 2 months ago