You are on page 1of 20

Fall

2013
Lineduino
ME190 MECHATRONICS TERM PROJECT
SAN JOSE STATE UNIVERSITY, DEPARTMENT OF MECHANICAL ENGINEERING
DAMION ENGELBART
ERIC TSE

12.9.2013
1 | P a g e

Contents
Summary ...................................................................................................................................................... 2
Introduction .................................................................................................................................................. 3
Modeling ....................................................................................................................................................... 4
Simulation ..................................................................................................................................................... 6
Design ............................................................................................................................................................ 8
Results ......................................................................................................................................................... 11
References .................................................................................................................................................. 13
Appendix ..................................................................................................................................................... 13
A. Drawings ......................................................................................................................................... 13
B. Source Code .................................................................................................................................... 16
C. Bill of Materials ............................................................................................................................... 19
D. Data sheet links ............................................................................................................................... 19
E. YouTube Link ................................................................................................................................... 19


2 | P a g e


Summary
The Lineduino is a three wheeled autonomous line following robot that utilizes closed loop
feedback PID control to track and follow a black line. Using sensors to constantly read the position of
the robot relative to the line, Lineduinos control system is able to make corrections to individual motor
speed to keep it on track. Conventional line following robots use an on-off or bang-bang hysteresis
controller that execute a predefined motor correction response when it begins to deviate from the line.
The Lineduino differs from conventional line following robots in that it utilizes a PID controller to track
position. Using PID allows the robot to correct its trajectory, store information about previous offset
and predict how the line will change. This allows it to reach higher speeds while oscillating less about
the line.

Figure 1. The Lineduino autonomous line following robot
The control system of the Lineduino was modeled as a closed-loop feedback system with PID
control. Its operation begins with the sensor array reading the individual IR sensor values representing
the robots position relative to the line. This offset information is sent to the Arduino microcontroller
where error is calculated and corrective commands are sent to the two rear servo motors with the use
of a PID algorithm. The input used in this control system is the IR arrays determination of line offset
while output is a differential corrective voltage applied to the motors to correct the robots trajectory.
The PID variables K
p,
K
i
and K
d
were experimentally derived by iterative tuning. By recognizing that
system overshoot is characterized by wobble while steady state error is represented by the robot falling
off the line completely, parameters can be tuned following standard techniques.
The implementation of PID control in the Lineduinos control system was evident in that it
tracked the line better than a bang-bang controlled system. Wobble was reduced along straight lines
due to the proportion and integral terms, while curved paths were negotiated much quicker due to the
derivative term. Before the PID variables were tuned, the robot was unable to accurately track the line
and its trajectory often fell off completely. However, through experimental parameter tuning the robot
was able to navigate the track much quicker and with increased precision.
Through this project, a greater understanding of control systems and mechatronic concepts was
obtained. The opportunity to implement knowledge of closed-loop PID control theory in a real life
3 | P a g e

system was beneficial to the understanding of the subject. This project illustrated how the individual
proportional, integral and derivative components of the controller affected the systems overall
performance.
Introduction
The use of control systems in the automation industry has risen in proportion with the
complexity of mechatronic systems and the wider accessibility of powerful, miniaturized technology.
The incorporation of microcontrollers allows machines to become smarter by making decisions based on
external input. The Lineduino robotic line follower was designed and fabricated for fulfillment of the
SJSU ME190 term project following a set of guidelines. The requirements stipulates that the device use
a microcontroller as the primary means for system operation, as well as implementation of a closed-
loop control system.
Following these prerequisites, the Lineduino is designed to be a line following robot using an
Arduino Uno R3 microcontroller and a closed-loop PID control system to navigate along a road or a
solid black line. These conditions were acknowledged throughout all stages of design. A microcontroller
is a miniature computer that can be programmed to provide output actuation as well as input sensing.
Microcontrollers are classified as miniature computers due to the fact that their processing, memory, as
well as input/output components are contained in one small package. The need for microcontrollers in
autonomous systems is critical due to their ability to be programmed to make decisions based on
dynamic input. The Arduino R3 Uno microcontroller shown in figure 2, was selected due its ease of use
and expandability.

Figure 2. Arduino Uno R3 Microcontroller
http://arduino.cc/en/uploads/Main/ArduinoUno_R3_Front_450px.jpg
Unlike a traditional bang-bang controlled line following robot that only adjusts direction when
the line is hit, the Lineduino utilizes a PID style control algorithm to improve performance and
responsiveness illustrated in figure 3. A bang-bang controlled system refers to the way that a system
deals with external disturbances. If a sensor used to detect an instantaneous deviation from the
systems normal operation representing the first bang, a predefined corrective action will execute
representing the second bang. A PID or Proportional Integral Derivative control system was chosen
due to its ability to not only make trajectory corrections, but to store information about previous offset
and predict potential change in the line.
4 | P a g e


Figure 3. On-Off vs PID controller

Modeling
The Lineduino utilizes a closed loop control system with a PID controller to increase system
responsiveness and reduce error. A closed loop system is defined as one that contains a feedback loop
that is compared to the input or set point to influence the systems operation. A typical closed loop
system is shown below in figure 4.

Figure 4. Closed loop system with feedback
Proportional Integral Derivative controllers or PIDs work to reduce error by manipulating error
signals with three different operators. The standard model for a closed loop control system with PID
control is shown below in figure 5. The output of a PID controller is a summation of the Proportional,
Integral and Derivative terms shown below.


5 | P a g e



Figure 5. Closed loop PID controller block diagram
The proportional term multiplies the error of a system by a proportional gain K
P
and typically
contributes the greatest change to an output. A high value of K
P
means that a quicker correction of
error seen in the output will occur. However, due to this quick correction, the possibility of the
response overshooting its target is possible. A value of K
P
that is too low will cause the output response
to become too low and create steady state error.
The integral term multiplies the integral gain, K
i
, by the integral of error over time. The result is
a term that takes into account how long the error has been occurring. By taking the integral of error,
the system can reach the desired set point more quickly due to the error backing up and
accumulating, generating a heavier influence on the output. However, due to the nature of the integral
term, if error has been accumulating over time, the systems response may overshoot the set point
during that time interval.
The final derivative term multiplies the derivative gain K
d
, by the slope or difference between
the current error and the last. This contributes to the PIDs output by measuring the rate at which the
error is changing and predicting what the next error will be. The derivative term can benefit system
response performance by speeding up the response if error begins to trend in one direction. This
however is prone to errors that jump quickly, causing a derivative kick in control signal of the opposite
direction. This can be detrimental to systems where noisy signals are controlled.
The Lineduinos control system takes inputs in the form of line position offset and outputs a
correcting control signal to actuate the rear motors. Using an array of IR reflectance sensors placed at
the front of the vehicle, line position can be tracked by the readings taken from the IR sensors. The IR
line sensing array outputs a single number to define its location as shown below in figure 6. Their values
range between -1 representing maximum left offset and 1 representing maximum right offset. An
6 | P a g e

output of zero represents being centered over the line, while intermediate values -0.5 and 0.5 represent
smaller offsets left and right, respectively.

Figure 6. Three reflectance sensors form an array that output a number between -1 and 1 to define position
This signal can be fed into a difference junction where it is compared with a set point value
equal to zero and an error is calculated. The error signal is then fed into a PID controller block where
the proportional, integral and derivative components are calculated and summed. The resulting control
signal is passed to the microcontroller where it is used to actuate individual motors with pulse width
modulated or PWM signal in order to get the robot back on the desired path. A block diagram modeling
this is shown below in figure 7.

Figure 7. Control system block diagram
Simulation
In order to simulate the response of the control system, certain assumptions and simplifications
must be used. In order to simplify the simulation of the Lineduinos control system, it was assumed to
be initially moving directly over a straight portion of the line moving at a default speed. Input to the
system is in the form of offset angle from the line and output is the differential correction PWM signal
applied to the motors.
7 | P a g e

The IR sensor outputs values ranging from -1 when only the left sensor is hit, to +1 where only
the right sensor is hit representing a -/+ 6.328 degree offset, respectively shown below in figure 8. This
number is subtracted from the set point chosen to be zero representing no offset. The resulting error is
passed through the PID controller where a manipulating offset value is generated.

Figure 8. Deviation from the line is characterized as an angle of offset that the motor velocities must compensate
In order to obtain the relationship between a specific PWM signal applied to the motor and the
distance the wheel will travel in a time interval, the motors K
v
value or RPM per volt was first
determined. This value was experimentally obtained to be 6.88 RPM/volt. The 6 volt motor is attached
to a 42mm wheel that operates on PWM signal from 0 to 255 via the Arduino. This means that an
applied voltage of 6V is equivalent to a PWM signal of 255 or 0.0235 volts per 1ms pulse width. Using
this value, the relationship between PWM signals to linear velocity was obtained as follows.

6.88

6.88
1

60

42
60
2.19 /


255
6 .0235
. 161
. 352 /
In order to compensate for a 6.328 vehicle tilt, one wheel must speed up by a distance of 6.029
mm ahead, while the other must slow down by the same amount. This distance equates to
approximately 0.0456 of one full rotation for a 42 mm wheel. Since the other wheel is still moving, this
correction must be performed in the shortest time possible in order to reduce wobble and overshoot.
At any speed, if a side IR sensor was triggered, for a correction to happen in one second, one motors
PWM value must be powered up by 17 ms, while the other powered down by 17 ms. Times for one and
one-half theta correction are plotted against PWM in figure 9.
8 | P a g e


Figure 9. Correction time for one and one-half theta offset for different PWM signals
Tuning the PID parameters was performed experimentally by first setting all variables equal to
zero. Since K
p
has the greatest effect on the instantaneous direction of the vehicle, it was tuned first. As
stated before, a K
P
that is too low will cause it to fail in controlling error by falling off the line.
Conversely, a K
P
that is too high will cause oscillation due to the amplification of the error signals. Once
the vehicle was able to follow a line, the K
d
parameter was set to 1 and increased until it began to
wobble. This wobble is due to the fact that as the gain is increased and differential error flips back and
forth as each IR sensor was triggered.
Design
The mechanical design began by identifying the major subsystems of the vehicle which can be
seen below in figure 10. These included a power system, a microcontroller, the drive system (a motor
driver board and two motors), and a sensor array. Regardless of the final configuration of the vehicle,
the sensor array, drive system, and microcontroller will utilize the same components. The components
were selected and purchased before the design was finalized so features could be built into the
manufactured parts which accommodated the purchased components. A list of all the materials used in
this project along with some information on each one can be found in the appendix.
0
200
400
600
800
1000
1200
10 60 110 160 210
T
i
m
e

t
o

c
o
r
r
e
c
t

(
m
s
)

PWM Signal
PWM vs Correction Time
Theta
0.5 Theta
9 | P a g e


Figure 10. Major subsystems of LineDuino robot. This block diagram shows each major subsystem and how they
integrate into the whole system. Red lines represent power while green lines represent signal.
The component selection process began by choosing a sensor which could detect either a black
or white line. A QRD1114 reflectance sensor was chosen because it is readily available, and there are
many tutorials available online regarding its use. An inverting Schmitt trigger was utilized on the output
of the phototransistor in order to help eliminate low amplitude noise and generate a square wave. Once
the sensor was identified and sourced, a sensor array was designed around the dimensions of the sensor
and the width of standard black electrical tape, because that is what was used to create the path for the
robot to follow. The width of standard electrical tape is 19 mm, so the center to center distance of the
sensors was chosen to be 12 mm. While directly over the line, the center sensor can detect the line
without the side sensors hitting. While falling off the line, the center sensor and one side sensor can also
detect the line without the third sensor hitting. This enables higher resolution detection of the line by
allowing three sensors to detect five different states. Once the sensor spacing was decided upon, a
printed circuit board (PCB) was designed to hold all three sensors at the desired spacing and to house
and connect the rest of the components used to properly drive the sensors. A single stage of this circuit
can be seen below in figure 11.

