Created
February 11, 2017 09:45
-
-
Save e-Gizmo/6d84573846fe22edb9fc5df70418e3e2 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
e-Gizmo e-Bot 4x4 Line Follower | |
This demo program will make the eBOT detect to follow | |
a black line path laid out on a light colored surface. | |
track width is 15-19mm wide. | |
I/O Usage | |
- collision sensor inputs (not used in this demo) | |
2- colision1 as input | |
3- colision2 as input | |
4- colision3 as input | |
-line sensor inputs | |
5- linesense1 as input low on black | |
6- linesense2 as input | |
7- linesense3 as input | |
-motor control output (default PBOT pin) | |
8 - mot2 dir as output high=fwd | |
9 - mot2 run as output | |
11 - mot1 dir as output high= fwd | |
10 - mot1 run as output | |
March 30,2016 | |
Codes by e-Gizmo Mechatronix Central | |
http://www.e-gizmo.com | |
*/ | |
int col1= 2; | |
int col2= 3; | |
int col3= 4; | |
int ls1 = 5; | |
int ls2 = 6; | |
int ls3 = 7; | |
int m2dir = 8; | |
int m2run = 9; | |
int m1dir = 11; | |
int m1run = 10; | |
long int rtc; | |
// Initialization routine | |
void setup() { | |
pinMode(m2dir, OUTPUT); | |
pinMode(m2run, OUTPUT); | |
pinMode(m1dir, OUTPUT); | |
pinMode(m1run, OUTPUT); | |
pinMode(13, OUTPUT); | |
rtc=millis()+10; | |
} | |
int colsense = 0; | |
int linesense=0; | |
int giveup=0; | |
int lastsense; | |
int runspeed=90; | |
//timers | |
byte retry_dly=0; | |
byte ledflsh=25; | |
// Main Line following program loop | |
void loop() | |
{ | |
// Hardware Timer service | |
if(millis()>=rtc) | |
{ | |
rtc=rtc+10; | |
if(retry_dly>0) retry_dly--; | |
if(ledflsh>0) | |
{ | |
ledflsh--; | |
if(ledflsh==0) | |
{ | |
ledflsh=25; | |
PORTB ^= 0x20; | |
} | |
} | |
} | |
// read the status of colision and line sensors | |
colsense=0; | |
linesense=0; | |
if(digitalRead(col1)==LOW) colsense=1; | |
if(digitalRead(col2)==LOW) colsense=colsense+2; | |
if(digitalRead(col3)==LOW) colsense=colsense+4; | |
if(digitalRead(ls1)==LOW) linesense=1; | |
if(digitalRead(ls2)==LOW) linesense=linesense+2; | |
if(digitalRead(ls3)==LOW) linesense=linesense+4; | |
// if no line is detected (all high) | |
if((linesense==0) & (retry_dly==0)) | |
{ | |
if(giveup<10) | |
{ | |
//reverse for 20mS | |
if(lastsense==1) runBot(runspeed*15/10,runspeed,LOW); | |
if(lastsense==3) runBot(runspeed*12/10,runspeed,LOW); | |
if(lastsense==4) runBot(runspeed,runspeed*15/10,LOW); | |
if(lastsense==6) runBot(runspeed,runspeed*12/10,LOW); | |
delay(40); | |
giveup++; | |
} | |
else | |
Stop(); | |
//delay(1000); | |
} | |
// if line is detected | |
if(linesense!=0) | |
{ | |
lastsense=linesense; | |
giveup=0; | |
retry_dly=50; // retry delay=500ms | |
} | |
// extreme left | |
if(linesense==1) | |
{ | |
//Stop(); | |
dirfwd(); | |
analogWrite(m2run, runspeed); | |
analogWrite(m1run, 0); | |
// digitalWrite(m1dir,LOW); | |
delay(50); | |
} | |
// centered | |
if((linesense==2)|(linesense==7)) | |
{ | |
runBot(runspeed,runspeed,HIGH); | |
delay(50); | |
} | |
// skewed left | |
if(linesense==3) | |
{ | |
Stop(); | |
dirfwd(); | |
analogWrite(m2run, runspeed*12/10); | |
delay(50); | |
} | |
// extreme right | |
if(linesense==4) | |
{ | |
//Stop(); | |
dirfwd(); | |
analogWrite(m1run, runspeed); | |
analogWrite(m2run, 0); | |
// digitalWrite(m2dir,LOW); | |
delay(50); | |
} | |
// skewed right | |
if(linesense==6) | |
{ | |
Stop(); | |
dirfwd(); | |
analogWrite(m1run, runspeed*12/10); | |
delay(50); | |
} | |
} | |
void runBot(int spd1,int spd2, boolean direction ) | |
{ | |
digitalWrite(m2dir,direction); | |
digitalWrite(m1dir,direction); | |
analogWrite(m1run, spd1); | |
analogWrite(m2run, spd2); | |
} | |
void dirfwd(void) | |
{ | |
digitalWrite(m2dir,HIGH); | |
digitalWrite(m1dir,HIGH); | |
} | |
void rotBot(int speed, boolean direction) | |
{ | |
digitalWrite(m2dir,direction); | |
digitalWrite(m1dir,~direction); | |
analogWrite(m2run, speed); | |
analogWrite(m1run, speed); | |
} | |
void Stop(void) | |
{ | |
analogWrite(m2run, 0); | |
analogWrite(m1run, 0); | |
} | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment