code

#include <Arduino.h>
#define led 13
#define dryrunswitch 12
#define actualrunswitch 11
#define AIN1 6
#define BIN1 9
#define AIN2 5
#define BIN2 8
#define PWM1 3
#define PWM2 10
#define stby 7
#define basespeed 150
#define Kp 12  // to be determined
#define Kd 10
#define l 1
#define s 2
#define r 3
#define b 4
#define finish 5

void motor1(bool dir1,bool dir2,uint8_t pwm);
void motor2(bool dir1,bool dir2,uint8_t pwm);
uint8_t index=0;
uint8_t path [100];
bool linestate [5];
uint8_t error,error_prev;

void linefollow(){
  error_prev=error;
  error=linestate[0]*(-2)+linestate[1]*(-1)+linestate[3] + linestate[4]*2;
  if(error==0){
  motor1(HIGH,LOW,basespeed);
  motor2(HIGH,LOW,basespeed);
  delay(100);
  motor1(LOW,LOW,0);
  motor2(LOW,LOW,0);
  delay(5);  
  }
  else if(error>0){
  uint8_t in=Kp*error+Kd*(error-error_prev);
  motor1(HIGH,LOW,in+basespeed/4);
  in=(in>(basespeed/2))?(in-(basespeed/2)):((basespeed/2)-in);
  motor2(LOW,HIGH,in);
  delay(100);
  motor1(LOW,LOW,0);
  motor2(LOW,LOW,0);
  }
  else {
  uint8_t in=Kp*error+Kd*(error-error_prev);
  motor2(HIGH,LOW,in+basespeed/4);
  in=(in>basespeed/4)?(in-basespeed):(basespeed-in);
  motor1(LOW,HIGH,in);
  delay(100);
  motor1(LOW,LOW,0);
  motor2(LOW,LOW,0);        
}
}

void turnleft(){
  motor1(HIGH,LOW,basespeed);
  motor2(HIGH,LOW,basespeed);
  delay(300);
  motor1(HIGH,HIGH,0);
  motor2(HIGH,LOW,basespeed/2);
  while(1){
    readir();
    if(linestate[2]==0){
      break;
    }
  }
  while(1){
    readir();
    if(linestate[2]==1){
      break;
    }
  }
  motor2(HIGH,HIGH,0);
}

void turnright(){
  motor1(HIGH,LOW,basespeed/2);
  motor2(HIGH,HIGH,0);
  while(1){
    readir();
    if(linestate[2]==0){
      break;
    }
  }
  while(1){
    readir();
    if(linestate[2]==1){
      break;
    }
  }
  motor1(HIGH,HIGH,0);
}

void turnback(){
  motor1(HIGH,LOW,basespeed/2);
  motor2(LOW,HIGH,basespeed/2);
  while(1){
  readir();
  if(linestate[2]==0){
    break;
  }
  }
  while(1){
    readir();
    if(linestate[2]==1){
      break;
    }
  }
  motor1(HIGH,HIGH,0);
  motor2(HIGH,HIGH,0);
}


void readir(){
  for(int i=0;i<5;i++){
     linestate[i]=digitalRead(i+14);
  }
}

void motor1(bool dir1,bool dir2,uint8_t pwm){
  digitalWrite(AIN1,dir1);
  digitalWrite(AIN2,dir2);
  analogWrite(PWM1,pwm);
}

void motor2(bool dir1,bool dir2,uint8_t pwm){
  digitalWrite(BIN1,dir1);
  digitalWrite(BIN2,dir2);
  analogWrite(PWM2,pwm);
}


void setup(){
  ADCSRA |=(1<<ADPS2);
  ADCSRA &=~(1<<ADPS1);
  ADCSRA &=~(1<<ADPS0);
  pinMode(AIN1,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(PWM1,OUTPUT);
  pinMode(BIN1,OUTPUT);
  pinMode(BIN2,OUTPUT);
  pinMode(PWM2,OUTPUT);
  pinMode(actualrunswitch,INPUT);
  pinMode(dryrunswitch,INPUT);
  pinMode(led,OUTPUT);
  digitalWrite(stby,HIGH);
}

void loop(){
  if (digitalRead(dryrunswitch)){
    while (1){
      readir();
      
      if((linestate[0]+linestate[1]+linestate[2]+linestate[3]+linestate[4])<=3){//the bot has to follow a straight line
        linefollow();
      }

      else{

      if((linestate[0]==1) && (linestate[1]==1) && (linestate[2]==1)){//need to be modified after test run
        path[index]=l;
        index++;
        delay(100);
        motor1(HIGH,HIGH,0);
        motor2(HIGH,HIGH,0);
        delay(1000);
        turnleft();
      }

      else if((linestate[4]==1) && (linestate[3]==1) && (linestate[2]==1)){
          motor1(HIGH,LOW,basespeed);
          motor2(HIGH,LOW,basespeed);
          delay(300);
          motor1(HIGH,HIGH,0);
          motor2(HIGH,HIGH,0);
          readir();

          if((linestate[0]+linestate[1]+linestate[2]+linestate[3]+linestate[4])==3){//needs to be modified
            linefollow();
            path[index]=s;
            index++;
          }
          else if((linestate[0]+linestate[1]+linestate[2]+linestate[3]+linestate[4])==0){
            turnback();
            path[index]=b;
            index++;
          }

          else if((linestate[0]+linestate[1]+linestate[2]+linestate[3]+linestate[4])==5){
            //end reached
            digitalWrite(led,HIGH);
            path[index]=finish;
            break;
          }
          else{ 
            path[index]=r;
            index++;
            turnright();
          }
        }
      }


    }
  }
   //optmising the path
   uint8_t newpath [100]; 
   uint8_t newindex =0;
   for(int j=0;j<30;j++){
   for(int i=0;i<index;i++){
    if((path[i]==l) && (path[i+1]==b) && (path[i+2]==l)){
      i+=2;
      newpath[newindex]=s;
      newindex++;
      }
      else if((path[i]==l) && (path[i+1]==b) && (path[i+2]==r)){
        newpath[newindex]=b;
        newindex++;
        i+=2;
      }
      else if((path[i]==s) && (path[i+1]==b) && (path[i+2]==l)){
        newpath[newindex]=r;
        newindex++;
        i+=2;
      }
      else if((path[i]==r) && (path[i+1]==b) && (path[i+2]==l)){
        newpath[newindex]=b;
        newindex++;
        i+=2;
      }
      else {
        newpath[newindex]=path[i];
        newindex++;
      }
    }
    for(int i=0;i<newindex;i++){
      path[i]=newpath[i];
    }
    index=newindex;
  }
  
    newindex=0;
    if (digitalRead(actualrunswitch)==HIGH){
      while(1){
      if((linestate[0]+linestate[1]+linestate[2]+linestate[3]+linestate[4])==3){
        linefollow();
      }
      else {
        motor1(HIGH,LOW,basespeed);
        motor2(HIGH,LOW,basespeed);
        delay(350);
        motor1(HIGH,HIGH,0);
        motor2(HIGH,HIGH,0);
        switch(newpath[newindex]){
          case l:
          turnleft();
          break;

          case s:
          linefollow(); 
          break;

          case r:
          turnright();
          break;
          
          case finish :{
            motor1(HIGH,HIGH,0);
            motor2(HIGH,HIGH,0);
            digitalWrite(led,HIGH);
            while(1){
              delay(5);
            }
        }
      }
    }
    
   }
  }
}