Figure 11. Single stage of line sensor circuit. This circuit features a QRD1114 reflectance sensor which is
filtered by an inverting Schmitt Trigger. The output of the reflector is located at the junction labeled 12.

Figure 12. QRD1114 IR Reflector Array. This circuit includes all components necessary to run the sensors.
5 volts goes into the bottom left pad, then ground. The final three pads are the outputs for each sensor.
10 | P a g e

Two brushed DC motors that operate at 6 volts and have attached 298:1 gearheads were bought
from the internet. They were advertised as having 50:1 gearheads, but upon measuring the RPM to
voltage relationship, it was discovered that these motors match the characteristics of the motors with
298:1 gearheads. These motors also have 42mm diameter rubber wheels attached to the output shafts
which is convenient for integration with a vehicle. A mounting bracket was designed around the motor
using SolidWorks and 3D printed using a Stratasys Dimension 1200es printer; a detailed drawing can be
found in the appendix. This bracket was used to mount the motor to a central chassis that houses all the
electronics and various components. An omnidirectional roller ball from SparkFun placed at the front of
the robot provides stability and allows the distance from the sensor to the surface to be adjusted. The
chassis was then designed, again using SolidWorks, around all the components including the
microcontroller, the motor adapters, the battery holder, the omnidirectional roller, and the sensor PCB.
This part was designed to be laser cut out of 0.125 inch thick acrylic, and was cut on a Trotec Speedy 300
laser cutter. A detailed drawing of the chassis can be found in the in appendix. The 3D model of the line
following robot can be seen next to the final product below in figure 13.

Figure 13. 3D model of line following robot (left) and a picture of final line following robot (right).
Because an Arduino Uno microcontroller was going to be used to implement the control system,
an Arduino Motor Shield was chosen as the motor driver. It is a simple piggy-back style shield which
plugs into the Arduino and allows access to all of the unused pins. This particular model is capable of
delivering 2 Amps per channel at 5 to 12 volts; however the motors used draw a max of only 300 mA.
Finally, a power system was required for the LineDuino robot. An adjustable voltage regulator (LM317)
was chosen because it is capable of regulating the output to 1.5 Amps at between 1.2 and 37 volts. A 9
volt battery was wired to the input of the LM317, and the output of the LM317 was wired to the
Arduino Motor Driver Shield, which also supplied to the Arduino Uno with power. The complete circuit
diagram of the LineDuino robot is shown below in figure 14. The diagram is slightly simplified as an L298
driver chip (which is the driver chip used in the Arduino Motor Driver Shield) is used to represent the
whole motor driver shield. The L298 requires some passive components in order to properly function,
which the Arduino Motor Driver Shield has built in.
11 | P a g e


