I've attached a basic copy of the line following program i've been working on. So far, i've tested the motor speed functionality and the operation of the servo with the sensors. It seems to be working, but will definitely need to be calibrated for steering and motor speed. I have not attached an interrupt or something yet that will detect when it hits the line and needs to transfer over to the next lane.
plz reply with comments and any help with further improvements would be really appreciated.!!!
#include<stdlib.h>
#include<math.h>
#include<string.h>
#define SERVO1 8
#define MAX_PERIOD 40000 //20ms
#define SERVO_UPPER_LIMIT 4800 // 90 deg right (1850 from middle to right) 20.55 per degree
#define SERVO_LOWER_LIMIT 1100 // 90 deg left (1850 from middle to left)
void ext0_handler( void );
void setup_timer1( void );
void set_timer1( unsigned int val );
volatile int t1_state = 0;
volatile int S1 = 2950; // 0 degrees is 2950
volatile int S2 = 40000; // 20ms low pulse
int direct = 0; // direction of servo
int speedlow = 0; // speed for hard turn
int speedmed = 125; // speed for med turn
int speedhigh = 250; // speed for straight away
int counter = 0; // testing counter
int binary = 0; // variable for line sensors
int leftouter = LOW; // sensor inputs
int leftinner = LOW;
int middle = LOW;
int rightinner = LOW;
int rightouter = LOW;
int pinleftouter = 2;
int pinleftinner = 3;
int pinmiddle = 4;
int pinrightinner = 5;
int pinrightouter = 6;
int pinmotor = 10;
ISR( TIMER1_OVF_vect )
{
switch( t1_state )
{
case 0:
set_timer1(65536 - S1);
digitalWrite(SERVO1,HIGH);
t1_state = 1;
break;
case 1:
set_timer1(65536 - S2);
digitalWrite(SERVO1,LOW);
t1_state = 0;
break;
}
}
void setup()
{
Serial.begin(9800);
Serial.print("Initializing...");
pinMode(SERVO1,OUTPUT);
pinMode(pinleftouter,INPUT);
pinMode(pinleftinner,INPUT);
pinMode(pinmiddle,INPUT);
pinMode(pinrightinner,INPUT);
pinMode(pinrightouter,INPUT);
pinMode(pinmotor,OUTPUT);
digitalWrite(SERVO1,LOW); //setup default states for signals
setup_timer1();
sei(); // enable global interrupts
Serial.print("Done\r\n");
}
void loop()
{
binary = 0;
getBinary();
lineCase();
delay(100);
counter++;
//Serial.print("End of count :");
//Serial.println(counter);
}
void getBinary()
{
rightouter = digitalRead(pinrightouter);
rightinner = digitalRead(pinrightinner);
middle = digitalRead(pinmiddle);
leftinner = digitalRead(pinleftinner);
leftouter = digitalRead(pinleftouter);
Serial.print(leftouter);
Serial.print(" ");
Serial.print(leftinner);
Serial.print(" ");
Serial.print(middle);
Serial.print(" ");
Serial.print(rightinner);
Serial.print(" ");
Serial.print(rightouter);
binary = rightouter*1 + rightinner*2 + middle*4 + leftinner*8 + leftouter*16;
Serial.print(" ");
Serial.print("Binary: ");
Serial.println(binary);
/*binary = rightouter << 0; // forces nth bit of x to be state. all other bits left alone
binary = rightinner << 1;
binary = middle << 2;
binary = (leftinner << 3);
binary = (leftouter << 4);*/
}
void lineCase()
{
switch(binary)
{
case 16: // left outer
//Serial.print("B10000 ");
//Serial.println(binary);
analogWrite(pinmotor,speedlow);
S1 = 3875;//digitalWrite(SERVO1,3875); // turn right 45 degrees
break;
case 24: // left outer, left middle
//Serial.print("B11000 ");
//Serial.println(binary);
analogWrite(pinmotor,speedlow);
S1 = 3875;//digitalWrite(SERVO1,3875); // turn right 45 degrees
break;
case 8: // left middle
//Serial.print("B01000 ");
//Serial.println(binary);
analogWrite(pinmotor,speedmed);
S1 = 3258;//digitalWrite(SERVO1,3258); // turn right 15 degrees
break;
case 12: // left middle, middle
//Serial.print("B01100 ");
//Serial.println(binary);
analogWrite(pinmotor,speedmed);
S1 = 3258;//digitalWrite(SERVO1,3258); // turn right 15 degrees
break;
case 4: // middle
//Serial.print("B00100 ");
//Serial.println(binary);
analogWrite(pinmotor,speedhigh);
S1 = 2950;//digitalWrite(SERVO1,2950); // turn 0 degrees
break;
case 6: // middle, right middle
//Serial.print("B00110 ");
//Serial.println(binary);
analogWrite(pinmotor,speedmed);
S1 = 2642;//digitalWrite(SERVO1,2642); // turn left 15 degrees
break;
case 2: // right middle
//Serial.print("B00010 ");
//Serial.println(binary);
analogWrite(pinmotor,speedmed);
S1 = 2642;//digitalWrite(SERVO1,2642); // turn left 15 degrees
break;
case 3: // right middle, right outer
//Serial.print("B00011 ");
//Serial.println(binary);
analogWrite(pinmotor,speedlow);
S1 = 2025;//digitalWrite(SERVO1,2025); // turn left 45 degrees
break;
case 1: // right outer
//Serial.print("B00001 ");
//Serial.println(binary);
analogWrite(pinmotor,speedlow);
S1 = 2025;//digitalWrite(SERVO1,2025); // turn left 45 degrees
break;
case 0: // all white
//Serial.print("B00000 ");
//Serial.println(binary);
break;
case 33: // all black
//Serial.print("B11111 ");
//Serial.println(binary);
break;
}
}
void setup_timer1( void )
{
//Initialize Timer 1
TCCR1A = 0x00;
//Setup Prescale
TCCR1B = 0x02; //divide by 8
//Unmask overflow interrupt
TIMSK1 = 0x01;
return;
}
void set_timer1(unsigned int val)
{
//High byte
TCNT1H = (val >> 8) & 0x00FF;
//Low byte
TCNT1L = val & 0x00FF;
}