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
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
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