What is the modification required in my code?

adith_kumar_shetty · August 12, 2023, 2:45pm

This project is a black line following robot on a white background.I have been using this code and everything is working pretty well,but i found that my vehicle if moved out of the track is moving everywhre randomly i just wanted few modifications in this programme which can stop the vehicle if moved to white background and i want this vehicle to stop in a black box that is at the arena.It would be great if someone would guide me in this process.
This is the code i have used in the process.

// Number of sensors to use
#define IR 8

// Sensor factor
#define KS 1000  

// Overlap factor
#define OL 200

// Base Speed Motors 
#define SpBSE 140 //140

// Calibration Speed Motors
#define SpBSB 40 //40

// Break Speed Motors
#define SpFWD 140 //140
#define SpREV 190 //190

// PID constants
float Kp = 0.020; // 0.020;
float Ki = 0.001; // 0.001;
float Kd = 0.010; // 0.010;
long P=0, I=0, D=0, PIDv=0, pErr=0;

// Sensor Pins
const byte pSen[IR] = {21, 20, 19, 18, 17, 16, 15, 14};

// LEDs Pins
const byte pLED[3] = {4, 7, 8};
int SenX[IR];
int MinX[IR];
int MaxX[IR];

// Sensor Position
unsigned long PosX = 0;
unsigned long PosM = (IR*KS-KS)/2;
int PosH = 0;

// Detection Line
bool detLe = false;

// Running Status
bool OnRun = false;

// Time Counters
unsigned long Tm0 = 0;
unsigned long Tm1 = 0;

// Motor-A
int IN1 = 5;
int IN2 = 11;

// Motor-B
int IN3 = 10;
int IN4 = 6;

// Speed Motors
int SMoA = 0;
int SMoB = 0;

// Switch Pin
const byte pSW = 13;

void CalSnX(void);
void BlinkX(void);
void EstSnX(void);
void PosLED(void);
void CalPID(void);
void MoCTRL(int x);

void setup(){
// Only to sensors test
//  Serial.begin(9600);
  
// Set all the motor control pins to outputs
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  
  // Motor A
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  
  // Motor B
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);

  // Start SW
  pinMode(pSW, INPUT);  
      
  for(byte i=0;i<3;i++){
    pinMode(pLED[i], OUTPUT);
    digitalWrite(pLED[i], LOW);
  }
  delay(1500);
  
  // Calibration Init
  digitalWrite(pLED[1], HIGH);
  CalSnX();
  digitalWrite(pLED[1], LOW);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  // Calibration End
  
  delay(500);  
}

void loop(){
  if(digitalRead(pSW)) OnRun=true;
  EstSnX();
  PosLED();
  CalPID();
  MoCTRL(OnRun);
}

void CalSnX(){
  Tm0 = millis();
  Tm1 = Tm0;
  unsigned long TmL;
  for(byte i=0; i<IR; i++){
    SenX[i]=analogRead(pSen[i]);
    MinX[i]=SenX[i];
    MaxX[i]=SenX[i];
  }
  while((millis()-Tm0)<=10000){
    for(byte i=0; i<IR; i++){
      SenX[i]=analogRead(pSen[i]);
      if(SenX[i]<MinX[i]) MinX[i]=SenX[i];
      if(SenX[i]>MaxX[i]) MaxX[i]=SenX[i];
    }
    TmL = millis();
    if ((TmL-Tm1)>=100){
      BlinkX();
      Tm1 = TmL;
    }
    analogWrite(IN2, SpBSB);
    analogWrite(IN4, SpBSB+10);
  }
}

void BlinkX(){
  for(byte i=0;i<3;i++){
    digitalWrite(pLED[i], !digitalRead(pLED[i]));
  }
}

void EstSnX(){
  detLe = false;
  unsigned long avgS = 0;
  unsigned int sumS = 0;
   
  for(byte i=0; i<IR; i++){
    SenX[i] = analogRead(pSen[i]);
    SenX[i] = map(SenX[i], MinX[i], MaxX[i], KS, 0);
    SenX[i] = constrain(SenX[i], 0, KS);
    if(SenX[i]>200)detLe = true;
    if(SenX[i]>50){
      avgS += (long)SenX[i]*(i*KS);
      sumS += SenX[i];
    }
  }
  if(detLe)PosX = avgS/sumS;
  else if(PosX < PosM) PosX = 0;
  else PosX = PosM*2;
  PosH = PosX-PosM;

/*
  // Only to sensors test //
  char DataX[100];
  char sPosH[6];
  sprintf(sPosH,"%5d", PosH);
  sprintf(DataX,"%4d  %4d  %4d  %4d  %4d  %4d  %4d  %4d  %4d", SenX[0], SenX[1], SenX[2], SenX[3], SenX[4], SenX[5], SenX[6], SenX[7], PosX);
  Serial.print(DataX);
  Serial.println(sPosH);
*/
}