Figure 14. Circuit Diagram of LineDuino robot. Arduino Motor Driver Shield not included in its entirety for
simplification.
Results
Before beginning this project some design specifications were laid out in a project proposal.
These design specifications include:
The vehicle will be able to quickly and accurately follow a continuous path generated by
anyone, including self-crossing paths and those with sharp directional changes.
Two brushed DC motors with gear heads will be used to drive the two rear wheels, with a
third passive omnidirectional wheel to provide stability. A suite of QRD1114 Reflective Object
Sensors will be used to sense the line and allow for detection of deviations in the path.
A calibration mode will be implemented which will allow the vehicle to learn the
characteristics of the line it is supposed to follow. This will be accomplished by measuring the
reflectivity of the line and surrounding surface before attempting to follow, and using these
values to set thresholds within the code.
The device will be very compact.
The total cost of the project should be very low (<$75).
Due to time constraints only three of these five goals were met. The vehicle is able to quickly and
accurately follow a continuous path, even with very sharp directional changes, however, the operation
of the robot could be greatly sped up by using motors with lower gear ratios. There are many motors
available in the exact same form factor with lower gear ratio gearheads that would be drop-in solutions.
It is also able to follow a path which is self-intersecting about 75% of the time, however, the other 25%
12 | P a g e

of the time it follows the wrong line. The two brushed motors with gearheads, omnidirectional caster,
and the Reflective Object Sensors were used successfully in the final design. The calibration mode was
not implemented, and the robot can only follow a non-reflective line on a reflective surface currently,
but that allows it to follow a black line on a white surface very effectively. The final design of the device
was very compact, the overall dimensions are 120 mm long, 120 mm wide, and about 75 mm tall, which
allows it to follow a relatively small line. The overall cost of the device ended up exceeding the $75
mark, and actually ended up at $100 instead. This was largely due to the fact that the components were
bought in small quantities, and generally chosen for simplicity. The motor driver shield alone cost
$30.00, and could have easily been replaced by an H-bridge or a discrete motor driver IC which would
have been much less expensive.
Most of the features which were not implemented in the Lineduino robot can easily be added in
to the existing code by spending some time to develop it. The ability to follow self-intersecting lines can
be improved by adding logic which specifically handles the case of a self-intersecting line. Currently, if
the intersection is not at a 90 degree angle to the line being followed, the robot reacts to the
intersection the same way it does a turn. Implementing a calibration mode would be very simple,
especially with the addition of an input button that would enter the Lineduino into a calibration mode.
The cost can be reduced in many different ways. By sourcing components from low-cost markets such as
eBay, all the electronics could have been procured for half of the cost.
The PID control system which was implemented in the Lineduino robot worked extremely well.
The robot is able to follow a line without using a bang-bang style method; the output of the Lineduino is
very smooth when compared with non-PID line following robots. Without using PID the robot could
follow a curve, but the time per lap was generally about 5 seconds longer than when using PID. The PID
constants were tuned by setting the constants to different values and observing how long the robot
takes to complete a lap on a given track. Even with extensive tuning, the lap time was only improved by
about 1.5 seconds. With more time, a more accurate model could be developed which would allow for
the derivation of better PID constants for smoother and faster operation. From the simplified model of
the PWM controlled DC motor we can conclude that when one of the far sensors hits the line, if a PWM
value of 245 is used, it will take approximately 70 ms to return to the line. This would imply that it would
take a little less than one tenth of a second to correct back to the line. There is currently no way to verify
this operation quantitatively, however, by observing the operation of the robot, it seems that 70 ms is
probably not enough time for it to fully return to the line. This could be due to the fact that the model
assumes no mass and no friction of any of the components, which would lead to a shorter return time.
Further work on the model is needed, along with an apparatus to verify the operation of the robot
quantitatively.
Due to the method of problem solving utilized for this project, very few issues arose. The basic
mechanical design was based around parts that were already purchased, which helped to ensure that
there would be no interference or fitment issues. All parts that were manufactured were first drawn in
3D CAD and checked for errors or problems. The sensing circuit was built on a breadboard and
extensively tested before drawing it in CAD to be fabricated. Because everything was designed using
CAD from the ground up, there were no surprises while assembling this robot, which was very helpful
13 | P a g e

when it came time to build and program everything. The only real uncertainty was how the code would
run with the robot, and it only took a little tuning to get everything running effectively.
References
http://arduino.cc/en/Main/ArduinoMotorShieldR3
http://www.nteinc.com/specs/7400to7499/pdf/nte74HC14.pdf
http://www.fairchildsemi.com/ds/LM/LM317.pdf
http://www.fairchildsemi.com/ds/QR/QRD1114.pdf
Appendix
A. Drawings
Overall Dimensions of vehicle:




