#include <Servo.h>// Sensor valuesint value_middle = 0;int value_right = 0;int value_left = 0;// Averaged Sensor valuesfloat value_middle_ave = 0;float value_right_ave = 0;float value_left_ave = 0;// PWM, servo and default servo angleint pwm = 100;Servo servo;int angle = 80;//Laps completed and line sensor value (0 for black, 1 for white)volatile boolean completed = false;volatile boolean sensor = 0;//Timer variable initializationboolean flag = 0; // Flag after crossing three linesint timer = 0;float timer1 = 0;float timer2 = 0;float timer3 = 0;int linecount = 0;int lap = 0;float dur1 = 0;float dur2 = 0;float speedtimer1=0;float speedtimer2=0;// Speed thresholdsint fast = 230;int mediumfast = 110;int medium = 110;int slow = 80;int turning_speed = 180;void setup() { pinMode(0, INPUT); //adc sensor left pinMode(1, INPUT); //adc sensor right pinMode(2, INPUT); //adc sensor middle pinMode(8, INPUT); //front sensor TIMSK2=0x01; // Enable global and timer overflow interrupts; TCCR2A = 0x00; // Normal port operation TCNT2=0xEC; // 8bit counter register (Initalized to 248 for .1 millisecond overflow) TCCR2B = 0x6; // Start timer/ set clock (prescaler = 256) // Servo initialization servo.attach(9); servo.write(95); // Default servo angle pinMode(9, OUTPUT); // Servo pwm //motor pinMode(6, OUTPUT); // Motor pwm pinMode(4, OUTPUT); // Bit 1 pinMode(5, OUTPUT); // Bit 2 digitalWrite(4, HIGH); // HBridge control bits (Forward) digitalWrite(5, HIGH); // HBridge control bits (Forward)}void loop() { if (!completed) { // If vehicle has not completed full laps // Read all sensors read_sensors(); // Nothing in front, go straight full speed (Fastest threshold) if (value_middle > 150) { analogWrite(6, fast); check_bs(); // Check blindspot } //Medium fast threshold else if (value_middle < 140 && value_middle > 130) { analogWrite(6, mediumfast); turn(); // Turn according to sensor values } else if (value_middle < 130 && value_middle > 70) { analogWrite(6, medium); turn(); // Turn according to sensor values } else if (value_middle < 70 && value_middle > 30) { analogWrite(6,slow); turn(); // Turn according to sensor values } else if (value_middle < 22) backup(); // If something is too close, reverse. // Count laps and read bottom sensor if (flag == 0 && sensor == 1){ //when the sensor crosses from white area onto a black tape flag = 1; linecount = linecount + 1; //line count increments by 1 timer = timer + 1; // choose which timer to save the time in savetimer(); } if (flag == 1 && sensor == 0) flag = 0; // when the sensor gets off the black tape, flag is set to 0 if (timer >= 3){ calculatetime(); timer = 0; //if three lines are counted then reset timer to 0 to prepare for the next set of three lines } if (linecount == 15){ //if the line count is 15, then a lap is completed linecount = 0; lap = lap + 1; if (lap == 5) { // if 5 laps are completed, then car stops analogWrite(6, 0); completed = true; } } } else { // Else if completed is true // Fast stop analogWrite(6, 255); // Max speed digitalWrite(4, HIGH); // HBridge control bits (Reverse) digitalWrite(5, LOW); // HBridge control bits (Reverse) delay(600); while(1){ // Trap control in loop analogWrite(6, 0); // Speed zero servo.detach(); // Detach servo control digitalWrite(4, HIGH); // HBridge control bits (Motor off) digitalWrite(5, LOW); // HBridge control bits (Motor off) } }// end of else} // end of loop()void turn() { //for turning right analogWrite(6, turning_speed); if ((value_right - value_left) > 15) { // Difference threshold for turning servo.write(120); }//if //for turning left else if ((value_left - value_right) > 15) { // Difference threshold for turning servo.write(60);//60 }//if else if ((value_left > 158) && (value_right > 158)) servo.write(75); // if two distances are equal~ else if((value_left - value_right) == 0){ servo.write(60); }}// end of turn()//check the blind spotvoid check_bs() { if (value_right < 70) servo.write(60); // Anything within 70 cm of right sensor turn left else if (value_left < 70) servo.write(120); // Anything within 70 cm of left sensor turn right else servo.write(95); // Else default servo angle}// Read sensorsvoid read_sensors() { // ADC value smoothing for (int i = 0; i < 10; i++) { value_left += (12781.87224/(analogRead(A0)+6.118918705))-3; //left long range value_right += (12781.87224/(analogRead(A1)+6.118918705))-3; //right long range value_middle += (12781.87224/(analogRead(A2)+6.118918705))-3; //middle long range } // Take average values value_left_ave = value_left/10; value_right_ave = value_right/10; value_middle_ave = value_middle/10; // If value is valid, save value else assume out of range and default to max if ((value_middle_ave > 0) && (value_middle_ave < 160)) value_middle = value_middle_ave; else value_middle = 160; // If value is valid, save value else assume out of range and default to max if ((value_left_ave > 0) && (value_left_ave < 160)) value_left = value_left_ave; else value_left = 160; // If value is valid, save value else assume out of range and default to max if ((value_right_ave > 0) && (value_right_ave < 160)) value_right = value_right_ave; else value_right = 160;}// Reverse functionvoid backup() { digitalWrite(4, HIGH); //HBridge control bits (Reverse) digitalWrite(5, LOW); //HBridge control bits (Reverse) read_sensors(); //Read all sensors if ((value_right - value_left) > 10) servo.write(60);// left turn reverse if ((value_left - value_right) > 10) servo.write(120);// right turn reverse analogWrite(6, 250); //Fast reverse delay(2000); // Go forward digitalWrite(4, LOW); //HBridge control bits (Forward) digitalWrite(5, HIGH); //HBridge control bits (Forward) analogWrite(6, 255); //Max speed}//Save timer valuesvoid savetimer() { if (timer == 1) timer1 = millis(); if (timer == 2) timer2 = millis(); if(timer == 3) timer3 = millis();}// U-turn for direction correctionvoid uturn() { analogWrite(6, medium); // Medium speed if ((value_left - value_right) < 15) { // Turn towards side with larger range servo.write(60); delay(750); backup(); servo.write(95); } else if ((value_right - value_left) < 15){ // Turn towards side with larger range servo.write(120); delay(750); backup(); servo.write(95); }}// Calculate time difference between linesvoid calculatetime() { dur2 = timer3-timer2; dur1 = timer2-timer1; if (dur2 > dur1) { // If time 2 is less than time 1, uturn. uturn(); linecount = linecount-6; // Subtract six lines from line count }}//Timer overflow for polling bottom sensorISR(TIMER2_OVF_vect) { TCCR2B = 0x6; // Restart timer sensor = digitalRead(8);}