void PosLED(){
  unsigned long TmL = millis();
  if((PosX>(PosM-KS/2))&&(PosX<(PosM+KS/2))) digitalWrite(pLED[1], HIGH);
  else digitalWrite(pLED[1], LOW);
  
  if(detLe){
    if(PosX<(PosM-OL)) digitalWrite(pLED[0], HIGH);
    else digitalWrite(pLED[0], LOW);
    if(PosX>(PosM+OL)) digitalWrite(pLED[2], HIGH);
    else digitalWrite(pLED[2], LOW);
  }
  else{
    if((PosX<(PosM-OL))&&((TmL-Tm1)>100)){
      digitalWrite(pLED[0], !digitalRead(pLED[0]));
      Tm1 = TmL;
    }
    if((PosX>(PosM+OL))&&((TmL-Tm1)>100)){
      digitalWrite(pLED[2], !digitalRead(pLED[2]));
      Tm1 = TmL;
    } 
  }
}

void CalPID(){
  P = PosX - PosM;
  I = P + pErr;
  D = P - pErr;
  PIDv = (Kp*P) + (Ki*I) + (Kd*D);
  pErr = P;
}

void MoCTRL(int x){
  int MoSpL = 0;
  int MoSpR = 0;
  if(detLe){
    if(x){
      MoSpL = SpBSE + PIDv;
      MoSpR = SpBSE - PIDv;
    
      MoSpL = constrain(MoSpL, 0, 255);
      MoSpR = constrain(MoSpR, 0, 255);
    
      // Motor A
      analogWrite(IN1, 0);
      analogWrite(IN2, MoSpL);  
      // Motor B
      analogWrite(IN3, MoSpR);
      analogWrite(IN4, 0);          
    }
    else{
      if(P<-OL){
        MoSpL = -PIDv;
        MoSpR = MoSpL;
        // Motor A
        analogWrite(IN1, MoSpL);
        analogWrite(IN2, 0);  
        // Motor B
        analogWrite(IN3, MoSpR);
        analogWrite(IN4, 0);
      }
      else if(P>OL){
        MoSpL = PIDv;
        MoSpR = MoSpL;
        // Motor A
        analogWrite(IN1, 0);
        analogWrite(IN2, MoSpL);  
        // Motor B
        analogWrite(IN3, 0);
        analogWrite(IN4, MoSpR);
      }
      else{
        // Motor A
        analogWrite(IN1, 0);
        analogWrite(IN2, 0);  
        // Motor B
        analogWrite(IN3, 0);
        analogWrite(IN4, 0); 
      }
    }
  }
  else{
    if(P==-PosM){
      MoSpL = SpFWD;
      MoSpR = SpREV;
      // Motor A
      analogWrite(IN1, MoSpL);
      analogWrite(IN2, 0);  
      // Motor B
      analogWrite(IN3, MoSpR);
      analogWrite(IN4, 0);     
    }
    else if(P==PosM){
      MoSpL = SpREV;
      MoSpR = SpFWD;
      // Motor A
      analogWrite(IN1, 0);
      analogWrite(IN2, MoSpL);  
      // Motor B
      analogWrite(IN3, 0);
      analogWrite(IN4, MoSpR);
    }
  }
}

It would be great if someone helps me in this process.PLease help me if you can as i have my competition the coming saturday.

UKHeliBob · August 12, 2023, 2:51pm

I have deleted your duplicate topic

Thank you for using code tags in this one, but why did you create a second topic when the code with tags could have been posted in the other one ?

paulpaulson · August 12, 2023, 2:55pm

Hello adith_kumar_shetty

This seem to be a "hidden" delay() function call.

  while((millis()-Tm0)<=10000){
    for(byte i=0; i<IR; i++){
      SenX[i]=analogRead(pSen[i]);
      if(SenX[i]<MinX[i]) MinX[i]=SenX[i];
      if(SenX[i]>MaxX[i]) MaxX[i]=SenX[i];
    }

Check it.

Have a nice day and enjoy coding in C++.

adith_kumar_shetty · August 13, 2023, 3:27am

So what should I be doing next sir?

paulpaulson · August 13, 2023, 3:35am

verify this hypothesis

adith_kumar_shetty · August 13, 2023, 4:07am

What should be the changes according to you sir?I have less knowledge about it,this code is designed by my friends altogether.

paulpaulson · August 13, 2023, 4:14am

I assume that you have written the programme by yourself or friends, then it is quite easy to find the error:

Use a logic analyzer to see what happens.

Insert Serial.println()´s at points of interrest and analyze the test results.

Have a nice day and enjoy coding in C++.

xfpd · August 13, 2023, 7:45pm

Have you considered programming and experimenting with each of the parts of this project separately? In doing this, you will have better knowledge of how each device should work alone, and when you combine all the devices and their code into an automated, line following robot, you will have the knowledge to identify the problems.

adith_kumar_shetty · August 28, 2023, 1:56pm

At kp = 0.020
Ki = 0.001
Kd = 0.010
My robot works well with speed 40.But I am clueless after many tries how to make it tune for speed 140.Please help me sir

xfpd · August 28, 2023, 3:57pm

This is PID control. I do not know how to help you with this. @jremington is an authority on PID.

system · February 24, 2024, 3:58pm

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Line follower robot not tracking line Programming	6	45	April 18, 2026
Line follower code improvement Programming	26	341	December 8, 2025
Line following robot using PID Robotics	31	755	May 7, 2025
Line follower pid didnt work General Guidance	35	358	December 24, 2024
Why is my line-following robot spinning around? General Guidance	11	181	September 18, 2025

What is the modification required in my code?

Related topics