LineFollower or LineTracking Rover with PID on Arduino UNO

#define BLYNK_PRINT Serial
#include <BlynkSimpleSerialBLE.h>
#include <SoftwareSerial.h>

// You should get Auth Token in the Blynk App.
// Go to the Project Settings (nut icon).
char auth[] = "65176043c54744dbb5615ff0a722898a";

SoftwareSerial SerialBLE(10, 11); // RX, TX

//*************************************************************
int Trig=5;
int Echo=3;
int INA=2;
int INB=4;
int ENA=6;

int INC=7;
int IND=8;
int ENB=9; //TAMBIEN EST USADO PARA LA MSICA (cable blanco cortado)

//float LMotor_offset,RMotor_offset;
float Turn_Speed = 0;//Turn_Speed_K = 0;

float Run_Speed = 0; //Run_Speed_K = 0, Run_Speed_T = 0;*/
float LOutput=0;float ROutput=0;
//Variable en la que se va a almacenar el valor correspondiente a la distancia
int Dist,Dist1,Dist2;
// APP DEL MVIL
 int x,y;

///Sensores line follower algoritmo sencillo
int D = A0;
int C = A1;
int I = A2;
float Val_D=0;
float Val_C=0;
float Val_I=0;
float Turn_Speed_LF=0;
int MODO_LINE_FOLLOWER;

///// PID
long sensors_weighted_sum;
int sensors_sum;
int position1;
long sensors[] = {0, 0, 0}; //Array used to store 3 readings for 3 sensors
float kp=0;//0.2 y resto a cero va algo bien   0.1
float kd=1.7;//20                                  3
float ki=0.1;//1.2                               0.1 
float proportional, integral, derivative,last_proportional,error_value,error_value1;


BLYNK_WRITE(V10) {
  x = param[0].asInt(); // It extracts the x value of the Joystick inlcuded in the Mobile APP Blynk
  y = param[1].asInt(); // It extracts the y value of the Joystick inlcuded in the Mobile APP Blynk
}


void setup()
{
  // Debug console
  pinMode(13,INPUT);
  Serial.begin(9600);
  Serial.println("FICHERO coche26_with_blynk_or_lineFollower_IN13.ino");
  delay(2000);
  MODO_LINE_FOLLOWER=digitalRead(13); //Si la input 13 est a 0 se comanda desde el Mvil (Blynk)
    if (MODO_LINE_FOLLOWER==LOW){
  SerialBLE.begin(9600);
  Blynk.begin(SerialBLE, auth);
  Serial.println("Waiting for connections...");
  Serial.println(" Arranca con modo control desde mvil con Blynk porque IN13==LOW....");
  delay(5000);
    }
  else{
      Serial.println(" Arranca con modo seguimiento de lnea con PID porque IN13==HIGH .....");
  }
  pinMode (Trig, OUTPUT);
  pinMode (Echo, INPUT);
  pinMode(INA,OUTPUT);
  pinMode(INB,OUTPUT);
  pinMode(INC,OUTPUT);
  pinMode(IND,OUTPUT);
  x=0;
  y=0;
  LOutput=0; 
  ROutput=0;
}

void loop()
{
  MODO_LINE_FOLLOWER=digitalRead(13); //Si la input 13 est a 0 se comanda desde el Mvil (Blynk)
  if (MODO_LINE_FOLLOWER==LOW){
      //Serial.println(" Selected control by the Smartphone by Blynk dashboard due to IN13==LOW....To change the mode, put the IN13 to 5v and reset the arduino");
      Blynk.run(); // To Run Blynk
      XY_TO_RUN_TURN_SPEEDS_BLYNK(); //X of the Joystick represent the turn speed and Y the RUN speed
      PWMControl1();// It controls the logic for the Motors H bridge 
  }
  else{
      //Serial.println(" LineTracking or following mode selected due to IN13==HIGH .....A PID is implemented in the code. To change the mode put the <in13 to "0v" and press the reset on the Arduino");
      sensor_bar();
      pid(position1);
      calc_turn();
      /*
      /////////////////////Descomentar para usar algoritmo 2 ( EN CASO DE PROBAR COMENTAR LAS DOS LNEAS ANTERIORES)
      Val_D = analogRead(D);
      Val_C = analogRead(C);
      Val_I = analogRead(I)*1.1;
      Turn_Speed_LF=LineFollower(Val_D,Val_C,Val_I);
      RUN_TURN_SPEEDS_LF();
      /////////////////////FIN USO ALGORITMO 2 SIN PID
      */
      PWMControl2();
      }
  }
  void sensor_bar(){
    sensors_weighted_sum = 0;
      sensors_sum = 0;
      for (int i = 0; i < 3; i++){
      sensors[i] = analogRead(i);
      sensors_weighted_sum += sensors[i] * i * 1000; //calculating the weighted sum
      sensors_sum += int(sensors[i]);} //Calculating sum of sensor readings
      position1=int(sensors_weighted_sum/sensors_sum);//Calculating the weighted mean
  }
  void pid(int calculated_position){
    proportional=1000-calculated_position;  //The setpoint for the position is at the middle of the value compose by the 3 sensors --> 1000. The calculated position range goes 0 to 2000
    integral=integral+proportional;
    derivative=proportional-last_proportional;
    last_proportional=proportional;
    error_value=float(proportional*kp+integral*ki+derivative*kd); //kp is set to zero since the setpont of position is fixed
    integral=constrain(integral,-350,350);
  }
