Line

/*
2piLineFollower.c - Mark Moran 2014
Redistribution and use in source and binary forms, with or without
modification, are permitted freely.
Inspired by Pololu's QTRSensors & 3pi Line Follower apps written by Ben Schmide
l et al.
Written for ATmega328p running about 8MHz with internal RC time source.
(Should work with any AVR chip with 16bit & 8bit timers and enough pins)
Will work with external clock source but may need to change clock scale prefact
or.
Drives two motors connected to SN754410 quad H-bridge.
Detects lines using Pololu QTR-8RC chip (with sensors 7 & 8 removed)
*/
#include <avr/io.h>
#include <util/delay.h>
/*************** MOTOR FUNCTIONS ***************/
#define MOTORPORT PORTD
#define MOTORDDR DDRD
#define PIN1A PD3
#define PIN2A PD2
#define PIN3A PD7
#define PIN4A PD4
#define PIN12EN PD5
#define PIN34EN PD6
#define PIN12CNTR OCR0B
#define PIN34CNTR OCR0A
#define LEFTA PIN4A
#define LEFTB PIN3A
#define LEFTON PIN34EN
#define LEFTCNTR PIN34CNTR
#define LEFTBIT COM0A1
#define RIGHTA PIN1A
#define RIGHTB PIN2A
#define RIGHTON PIN12EN
#define RIGHTCNTR PIN12CNTR
#define RIGHTBIT COM0B1
static inline void initMotors(void) {
TCCR0A = (1<<WGM00) | (1<<WGM01); //set Timer0 to fast PWM
TCCR0B = (1<<CS00) | (1<<CS01); //clock at CPU/64 frequen
cy (488Hz)

// set all 6 motor pins to outputs
MOTORDDR |= (1<<PIN1A) | (1<<PIN2A) | (1<<PIN3A) | (1<<PIN4A) | (1<<PIN12EN)
| (1<<PIN34EN);
}
static inline void setLeftSpeed(int16_t speed) {
uint8_t reverse = 0;
if (speed < 0) {
speed = -speed;
reverse = 1;
}
if (speed > 255)
speed = 255;

if (speed == 0)
TCCR0A &= ~(1<<LEFTBIT); // disconnect timer from lef
t enable pin
else {
TCCR0A |= (1<<LEFTBIT); // re-connect timer to left
enable pin
LEFTCNTR = speed;
if (reverse) {
MOTORPORT |= (1<<LEFTB);
MOTORPORT &= ~(1<<LEFTA);
}
else {
MOTORPORT |= (1<<LEFTA);
MOTORPORT &= ~(1<<LEFTB);
}
}
}
static inline void setRightSpeed(int16_t speed) {
uint8_t reverse = 0;
if (speed < 0) {
speed = -speed;
reverse = 1;
}
if (speed > 255)
speed = 255;

if (speed == 0)
TCCR0A &= ~(1<<RIGHTBIT); // disconnect timer from r
ight enable pin
else {
TCCR0A |= (1<<RIGHTBIT); // recoonnect timer to rig
ht enable pin
RIGHTCNTR = speed;
if (reverse) {
MOTORPORT |= (1<<RIGHTB);
MOTORPORT &= ~(1<<RIGHTA);
}
else {
MOTORPORT |= (1<<RIGHTA);
MOTORPORT &= ~(1<<RIGHTB);
}
}
}
static inline void setSpeed(int16_t leftSpeed, int16_t rightSpeed) {
setLeftSpeed(leftSpeed);
setRightSpeed(rightSpeed);
}
static inline void brake(void) {
// hard brake by setting both motor sides high (or low)
MOTORPORT |= (1<<LEFTA) | (1<<LEFTB) | (1<<RIGHTA) | (1<<RIGHTB);
}
/*************** QTR FUNCTIONS ***************/
#define READPORT PORTC
#define READDDR DDRC
#define READPIN PINC // lines connected to PC0 - PC5
#define LEDPORT PORTB // optionally turn off IR LEDs to save energy
#define LEDDDR DDRB
#define PINLEDON PB5
#define SLOW 1
#define FAST 0
#define MAXTICKS 2500
#define QTRCNT 6
static inline void initQTRs(void) {
TCCR1B = (1<<CS11); // set Timer1 to /8 prescali
ng

LEDDDR |= (1<<PINLEDON); // enable LED pin as output
LEDPORT |= (1<<PINLEDON); // currently keep LEDs on al
l the time
}
uint16_t QTRtime[QTRCNT], QTRmax[QTRCNT], QTRmin[QTRCNT];
static inline void readQTRs(uint8_t forceSlow) {
uint8_t lastPin, i, done = 0;

for (i = 0; i < QTRCNT; i++) // clear out previous times
QTRtime[i] = 0;

READDDR |= 0b00111111; // set pins to output
READPORT |= 0b00111111; // drive them high

_delay_us(10); // wait 10us to charge capac
itors

READDDR &= 0b11000000; // set pins to input
READPORT &= 0b11000000; // turn off pull-up register
s

TCNT1 = 0; // start 16bit timer at 0
lastPin = READPIN;

while ((TCNT1 < MAXTICKS) && ((done < QTRCNT) || forceSlow)) // if force
Slow, always take MAXTICKS time
{
if (lastPin != READPIN) // if any of the pins change
d
{
lastPin = READPIN;
for (i = 0; i < QTRCNT; i++)
{
if ((QTRtime[i] == 0) && (!(lastPin & (1<<i)))) // did pin go
low for the first time
{
QTRtime[i] = TCNT1;
done++;
}
}
}
}
if (done < QTRCNT) // if we timed out, set any
pins that didn't go low to max
for (i = 0; i < QTRCNT; i++)
if (QTRtime[i] == 0)
QTRtime[i] = MAXTICKS;
}
void calibrateQTRs(void) {
uint8_t i, j;
for (j = 0; j < 10; j++) { // take 10 readings and find
min and max values
readQTRs(SLOW);
for (i = 0; i < QTRCNT; i++) {
if (QTRtime[i] > QTRmax[i])
QTRmax[i] = QTRtime[i];
if (QTRtime[i] < QTRmin[i])
QTRmin[i] = QTRtime[i];
}
}
}
void readCalibrated(void) {
uint8_t i;
uint16_t range;

readQTRs(FAST);

for (i = 0; i < QTRCNT; i++) { // normalize readings 0-1000
relative to min & max
if (QTRtime[i] < QTRmin[i]) // check if reading is withi
n calibrated reading
QTRtime[i] = 0;
else if (QTRtime[i] > QTRmax[i])
QTRtime[i] = 1000;
else {
range = QTRmax[i] - QTRmin[i];
if (!range) // avoid div by zero if min
& max are equal (broken sensor)
QTRtime[i] = 0;
else
QTRtime[i] = ((int32_t)(QTRtime[i]) - QTRmin[i]) * 1000 / range;
}
}
}
uint16_t readLine(void) {
uint8_t i, onLine = 0;
uint32_t avg; // weighted total, long befo
re division
uint16_t sum; // total values (used for di
vision)
static uint16_t lastValue = 0; // assume line is initially
all the way left (arbitrary)

