Maze solving code with PID Control by irtesam

#define PinL1 4
#define PinL2 5
#define PinR1 6
#define PinR2 7
#define ENA 3
#define ENB 11
#define ENASpeed 110
#define ENBSpeed 110
#define Sensor1 0
#define Sensor2 1
#define Sensor3 2
#define Sensor4 3
#define Sensor5 4


//stuffs needed for maze solving

int sensorWeight[5]={-7,-4,0,4,7};
int sensorData[5]={0,0,0,0,0};
int pathLog[8]={0,0,0,0,0,0,0,0};
int q=0;






void pathLogCorrection()
      {

        if(pathLog[q]==4)
        {

          pathLog[q]=0;
          q--;

          }
          else
          {
            q++;
          }

        }

void goForward()
{

     analogWrite(ENA,ENASpeed);
     analogWrite(ENB,ENBSpeed);

     digitalWrite(PinR1,HIGH);
     digitalWrite(PinR2,LOW);
     digitalWrite(PinL1,HIGH);
     digitalWrite(PinL2,LOW);

  }
void goRight()
{

     analogWrite(ENA,ENASpeed);
     analogWrite(ENB,ENBSpeed);

     digitalWrite(PinR1,HIGH);
     digitalWrite(PinR2,LOW);
     digitalWrite(PinL1,HIGH);
     digitalWrite(PinL2,HIGH);

     delay(800);

}
void goLeft()
{


     analogWrite(ENA,ENASpeed);
     analogWrite(ENB,ENBSpeed);

     digitalWrite(PinR1,HIGH);
     digitalWrite(PinR2,HIGH);
     digitalWrite(PinL1,HIGH);
     digitalWrite(PinL2,LOW);

     delay(800);

}

void takeUturn()
{


     analogWrite(ENA,ENASpeed);
     analogWrite(ENB,ENBSpeed);

     digitalWrite(PinR1,HIGH);
     digitalWrite(PinR2,LOW);
     digitalWrite(PinL1,LOW);
     digitalWrite(PinL2,HIGH);

     delay(1500);
  }

void stopMotor()
{
     analogWrite(ENA,0);
     analogWrite(ENB,0);

     digitalWrite(PinR1,LOW);
     digitalWrite(PinR2,LOW);
     digitalWrite(PinL1,LOW);
     digitalWrite(PinL2,LOW);


  }
void goExtraInch()
{

  goForward();
  delay(500);

  }

void runSolvedPath()
{


   //Emon eita tui likhbi :D //sathe Delay er bepar tao dekhis //ar PID er sathe jeno Maze er code clash na kore tao dekhis


 }





 //stuffs needed for PID


  const int Kp=7,Ki=0,Kd=3;
  int error=0;
  int prev_error=0;
  int correction=0;
  int mod_ENASpeed;
  int mod_ENBSpeed;

  int p=0;
  int i=0;
  int d=0;

   //calculating error to go in the PID


   void errorCalc()
{
   error=0;

  for(int j=0;j<5;j++)
  {
    error=error+(sensorWeight[j]*sensorData[j]);
  }

  }


 void PID()
 {

  p=error;
  i=error+prev_error;
  d=error-prev_error;

  prev_error=error;

  correction = (Kp*p )+ (Ki*i )+ (Kd*d) ;

  mod_ENASpeed=ENASpeed-correction;
  mod_ENBSpeed=ENBSpeed+correction;




  Serial.print("Correction:");
  Serial.println(correction);




 }
 void applyCorrection()

 {

  analogWrite(ENA,mod_ENASpeed);
  analogWrite(ENB,mod_ENBSpeed);

  Serial.print("mod_ENASpeed:");
  Serial.println(mod_ENASpeed);

  Serial.print("mod_ENBSpeed:");
  Serial.println(mod_ENBSpeed);



  digitalWrite(PinR1,HIGH);
  digitalWrite(PinR2,LOW);
  digitalWrite(PinL1,HIGH);
  digitalWrite(PinL2,LOW);

 }



void setup() {
 pinMode(Sensor1,INPUT);
 pinMode(Sensor2,INPUT);
 pinMode(Sensor3,INPUT);
 pinMode(Sensor4,INPUT);
 pinMode(Sensor5,INPUT) ;

 pinMode(PinR1,OUTPUT);
 pinMode(PinR2,OUTPUT);
 pinMode(PinL1,OUTPUT);
 pinMode(PinL2,OUTPUT);


 pinMode(ENA,OUTPUT);
 pinMode(ENB,OUTPUT);
 Serial.begin(9600);
}

void printSensor()
{

 for(int j=0;j<5;j++)
{

  Serial.print(sensorData[j]);
  Serial.print(" ");


}


Serial.println("");


}

void loop() {






  readSensor();
  printSensor();


  runMotor();
  errorCalc();
  PID();
  applyCorrection();
  delay(300);


}


void readSensor()
{


  for(int i=0;i<5;i++)
  {


    if((analogRead(i)>350))

    {
      sensorData[i] = 0;
    }
    else
    {

      sensorData[i] = 1;

      }

    }

  }



  void runMotor()

 {

 if(sensorData[0]==0 && sensorData[1]==0 && sensorData[2]==1 && sensorData[3]==0 && sensorData[4]==0) //could be 8 or dead end

  {
    goExtraInch();
    readSensor();
   if(sensorData[0]==0 && sensorData[1]==0 && sensorData[2]==1 && sensorData[3]==0 && sensorData[4]==0) //path 8
    {

    goForward();
    delay(300);//robot will go straight
    pathLog[q]=2;
    pathLogCorrection();

                 }
    }
    else
    {

      takeUturn(); //dead end
      q--;

      }
  }
 if(sensorData[0]==1 && sensorData[1]==1 && sensorData[2]==1 && sensorData[3]==0 && sensorData[4]==0) //could be 1 or 5

  {
    goLeft();       //as left is out priority ,either way robot has go left
    pathLog[q]=1;
    pathLogCorrection();

  }
  if(sensorData[0]==0 && sensorData[1]==0 && sensorData[2]==1 && sensorData[3]==1 && sensorData[4]==1) //could be 2 or 6

  {

    goExtraInch();

    readSensor();

    if(sensorData[0]==0 && sensorData[1]==0 && sensorData[2]==1 && sensorData[3]==0 && sensorData[4]==0)  // path 6

    {

      goForward(); //robot will go forward
      pathLog[q]=2;
      pathLogCorrection();


      }
      else // path 2
      {

        goRight(); //robot will go right
        pathLog[q]=3;
        pathLogCorrection();

      }

  }

   if(sensorData[0]==1 && sensorData[1]==1 && sensorData[2]==1 && sensorData[3]==1 && sensorData[4]==1) //could be path 3 or 4 or 7

   {


    goExtraInch();
    readSensor();
    if(sensorData[0]==1 && sensorData[1]==1 && sensorData[2]==1 && sensorData[3]==1 && sensorData[4]==1) // path 7

    {

        stopMotor();
        delay(5000); //will run along the solved path after 10 seconds of running to the end of the maze
        runSolvedPath();

      }


    else //could be path 3 or 4

    {
      goLeft() //but either way ,our robot has to go left
      pathLog[q]=1;
      pathLogCorrection();
    }
   }
 }