void calc_turn()
{
//Restricting the error value between +256 and -256
if (error_value< -256){
                      error_value = -256;
                      }
if (error_value> 256){
                      error_value = 256;
                     }
 Run_Speed=map(abs(error_value),0,180,100,40); // It is just to reduce the Run_Speed when the error_value is growing and viceversa.
 
 LOutput = Run_Speed + error_value;  //output reference value for the Left motor Half bridge
 ROutput = Run_Speed - error_value;  //output reference value for the Right motor Half bridge
 
 //// next prints are for debugging
  Serial.print(ROutput);
  Serial.print(",");
  Serial.println(LOutput);
}

void XY_TO_RUN_TURN_SPEEDS_BLYNK()
{
if(x!=128){
    Turn_Speed=map(x,0,255,50,-50);  //Speed to turn to left and right
    }else{
          Turn_Speed=0;    
          }
if(y!=128){
    Run_Speed=map(y,0,255,-120,120);   //Speed to run forward and backwards
          }else{
                Run_Speed=0;
                }
LOutput = Run_Speed + Turn_Speed;
ROutput = Run_Speed - Turn_Speed;  
  }

///////Algoritmo 2: Un algoritmo simple poco eficiente . Si la velocidad es alta pierde la lnea y no recupera////////////////
int LineFollower(int d,int c,int i){
  int u,u1,u2;
   u=(d+c)-(c+i);
   u2=u1;
   u1=1.5*u; 
   return u;
}
void RUN_TURN_SPEEDS_LF(){ /// CON MUCHA VELOCIDAD SE SALE Y NO CONTROLA ERROR. POR ESO USAR UN PID.
  Run_Speed=75;
  LOutput = Run_Speed + Turn_Speed_LF;
  ROutput = Run_Speed - Turn_Speed_LF;  
  }
//////////fin algoritmo 2///////////

  void PWMControl1(){  //// H-Bridge logics   //To be used when IN13=LOW what means that the Blynk link is used
  if(LOutput > 0){
    digitalWrite(INA, HIGH);
    digitalWrite(INB, LOW);
  }
  else if(LOutput < 0){
    digitalWrite(INA, LOW);
    digitalWrite(INB, HIGH);
  }
  else{
    digitalWrite(INA, HIGH);
    digitalWrite(INB, HIGH);
  }
  if(ROutput > 0){
    digitalWrite(INC, HIGH);
    digitalWrite(IND, LOW);
  }
  else if(ROutput < 0){   
    digitalWrite(INC, LOW);
    digitalWrite(IND, HIGH);
  }
  else{
    digitalWrite(INC, HIGH);
    digitalWrite(IND, HIGH);
  }
    ultrasonido (Dist);
    Dist1=Dist;
    if (Dist1>1) {
                  analogWrite(ENA, min(255, abs(LOutput)));
                  analogWrite(ENB, min(255, abs(ROutput)));
    }else{
                 analogWrite(ENA, 0);
                 analogWrite(ENB, 0);
         }
   
}
 void PWMControl2(){  // H-Bridge logics
  if(LOutput > 0){
    digitalWrite(INA, HIGH);
    digitalWrite(INB, LOW);
  }
  else if(LOutput < 0){
    digitalWrite(INA, LOW);
    digitalWrite(INB, HIGH);
  }
  else{
    digitalWrite(INA, HIGH);
    digitalWrite(INB, HIGH);
  }
  if(ROutput > 0){
    digitalWrite(INC, HIGH);
    digitalWrite(IND, LOW);
  }
  else if(ROutput < 0){   
    digitalWrite(INC, LOW);
    digitalWrite(IND, HIGH);
  }
  else{
    digitalWrite(INC, HIGH);
    digitalWrite(IND, HIGH);
  }
    analogWrite(ENA, min(255, abs(LOutput)));
    analogWrite(ENB, min(255, abs(ROutput)));
}

//Este mdulo calcula y devuelve la distancia en cm.
/*
Puedes poner el cdigo del mdulo directamente en el loop o utilizar el mdulo
para reducir el nmero de lneas de cdigo del loop o reutilizar el cdigo
*/
void ultrasonido (int &Distancia){
//Para estabilizar el valor del pin Trig se establece a LOW
digitalWrite (Trig, LOW);
delay(10);
//Se lanzan los 8 pulsos
digitalWrite (Trig, HIGH);
delay(10);
digitalWrite (Trig, LOW);
/*
Se mide el tiempo que tarda la seal en regresar y se calcula la distancia.
Observa que al realizar pulseIn el valor que se obtiene es tiempo, no distancia
Se est reutilizando la variable Distancia.
*/
Distancia= pulseIn (Echo, HIGH);
Distancia=Distancia/58;
delay(10); 
}