readCalibrated();

avg = 0;
sum = 0;

for (i = 0; i < QTRCNT; i++) { // if following white line,
set QTRtime[i] = 1000 - QTRtime[i]
if (QTRtime[i] > 50) { // only average in values th
at are above a noise threshold
avg += (uint32_t)(QTRtime[i]) * (i * 1000);
sum += QTRtime[i];
if (QTRtime[i] > 200) // see if we're above the li
ne
onLine = 1;
}
}

if (!onLine) {
if (lastValue < ((QTRCNT-1)*1000/2)) // if last read right of cen
ter, return 0 (assume veered right)
return 0;
else // otherwise return max (ass
ume veered left)
return (QTRCNT-1)*1000;
}

lastValue = avg/sum; // no chance of div by zero
since onLine was true

return lastValue;
}
void spinAndCalibrate(void) {
uint8_t i;

for (i = 0; i < QTRCNT; i++) { // reset minimums and maximu
ms
QTRmax[i] = 0;
QTRmin[i] = MAXTICKS;
}

for(i = 0; i < 60; i++) {
if ((i < 15) || (i >= 45))
setSpeed(40, -40);
else
setSpeed(-40, 40);

calibrateQTRs();
_delay_ms(5);
}
setSpeed(0,0);
}
#define MAX 60
int main(void) {
int16_t proportional, derivative, lastProportional = 0, powerDiff;
uint16_t position;
uint32_t integral = 0;
uint8_t i;

initMotors();
initQTRs();

_delay_ms(1000); // pull hand away from s
witch

spinAndCalibrate();

while (1) {
position = readLine(); // 0 = far right, 5000 =
far left

proportional = ((int16_t)position) - 2500; // "proportional" term s
hould be 0 when we on the line
derivative = proportional - lastProportional; // change of position
integral += proportional; // sum of positions
lastProportional = proportional; // remember the last pos
ition

powerDiff = proportional/20 + integral/10000 + derivative*3/2;

if (powerDiff > MAX)
powerDiff = MAX;
else if (powerDiff < -MAX)
powerDiff = -MAX;

if (powerDiff < 0)
setSpeed(MAX, MAX+powerDiff);
else
setSpeed(MAX-powerDiff, MAX);
}
return 0;
}