[ Science Tutorial #1 ] - The World's Smallest Line Follower Robot!

one white : ideal situation, move straight

both black : no prior : random walk
: prior : turn in direction of last white
Sorry if that was hard to understand
*/

#include "Arduino.h"

// the pin definitions
#define lmotorpin 1 // PB1 pin 6
#define rmotorpin 0 // PB0 pin 5
#define lsensepin 3 //ADC3 pin 2
#define rsensepin 1 //ADC1 pin 7
#define ledpin 4 //PB4 pin 3

// numbers for random walk and memory
#define steplength 300
#define smallturn 200
#define bigturn 500
#define memtime 1000

uint8_t lspd, rspd;
uint16_t lsenseval, rsenseval, lwhiteval, rwhiteval;

// functions
void testSensors();
void followEdge();
void moveTime(uint8_t lspeed, uint8_t rspeed, uint16_t time);
void move(uint8_t lspeed, uint8_t rspeed);
void stop();
void senseInit();
void flashLED(uint8_t flashes);

// just for convenience and simplicity (HIGH is off)
#define ledoff PORTB |= 0b00010000
#define ledon PORTB &= 0b11101111

void setup(){
pinMode(lmotorpin, OUTPUT);
pinMode(rmotorpin, OUTPUT);
pinMode(2, INPUT);
pinMode(3, INPUT);
ledoff;
pinMode(ledpin, OUTPUT);
analogWrite(lmotorpin, 0);
analogWrite(rmotorpin, 0);

lspd = 17;
rspd = 17;

// give a 6 second pause to set the thing on a white surface
lsenseval = 6;
while(lsenseval){
lsenseval--;
flashLED(1);
delay(989);
}
flashLED(4);
delay(500);
senseInit();
}

void loop(){
followEdge();
}

void followEdge(){
// now look for edge
uint8_t lastMove = 1; //0=straight, 1=left, 2=right
unsigned long moveEndTime = 0; // the millis at which to stop
unsigned long randomBits = micros();

unsigned long prior = 0; // after edge encounter set to millis + memtime
uint8_t priorDir = 0; //0=left, 1=right, 2=both
uint8_t lastSense = 1; //0=edge, 1=both white, 2=both black
uint8_t i = 0; // iterator

while(true){
// only update about once every 20ms
delay(18);
// read the value 4 times and average
ledon;
delay(2);
lsenseval = 0;
rsenseval = 0;
for(i=0; i<4; i++){
lsenseval += analogRead(lsensepin);
rsenseval += analogRead(rsensepin);
}
// don't divide by 4 because it is used below
ledoff;

if(randomBits == 0){ randomBits = micros(); }

if((lsenseval > lwhiteval3) && (rsenseval > rwhiteval3)){
// both white - if prior turn to black, else random walk
if(lastSense == 2 || millis() < prior){
// turn toward last black or left
if(priorDir == 0){
moveEndTime = millis()+smallturn;
move(0, rspd); // turn left
lastMove = 1;
}else if(priorDir == 1){
moveEndTime = millis()+smallturn;
move(lspd, 0); // turn right
lastMove = 2;
}else{
moveEndTime = millis()+bigturn;
move(0, rspd); // turn left a lot
lastMove = 1;
}
}else{
// random walk
if(millis() < moveEndTime){
// just continue moving
}else{
if(lastMove){
moveEndTime = millis()+steplength;
move(lspd, rspd); // go straight
lastMove = 0;
}else{
if(randomBits & 1){
moveEndTime = millis()+smallturn;
move(0, rspd); // turn left
lastMove = 1;
}else{
moveEndTime = millis()+smallturn;
move(lspd, 0); // turn right
lastMove = 2;
}
randomBits >>= 1;
}
}
}
lastSense = 1;

}else if((lsenseval > lwhiteval3) || (rsenseval > rwhiteval3)){
// one white - this is the edge
// just go straight
moveEndTime = millis()+steplength;
move(lspd, rspd); // go straight
lastMove = 0;
lastSense = 0;
prior = millis()+memtime;
if(lsenseval > lwhiteval*3){
// the right one is black
priorDir = 1;
}else{
// the left one is black
priorDir = 0;
}

}else{
// both black - if prior turn to white, else random walk
if(lastSense == 1 || millis() < prior){
// turn toward last white or left
if(priorDir == 0){
moveEndTime = millis()+smallturn;
move(lspd, 0); // turn right
lastMove = 2;
}else if(priorDir == 1){
moveEndTime = millis()+smallturn;
move(0, rspd); // turn left
lastMove = 1;
}else{
moveEndTime = millis()+bigturn;
move(lspd, 0); // turn right a lot
lastMove = 2;
}
}else{
// random walk
if(millis() < moveEndTime){
// just continue moving
}else{
if(lastMove){
moveEndTime = millis()+steplength;
move(lspd, rspd); // go straight
lastMove = 0;
}else{
if(randomBits & 1){
moveEndTime = millis()+smallturn;
move(0, rspd); // turn left
lastMove = 1;
}else{
moveEndTime = millis()+smallturn;
move(lspd, 0); // turn right
lastMove = 2;
}
randomBits >>= 1;
}
}
}
lastSense = 2;
}
}
}

void moveTime(uint8_t lspeed, uint8_t rspeed, uint16_t time){
analogWrite(lmotorpin, lspeed);
analogWrite(rmotorpin, rspeed);
delay(time);
analogWrite(lmotorpin, 0);
analogWrite(rmotorpin, 0);
}

void move(uint8_t lspeed, uint8_t rspeed){
analogWrite(lmotorpin, lspeed);
analogWrite(rmotorpin, rspeed);
}

void stop(){
analogWrite(lmotorpin, 0);
analogWrite(rmotorpin, 0);
}

// stores the average of 16 readings as a white value
void senseInit(){
lwhiteval = 0;
rwhiteval = 0;
ledon;
delay(2);
for(uint8_t i=0; i<16; i++){
lwhiteval += analogRead(lsensepin);
delay(1);
rwhiteval += analogRead(rsensepin);
delay(9);
}
lwhiteval >>= 4;
rwhiteval >>= 4;
ledoff;
}

void flashLED(uint8_t flashes){
while(flashes){
flashes--;
ledon;
delay(200);
ledoff;
if(flashes){ delay(500); }
}
}