ECE456 - Final Project - Report

North Carolina State University
ECE456 - Mechatronics
Shufarrel Bonaparte
David Plonski
Andr Ramos
Final Project Report
1. Introduction
This document reports the final project developed as an assignment for the undergraduate
level course of Mechatronics on North Caroline State University, with the objective of designing
an Unmanned Vehicle (UV) for path tracking.
The robot should use Lego Mindstorms NXT as controller and shall be able to perform
the following tasks:
Task 1 Path Tracking: UV shall move throughout a given path as fast as

possible. The robot shall also stop as soon as it detects a red marking on the path,
signaling the end of the track. For line following and track ending the robot may
use up to three light sensors;

Task 2 Parking: UV shall move throughout a given path and stop as close as
possible from a block, without touching it. For distance detection, a ultrasonic
interface shall be developed for ultrasonic sensor SRF04, which shall be
connected to port 4 of the NXT;

Task 3 Platooning: UV must follow another UV, keeping a constant distance
while following a straight line.

Shufarrel Bonaparte
David Plonski
Andr Ramos
2. Project Setup
In order to have a working robot, several steps had to be fulfilled on advance. From
assembling the robot with Lego parts until having a

Shufarrel Bonaparte
David Plonski
Andr Ramos
2.1.
Programming Platform
Lego Mindstorms NXT supports several programming languages and platforms. The
default one, developed and distributed by Lego, is NXT-G, a visual block based language. Even
though it is possible to develop complex algorithms in NXT-G, using user programmed blocks
and other resources, overall it is not the most flexible and appropriate language for complex
tasks.
From the several other supported languades and GUIs, such as Java on LeJOS, Matlab
and Simulink programming with extension modules for NXT communication, we choose to use
NXC with BricxCC.
NXC is based on standard C and provides several built-in functions, such as motor
regulation, sensor reading, file handling, etc. It also comprises a wider support community than
other languages. BricxCC provides all functionalities for coding, compiling and uploading
programs into NXT. It also provides debug tools for sensor reading, output testing and file
management.

Shufarrel Bonaparte
David Plonski
Andr Ramos
2.2.
Sensors and Motors Positioning
Prior to robot assembling, it was important to determine where each part would be
positioned. It was decided that:
The two motors would be assembled side by side, with wheels on the front end of
the robot. This mounting aimed to provide enough stability for the robot, keeping
its mass center lower;

A third, loose wheel would be positioned on the rear of UV, in order to provide
stability without offering resistance to movement;

The three light sensors would be positioned lined up side by side as close as
possible in order to detect well the track black tape. Light sensor pack was
positioned on the front of the robot, making it faster to detect the red tape and stop
the robot appropriately;

Ultrasonic sensor should not be attached to the interface board, providing
flexibility on its positioned. The preferable location of the ultrasonic sensor was
on the front of the robot, avoiding as much as possible interference form any other
cables and Lego parts. Interface board, was positioned between the motors and the
NXT controller block.
The following pictures show the final UV mounting and details of assembly.
PUT PICTURES HERE