14 | P a g e


Chassis:

Motor Mount:





15 | P a g e





Motor Mount Spacer:

Lineduino sensor array:

16 | P a g e

B. Source Code
#include <PID_v1.h>
//----Sensor Pin Definitions
#define lir 4 //left IR sensor
#define cir 5 //center IR sensor
#define rir 6 //right IR sensor

/*----Motor shield pin config
dir=polarity(forward(1)/backward(0))
pwm=speed(0-255)
brake=engage(1)/disengage(0) brake
*/
#define lmotor_dir 12 //left motor direction pin
#define lmotor_PWM 3 //left motor PWM pin
#define lmotor_brake 9 //left motor brake pin
#define lmotor_sense A0 //left motor current sense pin

#define rmotor_dir 13 //right motor direction pin
#define rmotor_PWM 11 //right motor PWM pin
#define rmotor_brake 8 //right motor brake pin
#define rmotor_sense A1 //right motor current sense pin

//----PID Variables
float kp=40; //proportional constant
float ki=1; //integral constant
float kd=10; //derivative constant
double Isum, last_input, output; //used for error tracking
int motorMinSpeed = 20; //minimum motor speed
int motorMaxSpeed = 250; //maximum motor speed
#define min_out -motorMaxSpeed*2 //Isum min
#define max_out motorMaxSpeed*2 //Isum max
int outputScaleFactor = 3; //used to scale output to have greater effect on vehicle
int IRScaleFactor = 5; //used to scale output from sensor array
double errorDivideFactor = 2.25; //used to drive error to zero when only center sensor is activated

int motorStartSpeed = motorMaxSpeed; //speed to start running motor at
int loopDelay = 50; //sets frequency of main loop
int lastpos = 0; //variable tracks last direction in case vehicle falls off line
int constrainedPWML = motorStartSpeed; //speed used in PID control
int constrainedPWMR = motorStartSpeed;
int constrainedPWM = motorStartSpeed;
int motorOffsetR = 255 - motorMaxSpeed; //used to correct for difference between motors

void setup(){
//IR Sensor Setup
pinMode(lir, INPUT);
pinMode(cir, INPUT);
pinMode(rir, INPUT);

//Motor Pin Setup
pinMode(lmotor_dir, OUTPUT);
pinMode(lmotor_PWM, OUTPUT);
pinMode(lmotor_brake, OUTPUT);
pinMode(lmotor_sense, INPUT);
pinMode(rmotor_dir, OUTPUT);
pinMode(rmotor_PWM, OUTPUT);
pinMode(rmotor_brake, OUTPUT);
pinMode(rmotor_sense, INPUT);
digitalWrite(lmotor_dir, 1);
digitalWrite(rmotor_dir, 0);
digitalWrite(lmotor_brake, 0);
17 | P a g e

digitalWrite(rmotor_brake, 0);
//Start motors driving forward to look for line
forward(motorStartSpeed);
}

void loop(){
//find position of line relative to sensor
double ir_pos=get_position();
//Compute error setpoint(0)-input(ir_pos)
double error=0-ir_pos;
if (error==0){
//drive error to zero if only center sensor is active
Isum-=Isum/errorDivideFactor;
}
else{
//accumulate error
Isum+=(ki*error);
}
//Prevent Integral Windup
Isum = constrain(Isum, min_out, max_out);
double dterm=(ir_pos-last_input);
//output is Kp*error + sum of error - Kd * rate of change of the error
output=constrain((kp*error+Isum-kd*dterm),(-255*2)/outputScaleFactor,(255*2)/outputScaleFactor);
//remember last directoin turned
last_input=ir_pos;
//Drive robot in correct direction, scaling output to have a greater effect
navigate(output*outputScaleFactor);
//set loop frequency
delay(loopDelay);
}

