// Line following software for Solarbotics sumovore with Atmel brainboard. /* Craig Limber, July 2004. version 1.0 permission is given to copy and modify this program all you want. Just remember where it came from! This code is based on the software I used for my gunnagitcha line-following bot I compete with at the western canadian robot games. For this version of the software I wanted to see how accurately I could make it run on my personal line-following torture tracks while keeping the robot hardware as stock as possible. */ #include #include #include enum {LINE4, LINE3, LINE2, LINE1, LINE0}; enum { FORWARD, // 0 STOP, // 1 NORMALLEFT, // 2 SNAPLEFT, // 3 NORMALRIGHT,// 4 SNAPRIGHT, // 5 BACKUP // 6 }; uint32_t maxlast[2] = {900, 700}, stoptime[2] = {600, 700}, linetimeout, linelost[2] = {40, 30}; int32_t lastleft = 0, lastright = 0; uint8_t motortab[16][2][2] = { // PWM range, top speed, motor {{ 0, 0}, { 0, 0}}, {{ 26, 26}, { 32, 32}}, {{ 39, 39}, { 48, 48}}, {{ 52, 52}, { 64, 64}}, {{ 65, 65}, { 80, 80}}, {{ 78, 78}, { 96, 96}}, {{ 91, 91}, {112, 112}}, {{104, 104}, {128, 128}}, {{117, 117}, {144, 144}}, {{130, 130}, {160, 160}}, {{143, 143}, {176, 176}}, {{156, 156}, {192, 192}}, {{169, 169}, {208, 208}}, {{182, 182}, {224, 224}}, {{195, 195}, {240, 240}}, {{208, 208}, {255, 255}} }, topspeed; // current speed, 0 = slow, 1 = fast //------------------------------------------------------------------------- void init(void) { /* sumovore atmel brainboard pinout +--------------------------------+ | 1 pc6 reset adc5/SCL pc5 28 | no connect (NOT FOR LONG) IR left | 2 pd0 rxd adc4/SDA pc4 27 | line left LINE4 IR right | 3 pd1 txd adc3 pc3 26 | line left cent LINE3 LED4 | 4 pd2 INT0 adc2 pc2 25 | line center LINE2 LED3 | 5 pd3 INT1 adc1 pc1 24 | line right cent LINE1 LED2 | 6 pd4 XCK/T0 adc0 pc0 23 | line right LINE0 | 7 VCC GND 22 | | 8 GND AREF 21 | | 9 pb6 XTAL1 AVCC 20 | | 10 pb7 XTAL2 SCK pb5 19 | left motor direction LED1 | 11 pd5 T1 MISO pb4 18 | right motor direction LED0 | 12 pd6 AIN0 MOSI/OC2 pb3 17 | servo1 servo 2 | 13 pd7 AIN1 !SS/OC1B pb2 16 | left motor PWM servo 3/speed | 14 pb0 ICP OC1A pb1 15 | right motor PWM +--------------------------------+ */ // set up ports as outputs DDRB = _BV(DDB1) // right motor PWM | _BV(DDB2) // left motor PWM | _BV(DDB3) // servo 1 | _BV(DDB4) // right motor direction | _BV(DDB5); // left motor direction DDRD = _BV(DDD2) // LED 4 | _BV(DDD3) // LED 3 | _BV(DDD4) // LED 2 | _BV(DDD5) // LED 1 | _BV(DDD6); // LED 0 // turn on pullups sbi(PORTB, PB0); // servo header 3 - jumper on = fast, off = slow // set up PB0 and PB1 with 8bit PWM TCCR1A = _BV(WGM10) // 8 bit, phase correct PWM | _BV(COM1A1) | _BV(COM1B1); TCCR1B = _BV(CS10) // no prescale | _BV(WGM12) ; // fast PWM } //------------------------------------------------------------------------- uint8_t getsensor(uint8_t sensor) // // this here function gets what the sensor is reading and returns a 1 // if it is getting tickled, 0 if not // { switch(sensor) { case LINE0: return(PINC & _BV(PINC4)); break; case LINE1: return(PINC & _BV(PINC3)); break; case LINE2: return(PINC & _BV(PINC2)); break; case LINE3: return(PINC & _BV(PINC1)); break; case LINE4: return(PINC & _BV(PINC0)); break; } return(0); } //------------------------------------------------------------------------- uint8_t getsensres(void) // // this here function returns a 4 bit value that represents what the line // sensors see. Also sets what the LEDs are seeing. { uint8_t result; if (PINB & _BV(PINB0)) topspeed = 0; else topspeed = 1; result = 0; if (getsensor(LINE4)) { lastleft = 0; } else { if (lastleft < maxlast[topspeed]) { sbi(PORTD, PD2); lastleft++; } else cbi(PORTD, PD2); } if (getsensor(LINE0)) { lastright = 0; } else { if (lastright < maxlast[topspeed]) { sbi(PORTD, PD6); lastright++; } else cbi(PORTD, PD6); } if (getsensor(LINE1)) { result |= 0x4; sbi(PORTD, PD5); } else cbi(PORTD, PD5); if (getsensor(LINE2)) { result |= 0x2; sbi(PORTD, PD4); } else cbi(PORTD, PD4); if (getsensor(LINE3)) { result |= 0x1; sbi(PORTD, PD3); } else cbi(PORTD, PD3); return(result); } //------------------------------------------------------------------------- void setmotorspeeds(uint8_t left, uint8_t leftdir, uint8_t right, uint8_t rightdir, uint8_t topspeed) { OCR1B = motortab[left][topspeed][0]; OCR1A = motortab[right][topspeed][1]; if (leftdir == FORWARD) sbi(PORTB, PB5); else cbi(PORTB, PB5); if (rightdir == FORWARD) sbi(PORTB, PB4); else cbi(PORTB, PB4); } //------------------------------------------------------------------------- void go(uint8_t dir) { switch (dir) { case STOP: setmotorspeeds( 0, FORWARD, 0, FORWARD, topspeed); break; case FORWARD: setmotorspeeds(15, FORWARD, 15, FORWARD, topspeed); break; case NORMALLEFT: setmotorspeeds( 4, BACKUP, 15, FORWARD, topspeed); break; case SNAPLEFT: setmotorspeeds(14, FORWARD, 14, BACKUP, topspeed); break; case NORMALRIGHT: setmotorspeeds(15, FORWARD, 4, BACKUP, topspeed); break; case SNAPRIGHT: setmotorspeeds(14, BACKUP, 14, FORWARD, topspeed); break; case BACKUP: setmotorspeeds(15, BACKUP, 15, BACKUP, 3); break; } } //------------------------------------------------------------------------- int main (void) { uint32_t x, loop; uint8_t sensres, last; init(); go(STOP); for (loop=0; loop<3000000; loop++) x--; // let human oppressor remove finger go(FORWARD); last = FORWARD; while (1) { sensres = getsensres(); switch(sensres) { case 0x0: // lost site of the line if (linetimeout < linelost[topspeed]) // wait until line really gone { linetimeout++; break; } if ((lastleft < maxlast[topspeed]) && (lastleft < lastright)) { go(BACKUP); for (loop=0; loop