Shufarrel Bonaparte
David Plonski
Andr Ramos
2.3.
Ultrasonic Sensor Interface Development
The provided sensor for distance detection was the ultrasonic detector SRF04. This
sensor is capable of detecting
Figure 1 SRF04 Sensor Connections (Source: http://www.robot-electronics.co.uk)

Shufarrel Bonaparte
David Plonski
Andr Ramos
2.3.1.
Sensor Powering
The first

Shufarrel Bonaparte
David Plonski
Andr Ramos
2.3.2.
Sensor Triggering
s
Figure 2 SRF04 Sensor Triggering and response signal (Source: http://www.robot-electronics.co.uk)

Shufarrel Bonaparte
David Plonski
Andr Ramos
2.3.3.
Sensor signal conditioning

Shufarrel Bonaparte
David Plonski
Andr Ramos
3. Algorithm Design
The algorithm was designed to run two tasks in parallel. The main task is the control task,
responsible for initializing all variables, calling the PID control functions and setting motor
speed according to PID output. The task runs in parallel and is called in a 20 millisecond basis,
responsible for recording on file the reading of light sensors.

Shufarrel Bonaparte
David Plonski
Andr Ramos
3.1.
Main Task
Main task algorithm is responsible for performing the following steps:

1. Initialize all internal variables, making sure PIDs have the desired gains;
2. Check if UV is over a red tape. If thats the case, stop the program;
3. If currently working as Task2, check if its too close to the block. If thats the
case, stop the program;
4. Set up error signal for PID for Path tracking;
5. Set up error signal for PID for Platooning;
6. Call PID functions, getting the control output regarding the updated error and set
points;
7. Set wheel speeds based on the PID control output;
8. Go back to step 2 until a breaking condition is found.
10

Shufarrel Bonaparte
David Plonski
Andr Ramos
3.1.1.
Red Line Detection
For detecting if the red line was reached, Main Task calls the DetectRed() function. This
function is responsible for reading the light sensors and comparing the light sensor readings to
the reference values for red color, which is between 450 and 550 for Raw Sensor Value. In order
to avoid misreading, program is stopped only if the three sensors indicate red color
simultaneously.
11

Shufarrel Bonaparte
David Plonski
Andr Ramos
3.1.2.
Block Distance Detection
For detecting if the block is too close and, therefore, stopping the stopping the UV, Main
Tasks calls detectBlock() function. This function reads the ultrasonic sensor signal plugged into
port 4 and stops the program when Raw Value indication for distance is below 220. This specific
distance value was setup during several trials until reaching a setpoint that allows the robot to
fully stop before hitting the block.
12

Shufarrel Bonaparte
David Plonski
Andr Ramos
3.1.3.
PID Algorithm
PID algorithm is processed by PID_control function. PID output is calculated using the
following equation:
PID output=K Perror + K I errordt +
K Dd ( error )
dt
The actual response shall be given by the programming function:

PID output=K Perror + K I( accumulated error ) + K D(error derivative)
Integral portion is calculated by the error accumulation variable, which is updated on
each PID algorithm iteration with the current error reading. Derivative portion is given by the
difference between the current error reading and the last recorder error reading.
Pathtracking PID setpoint is set as when sensors on the edges have the same reading,
indicating that the robot is in the middle of the line. For that, the robot shall be positioned on the
center of the line on the beginning of the task, when both sensors are read and the difference
between them is set as the Pathtracking PID setpoint.
For the Platooning PID, setpoint was set as 6 inches, which corresponds to a Raw Value
Reading of 330.
13

Shufarrel Bonaparte
David Plonski
Andr Ramos
3.1.4.
Wheel speed controlling
Controlling the speed of the wheels depends on which task is being performed.
For task 1 and 2, the only running PID is the Line Following one. Therefore, the wheel
speed are calculated on the following equations:
Wheel A =( base speed )( Pathtracking PID control output)
WheelC = ( base speed )+( Pathtracking PID control output )
This set of equations works in a way that, when the PID controller requires an action, a
difference on wheel speed is caused. Since each wheel is positioned on one side of the robot,
setting difference speed makes the robot turn to the side of the wheel with the lowest speed.
Ideally, when the controller finishes the correction and theres no steady state error, the controller
output is zero and both wheels run on the set base speed.
For task 3, the equations are slightly modified to accommodate a base speed change
based on the reading distance:
Wheel A =( Platooning PID control output ) (Pathtracking PID control output)
WheelC = ( Platooning PID control output ) +( Pathtracking PID control output )
That way, when the robot is too close to the TAs UV, PID automatically lowers the
speed, increasing the distance between both UVs, and vice-versa.
14

Shufarrel Bonaparte
David Plonski
Andr Ramos
3.2.
Data Logging
For Data Logging, a parallel task was set, responsible for reading the sensors and current
timestamp and writing that information into a log file. After this function writes in the file the
string with the required information, it sleeps to allow the Main Task to be processed, being
awaken later in order to produce one entry on error log on, approximately 20 milliseconds.
15

Shufarrel Bonaparte
David Plonski
Andr Ramos
4. Algorithm Development
Once the basic functionalities of the code were set and coded, the Algorithm was refined
in order to find the ideal PID gains. For such, knowledge from Mechatronics classes were used,
determining which change on gains would be performed to achieve the desired behavior.
The following table was also used, resuming the usual effects that increasing each of the
PID gains might cause:
Table 1 - Impacts of gain increase over closed loop system performance
Gain
KP
Rise Time
Decrease
Overshoot
Increase
Settling Time
Small Change
Steady State Error

Decrease
KI
Decrease
Increase
Increase
Eliminate
KD
Small Change
Decrease
Decrease
Small Change
16

Shufarrel Bonaparte
David Plonski
Andr Ramos
4.1.
Tuning for Path Tracking PID
Considering that tasks 1, 2 and 3 are performed on different tracks, PID for Pathtracking
was set with different gains due to different requirements.
For task 1 and 2, track has turns, and for that reason base speed is set to a lower speed, of
60%, making line following easier, specially on tighter turns. Also, in order to perform well on
turns, it is acceptable to have a not so consistent performance on straight lines. Overall, this PID
have higher gains for Task 1 and 2, providing a more energetic response and, therefore, being
able to follow the line better on tight turns.
Task 3, on the other hand, is performed on a straight line. Also, TAs UV speed might be
set up between 0% and 75%. For that, PID is set with lower gains, avoiding shaking performance
that might have a negative impact on the Platooning Task.
17

Shufarrel Bonaparte
David Plonski
Andr Ramos
4.2.
Tuning for Platooning PID
For tuning the Platooning PID, the same methodology of changing gains following Table
1. Overall, this PID has a set of gains higher than the Path Tracking PID, since reference distance
signals is more sensitive, producing smaller signal changes over raw sensor values. Also, for
Pathtracking PID a small change of the controller output may cause a high turn, while the desired
change of the Platooning PID output is desired to be higher since it sets the base speed of the
robot.
18

Shufarrel Bonaparte
David Plonski
Andr Ramos
4.3.
Adjustments on File handling
One of the refinements performed during Algorithm Development were regarding file
handling. NXT can manage files with up to 1024 bytes, even when more space in memory is
available. This limitation required the development of auxiliary file handling functions.
Basically, the new functions analyses if the number of written bytes match the total number of
bytes in the string. If less bytes were written, the current file is closed and a new one is created.
By the end, NXT memory would have several number sequenced files, that may be joined
together in a PC for a complete error log.
19

Shufarrel Bonaparte
David Plonski
Andr Ramos
5. Results
20

Shufarrel Bonaparte
David Plonski
Andr Ramos
5.1.
Ultrasonic Sensor
21

Shufarrel Bonaparte
David Plonski
Andr Ramos
5.2.
PID Gains
The Tuning process for the PIDs resulted on the following gains:
For the Pathtracking PID on Tasks 1 and 2:

K P =0.02
K I =0.005
K D =2.2
For the Pathtracking PID on Task 3:

K P =0.0001
K I =0.0000 6
K D =1.2
For the Platooning PID:

K P =0.05
K I =0.01
K D =0.05
22

Shufarrel Bonaparte
David Plonski
Andr Ramos
5.3.
Performance Results
23

Shufarrel Bonaparte
David Plonski
Andr Ramos
6. Discussions and Conclusion
24

Shufarrel Bonaparte
David Plonski
Andr Ramos
7. References
25

ECE456 - Final Project - Report

Enviado por

Dados do documento

Direitos autorais

Formatos disponíveis

Compartilhar este documento

Compartilhar ou incorporar documento

Opções de compartilhamento

Você considera este documento útil?

Este conteúdo é inapropriado?

Direitos autorais:

Formatos disponíveis

ECE456 - Final Project - Report

Enviado por

Direitos autorais:

Formatos disponíveis

North Carolina State University

Final Project Report

Task 1 Path Tracking: UV shall move throughout a given path as fast as

use up to three light sensors;

connected to port 4 of the NXT;

North Carolina State University

Final Project Report

North Carolina State University

Final Project Report

North Carolina State University

Final Project Report

Sensors and Motors Positioning

its mass center lower;

stability without offering resistance to movement;

the robot appropriately;

North Carolina State University

Final Project Report

Ultrasonic Sensor Interface Development

Figure 1 SRF04 Sensor Connections (Source: http://www.robot-electronics.co.uk)

North Carolina State University

Final Project Report

North Carolina State University

Final Project Report

North Carolina State University

Final Project Report

Sensor signal conditioning

North Carolina State University

Final Project Report

North Carolina State University

Final Project Report

Main task algorithm is responsible for performing the following steps:

North Carolina State University

Final Project Report

Red Line Detection

North Carolina State University

Final Project Report

Block Distance Detection

North Carolina State University

Final Project Report

The actual response shall be given by the programming function:

North Carolina State University

Final Project Report

Wheel speed controlling

North Carolina State University

Final Project Report

North Carolina State University

Final Project Report

Steady State Error

North Carolina State University

Final Project Report

Tuning for Path Tracking PID

North Carolina State University

Final Project Report

Tuning for Platooning PID

North Carolina State University

Final Project Report

Adjustments on File handling

North Carolina State University

Final Project Report

North Carolina State University

Final Project Report

North Carolina State University

Final Project Report

For the Pathtracking PID on Tasks 1 and 2: