IARC Techkriti Wall Following

Wall_Following.ino

#include <NewPing.h>
#define TRIGGER_PINF  50
#define ECHO_PINF     51
#define TRIGGER_PINR  26 
#define ECHO_PINR     28 
#define TRIGGER_PINL  48 
#define ECHO_PINL     49 
#define SONAR_NUM 3      // Number of sensors.
#define MAX_DISTANCE 100 // Maximum distance (in cm) to ping.

#define Left_Dir1 25
#define Left_Dir2 24
#define Left_Speed 9
#define Right_Dir1 23
#define Right_Dir2 22
#define Right_Speed 10
float FrontDistance=200.0;
float RightDistance=200.0;
float LeftDistance=5.0;
NewPing sonar[SONAR_NUM] = {   // Sensor object array.
  NewPing(TRIGGER_PINF, ECHO_PINF, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping. 
  NewPing(TRIGGER_PINR, ECHO_PINR, MAX_DISTANCE), 
  NewPing(TRIGGER_PINL, ECHO_PINL, MAX_DISTANCE)
};
int sensor[7] = {A0,A1,A2,A3,A4,A5,A6}; // Pin assignment
int botSpeed=180;
int minBotSpeed=100;
int range=botSpeed-minBotSpeed;
float LsensorStart=0.0;  // Sensor reading array
float RsensorStart=0.0; // Sensor reading array
int activeSensor = 0; // Count active sensors
float SensorCenter=0.0;
int LOffSensor=0;
int ROffSensor=0;
float Kp = 300;//50  // Max deviation = 8-4.5 = 3.5 ||  255/3.5 = 72
float Kd = 30;//400
float Ki = 0.0; //100.0;

float error = 0.0;
float previousError = 0.0;
float totalError = 0;

float power = 0;
int prevActiveSensor=0;
int PWM_Right=botSpeed,PWM_Left=botSpeed,sens[7],truecount = 0,falsecount = 0;

void setup()
{
  Serial.begin(9600);
  pinMode(Left_Dir1, OUTPUT);
  pinMode(Left_Dir2, OUTPUT);
  pinMode(Left_Speed, OUTPUT);
  pinMode(Right_Dir1, OUTPUT);
  pinMode(Right_Dir2, OUTPUT);
  pinMode(Right_Speed, OUTPUT);
  for(int i=0; i<7; i++) 
  {
    pinMode(sensor[i], INPUT);
  }
  pinMode(13,OUTPUT);
  //Stop();
  Forward();
}
int time = 0;   
void blink(){
  digitalWrite(13,HIGH);
  delay(10);
  digitalWrite(13,LOW);
}
void loop()
{
  /*sensorread();
  if(sens[1]==1&&sens[2]==1&&sens[3]==1&&sens[4]==1&&sens[5]==1){
    if(time==0){
      Forward();
      Go(botSpeed);
      delayMicroseconds(120);
      time = 1;
    }
  }
  else time = 0;
  
  if(sens[1]==1&&sens[3]==0&&sens[5]==1)     // checking for node!!
  {
    blink();
    Forward();
    int nodeend = 0,truenode=1;
    while(!nodeend){
        Go(botSpeed);
        sensorread();
        if(sensor[3]==1){
          blink();
          truenode = 0;
        }
        if(sens[1]==1&&sens[3]==0&&sens[5]==1){
          nodeend=1;
          
          while(digitalRead(sensor[3])){
             
             Forward();
             Go(botSpeed);}   
        }
        blink();
    }
    if(truenode)truecount++;
    else falsecount++;
  }
  */
 PID_program();
}
void sensorread()    //reading the values from the sensor.
{
  for(int i = 0;i<7;i++){
    sens[i] = !digitalRead(sensor[i]);  
  }
}

void Left_Forward() 
{
  digitalWrite(Left_Dir1, LOW);
  digitalWrite(Left_Dir2, HIGH);
}

void Left_Reverse() 
{
  digitalWrite(Left_Dir1, HIGH);
  digitalWrite(Left_Dir2, LOW);
}

void Right_Forward()
{
  digitalWrite(Right_Dir1, LOW);
  digitalWrite(Right_Dir2, HIGH);
}

void Right_Reverse()
{
  digitalWrite(Right_Dir1, HIGH);
  digitalWrite(Right_Dir2, LOW);
}

void Go(int s)
{
  analogWrite(Right_Speed, s);
  analogWrite(Left_Speed, s);
}

void Forward()
{
  Left_Forward();
  Right_Forward();
}

void Reverse() 
{
  Left_Reverse();
  Right_Reverse();
}

void Turn_Left() 
{
  Right_Forward();
  Left_Reverse();
}

void Turn_Right() 
{
  Right_Reverse();
  Left_Forward();
}

void Stop() 
{
  analogWrite(Left_Speed,0);
  analogWrite(Right_Speed, 0);
  for(int i=0; i<7; i++)
   { Serial.print(analogRead(sensor[i]));
     Serial.print(" ");  }
  while(1);
  
}
void PID_program()
{ 
    readWallSensor();
    int error1;  
    error1 = LeftDistance-5.0; // Count how much robot deviate from center 
    error = -1*(error1 - previousError)+1000.0*exp(-1.0*(FrontDistance)*(FrontDistance)/324.0);
 //   Serial.println(error);
    totalError +=error; // Accumulate error for integral
    power = (Kp*error) + (Kd*(error-previousError)) + (Ki*totalError);
  previousError = error1; // save previous error for differential 
  if(power>0) // Turn left
      {
      PWM_Right = botSpeed;
      PWM_Left = botSpeed - abs(int(power));
      PWM_Left=constrain(PWM_Left,minBotSpeed,botSpeed);
      analogWrite(Left_Speed, PWM_Left);
      analogWrite(Right_Speed, PWM_Right);
      Forward();
    }
  else if(power<0)// Turn right
    { 
      PWM_Right = botSpeed- abs(int(power));
      PWM_Left = botSpeed ;
      PWM_Right=constrain(PWM_Right,minBotSpeed,botSpeed);
      analogWrite(Left_Speed, PWM_Left);
      analogWrite(Right_Speed, PWM_Right); 
      Forward();   
    }
//   else if( power>range&&power<=2*range ) 
//    { 
//      power = power-range; 
//      PWM_Right = botSpeed;
//      PWM_Left = minBotSpeed+power;
//      Turn_Left();
//      analogWrite(Left_Speed, PWM_Left);
//      analogWrite(Right_Speed, PWM_Right);
//    }
//   else if( power<-range&&power>=-2*range ) 
//    {  
//      power =-range-power; 
//      PWM_Right = minBotSpeed+power;
//      PWM_Left = botSpeed;
//      Turn_Right();
//      analogWrite(Left_Speed, PWM_Left);
//      analogWrite(Right_Speed, PWM_Right);
//    }
//    else if(power>2*range)
//    {
//      PWM_Left=botSpeed;
//      PWM_Right=botSpeed;
//      Turn_Left();
//      analogWrite(Left_Speed, PWM_Left);
//      analogWrite(Right_Speed, PWM_Right);
//    }
//    else if(power<-2*range)
//    {
//      PWM_Left=botSpeed;
//      PWM_Right=botSpeed;
//      Turn_Right();
//      analogWrite(Left_Speed, PWM_Left);
//      analogWrite(Right_Speed, PWM_Right);
//    } 
    else if (power==0)
    {
      PWM_Right = botSpeed;
      PWM_Left = botSpeed ;
      analogWrite(Left_Speed, PWM_Left);
      analogWrite(Right_Speed, PWM_Right);
      Forward();
   }
   Serial.print(PWM_Left);
   Serial.println(PWM_Right);
}

void lineFollow(void) {
   PID_program(); 
}

void readSensor(void) 
{
 // prevActiveSensor=activeSensor;
  LsensorStart=0;LOffSensor=0;
RsensorStart=0;ROffSensor=0;
activeSensor=0;
 for(int i=0; i<=6; i++)
 {
  if(!digitalRead(sensor[i]))
  {
    LsensorStart=i+1;
    break;
  }
  LOffSensor++;
 }
 for(int i=6; i>=0; i--)
 {
  if(!digitalRead(sensor[i]))
  {
    RsensorStart=i+1;
    break;
  }
  ROffSensor++;
 }
 activeSensor=7-LOffSensor-ROffSensor;
 if(activeSensor==-7)activeSensor=0;
 //Serial.println(activeSensor);
 /*if(activeSensor<0)
 {
  Stop();
 }
 else 
{*/
  if(activeSensor>0)SensorCenter=(LsensorStart+RsensorStart)/2.0;
  else SensorCenter=4.0;
  //Serial.print(LsensorStart);
  //Serial.print(" ");
  //Serial.print(RsensorStart);
  //Serial.print(" ");
  //Serial.println((SensorCenter-4.0));
 //}
}
void readWallSensor()
{
  activeSensor=-1;
  delay(50);
 FrontDistance=sonar[0].convert_cm(sonar[0].ping_median());
  if(FrontDistance==0)
  {
    FrontDistance=MAX_DISTANCE;
    activeSensor++;
  }
  //Serial.print(FrontDistance);
  //Serial.print(" ");
  //delay(50);
  //RightDistance=sonar[1].convert_cm(sonar[1].ping_median());
  //if(RightDistance>0)activeSensor++;
  delay(50);
  LeftDistance=sonar[2].convert_cm(sonar[2].ping_median());
  if(LeftDistance==0)
  {
    LeftDistance=MAX_DISTANCE;
    activeSensor++;
  }
  //Serial.println(LeftDistance);
}