/* Function to drive robot in desired direction. */
void navigate(double output){
if(output>0){
left(motorStartSpeed, output);
}
else if(output<0){
right(motorStartSpeed, -output);
}
else if(output==0){
forward(motorStartSpeed);
}
}

/*Function to detect line position.
Value returned is between -1(directly under the left IR sensor) and 1(directly under the right IR sensor)
Values of -.5(left & center) and +.5(right & center) are retunred for intermediate positons.
*/
double get_position(){
double pos;
int left, right, center;
left=digitalRead(lir);
center=digitalRead(cir);
right=digitalRead(rir);
if((left==0)&&(center==1)&&(right==1)){
pos=-1;
}
else if((left==1)&&(center==1)&&(right==0)){
pos=1;
}
else if((left==1) && (center==0) && (right==1)){
pos=0;
18 | P a g e

output = 0;
//Isum = 0;
}
else if((left==0)&&(center==0)&&(right==1)){
pos=-0.5;
}
else if((left==1)&&(center==0)&&(right==0)){
pos=0.5;
}
else if((left==1)&&(center==1)&&(right==1)){
pos = lastpos;
}
else if((left==1)&&(center==1)&&(right==1)){
pos = lastpos;
}
else
pos=0;
//scale the output of the IR sensor array
return pos*IRScaleFactor;
}

/* Function to drive robot left. One motor is sped up while the other motor is slowed down in order to turn
quickly */
void left(int current_pwm, int dir_pwm){
constrainedPWML = constrain((current_pwm - dir_pwm/2),motorMinSpeed,motorMaxSpeed);
constrainedPWMR = constrain((current_pwm + dir_pwm/2),motorMinSpeed,motorMaxSpeed) +
motorOffsetR;
digitalWrite(rmotor_dir, LOW);
digitalWrite(lmotor_dir, HIGH);
digitalWrite(rmotor_brake, LOW);
digitalWrite(lmotor_brake, LOW);
analogWrite(lmotor_PWM, constrainedPWML);
analogWrite(rmotor_PWM, constrainedPWMR);
//remember last direction
lastpos = -1;
}

/* Function to drive robot right. One motor is sped up while the other mode is slowed down in order to turn
quickly */
void right(int current_pwm, int dir_pwm){
constrainedPWML = constrain((current_pwm + dir_pwm/2),motorMinSpeed,motorMaxSpeed);
constrainedPWMR = constrain((current_pwm - dir_pwm/2),motorMinSpeed,motorMaxSpeed +
motorOffsetR);
digitalWrite(rmotor_dir, LOW);
digitalWrite(lmotor_dir, HIGH);
digitalWrite(rmotor_brake, LOW);
digitalWrite(lmotor_brake, LOW);
analogWrite(lmotor_PWM, constrainedPWML);
analogWrite(rmotor_PWM, constrainedPWMR);
//remember last direction
lastpos = 1;
}

/* Function to drive robot forward. Both motors are driven forward at the same speed with a small offset to
account for slight mechanical differences. */
void forward(int pwm){
constrainedPWM = constrain(pwm,motorMinSpeed,motorMaxSpeed);
digitalWrite(rmotor_dir, LOW);
digitalWrite(lmotor_dir, HIGH);
digitalWrite(rmotor_brake, LOW);
digitalWrite(lmotor_brake, LOW);
analogWrite(rmotor_PWM, constrainedPWM + motorOffsetR);
19 | P a g e

analogWrite(lmotor_PWM, constrainedPWM);
//remember last direction
lastpos = 0;
}

/* Function to drive robot backwards. Both motors are driven forward at the same speed with a small offset
to account for slight mechanical differences. */
void backward(int pwm){
constrainedPWM = constrain(pwm,motorMinSpeed,motorMaxSpeed);
digitalWrite(rmotor_dir, HIGH);
digitalWrite(lmotor_dir, LOW);
digitalWrite(rmotor_brake, LOW);
digitalWrite(lmotor_brake, LOW);
analogWrite(rmotor_PWM, constrainedPWM + motorOffsetR);
analogWrite(lmotor_PWM, constrainedPWM);
//remember last direction
lastpos = 0;
}
C. Bill of Materials

D. Data sheet links
3299-3/8 Square Trim Potentiometer
http://www.bourns.com/pdfs/3299.pdf
LM317 3-Terminal Regulator
http://www.ti.com/lit/ds/symlink/lm117.pdf
NTE74HC14 Schmitt Trigger
http://www.nteinc.com/specs/7400to7499/pdf/nte74HC14.pdf
QRD1114 Reflectance Sensor
http://www.fairchildsemi.com/ds/QR/QRD1114.pdf
E. YouTube Link
https://www.youtube.com/watch?v=Dzxty2CPiTU Operational video
Component Count Supplier Web link to product Unit Cost Total Cost
0.1 uF capacitor 1 Spark Fun https://www.sparkfun.com/products/8375 0.25 0.25
1.0k ohm resistor 1 Spark Fun https://www.sparkfun.com/products/8980 0.25 0.25
10k ohm linear potentiometer 1 Spark Fun https://www.sparkfun.com/products/9939 0.95 0.95
10k ohm resistor 3 Spark Fun https://www.sparkfun.com/products/8377 0.25 0.75
330 ohm resistor 3 Spark Fun https://www.sparkfun.com/products/8377 0.25 0.75
9V Alkaline Battery 1 Spark Fun https://www.sparkfun.com/products/10218 1.95 1.95
Adjustable voltage regulator (LM317T) 1 Spark Fun https://www.sparkfun.com/products/527 1.95 1.95
Amico 6V N20 298:1 geared motor + wheel 2 Amazon http://www.amazon.com/gp/product/B00BG9L8YG/ref=oh_details_o00_s00_i00?ie=UTF8&psc=1 12.26 24.52
Amico 9V battery holder 1 Amazon
http://www.amazon.com/Amico-Replacement-Battery-Holder-
Connector/dp/B0087ZU4UG/ref=sr_1_3?ie=UTF8&qid=1386385497&sr=8-
3&keywords=9V+battery+holder 4.57 4.57
Arduino Motor Shield R3 1 Amazon http://www.amazon.com/gp/product/B006UTE70E/ref=oh_details_o00_s00_i00?ie=UTF8&psc=1 25.95 25.95
Arduino Uno R2 1 Amazon http://www.amazon.com/gp/product/B004CG4CN4/ref=oh_details_o00_s01_i00?ie=UTF8&psc=1 29.95 29.95
Ball Caster - 3/8 in 1 Spark Fun https://www.sparkfun.com/products/8909 2.95 2.95
Inverting Schmitt Trigger (74HC14) 2 Amazon
http://www.amazon.com/microtivity-Pack-74HC14-Schmitt-Trigger-
Inverter/dp/B0092CRLBS/ref=sr_1_1?ie=UTF8&qid=1386588204&sr=8-
1&keywords=schmitt+trigger 1.50 2.99
IR Reflective Sensor (QRD1114) 3 Spark Fun https://www.sparkfun.com/products/246 1.13 3.39
M2 hex nut 4 McMaster Carr http://www.mcmaster.com/#94150A305 0.05 0.21
M2 x 12mm machine screw 4 McMaster Carr http://www.mcmaster.com/#90116A022 0.10 0.42
M3 hex nut 8 McMaster Carr http://www.mcmaster.com/#94150A325 0.03 0.25
M3 x 14mm machine screw 8 McMaster Carr http://www.mcmaster.com/#90116A157 0.08 0.63
Total: 102.68
Bill of Materials

You might also like