I am building an arduino rc car with 2 ultra sonic sensors (HC-SR04)

akisv · January 23, 2024, 5:53pm

So i made an arduino robot with 2 ultrasonic sensors (HC-SR04) and all the functions of it work fine. If u try to push the forward button and it has an obstacle in front of it it does the evasive maneuver as it is supposed to same as the back button. Although when it is already running forward or backward it doesnt stop if it finds an obstacle in the way.

I am trying to figure out how to alter the code and make it stop while moving if it finds an obstacle in the way.Τhe commands to the robot (F,B etc.) are received from a mobile app that controls it via Bluetooth.
EDIT: I've simplified the code a bit and took out some functions and stuff that wasn't needed like the battery level, but i still haven't solved the problem...

int RenaPin = 13;
int Rin1Pin = 22;
int Rin2Pin = 24;
int Rin3Pin = 26;
int Rin4Pin = 28;
int RenbPin = 12;

int FenaPin = 11;
int Fin1Pin = 30;
int Fin2Pin = 32;
int Fin3Pin = 34;
int Fin4Pin = 36;
int FenbPin = 10;
char command;
//Initializing all variables

//Variables where distances measured are stored.
int distFront;
int distLeft;
int distRight;
int distBack;
//Minimum distance of 20cm from any obstacle.
int distLimit = 20;

//HCSR04 Variables 
int trig = 6;
int trig2 = 2;    
int echo =  7;
int echo2 = 3;    
unsigned long pulsetime = 0;



// --------------------------------------------------------------------------- Setup
void setup() {
Serial.begin(9600);
pinMode(trig,OUTPUT);
pinMode(trig2,OUTPUT);
pinMode(echo,INPUT);
pinMode(echo2,INPUT);

// Setup motors

pinMode(RenaPin, OUTPUT);
pinMode(Rin1Pin, OUTPUT);
pinMode(Rin2Pin, OUTPUT);
pinMode(Rin3Pin, OUTPUT);
pinMode(Rin4Pin, OUTPUT);
pinMode(RenbPin, OUTPUT);

pinMode(FenaPin, OUTPUT);
pinMode(Fin1Pin, OUTPUT);
pinMode(Fin2Pin, OUTPUT);
pinMode(Fin3Pin, OUTPUT);
pinMode(Fin4Pin, OUTPUT);
pinMode(FenbPin, OUTPUT);
  
}

void loop() {
  if (Serial.available() > 0) {
    command = Serial.read();
    motor_stop();   //initialize with motors stopped
    //Change pin mode only if new command is different from previous.
    switch (command) {
      case 'F':
        //Checks for obstacle since forward command is received.
        distFront = readDistanceF();  
        if(distFront < distLimit)
        {
          motor_stop();
          delay(500);
          drive_backward();  
          delay(300);
          motor_stop();
            
        distFront = readDistanceF();
        Serial.print("A");
        Serial.print(";");
        Serial.print(distFront); //send distance to MIT App
        Serial.print(";");
        Serial.print(0,DEC); //2nd value...dummy variable
        Serial.println(";");
        }
        else 
        { 
        //No obstacle----> move forward.
        drive_forward();  
        }
        break;
      case 'B':
      distBack = readDistanceB();  
        if(distBack < distLimit)
        {
          motor_stop();
          delay(500);
          drive_forward();  
          delay(300);
          motor_stop();
        distBack = readDistanceB();
       
        }
        else 
        drive_backward();  
        break;
      case 'L':
        turn_left();  
        break;
      case 'R':
        turn_right();  
        break; 
      case 'O' :
        drive_open();  
        break;
      case 'C'  :
        drive_close();  
        break; 

      case 'T':     //reads distance
        {
        distFront = readDistanceF();
        Serial.print("A");
        Serial.print(";");
        Serial.print(distFront); //send distance to MIT App
        Serial.print(";");
        Serial.print(0,DEC); //2nd value...dummy variable
        Serial.println(";");
        }
        break;
    }   
}
}

// --------------------------------------------------------------------------- Drive

void motor_stop()
{ 
  analogWrite(FenaPin, 0);
  analogWrite(FenbPin, 0);
  analogWrite(RenaPin, 0);
  analogWrite(RenbPin, 0);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, LOW);
delay(25);
}

void drive_forward()
{  
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);

}

void drive_backward()
{  
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);

}

void turn_left()
{ 
  analogWrite(FenaPin, 180);
  analogWrite(FenbPin, 180);
  analogWrite(RenaPin, 160);
  analogWrite(RenbPin, 160);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);

}

void turn_right()
{  
  analogWrite(FenaPin, 180);
  analogWrite(FenbPin, 180);
  analogWrite(RenaPin, 160);
  analogWrite(RenbPin, 160);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);
}
void drive_open()
{  
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);
}
void drive_close()
{  
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);
}
 

//Returns distance value from ultrasonic sensor
int readDistanceF()
{  
  digitalWrite(trig, LOW); 
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);       
  delayMicroseconds(10);               
  digitalWrite(trig,LOW);         
  pulsetime = pulseIn(echo, HIGH);  
  return pulsetime/58.2;//magic                                               
}
int readDistanceB()
{  
  digitalWrite(trig2, LOW); 
  delayMicroseconds(2);
  digitalWrite(trig2, HIGH);       
  delayMicroseconds(10);               
  digitalWrite(trig2,LOW);         
  pulsetime = pulseIn(echo2, HIGH);  
  return pulsetime/58.2;//magic                                               
}

xfpd · January 24, 2024, 12:06am

This second "begin" of Serial will cause trouble on your serial communications, but probably not the cause of your stated problem.

It doesn't stop BECAUSE it DOES NOT find an obstacle. What are the values (1/true/high or 0/false/low) of the two evaluations (distFront < distLimit) and (distBack < distLimit)?

[edit] PulseTime is global, and used in readDistanceF() and readDistanceB()... and in those, if PulseTime is less than 58.2... the int returned is zero.

qubits-us · January 24, 2024, 12:17am

Hi @akisv ,

Welcome to the forum..
love bots..

quick look, all the logic in the loop is wrapped inside a serial available if..
need some logic outside of this if, to do your checking and stopping if need be..

good luck.. ~q

akisv · January 24, 2024, 12:48am

The value of distLimit is 20 always, since thats the threshold for obstacles, and distFront and Back Basically come from the readDistanceF() and B. Do you have any suggestions ?

akisv · January 24, 2024, 12:48am

Hello, thanks for the welcome. Do you have any suggestions ?

qubits-us · January 24, 2024, 12:55am

sure, untested though, sorry..

int RenaPin = 13;
int Rin1Pin = 22;
int Rin2Pin = 24;
int Rin3Pin = 26;
int Rin4Pin = 28;
int RenbPin = 12;

int FenaPin = 11;
int Fin1Pin = 30;
int Fin2Pin = 32;
int Fin3Pin = 34;
int Fin4Pin = 36;
int FenbPin = 10;
char command;
//Initializing all variables

//Variables where distances measured are stored.
int distFront;
int distLeft;
int distRight;
int distBack;
//Minimum distance of 20cm from any obstacle.
int distLimit = 20;

//HCSR04 Variables
int trig = 6;
int trig2 = 2;
int echo =  7;
int echo2 = 3;
unsigned long pulsetime = 0;

//Calculate Battery Level1. Arduino
const float maxBattery1 = 8.0;
int perVolt1;                 // Percentage variable
float voltage1 = 0.0;         // Read battery voltage
int level1;
//Calculate Battery Level2. Motor
const float maxBattery2 = 6.0;
int perVolt2;                 // Percentage variable
float voltage2 = 0.0;         // Read battery voltage
int level2;


long previousMillis = -1000 * 10;
long interval = 1000 * 10;
unsigned long currentMillis;

// --------------------------------------------------------------------------- Setup
void setup() {
  Serial.begin(9600);
  pinMode(trig, OUTPUT);
  pinMode(trig2, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(echo2, INPUT);

  // Setup motors

  pinMode(RenaPin, OUTPUT);
  pinMode(Rin1Pin, OUTPUT);
  pinMode(Rin2Pin, OUTPUT);
  pinMode(Rin3Pin, OUTPUT);
  pinMode(Rin4Pin, OUTPUT);
  pinMode(RenbPin, OUTPUT);

  pinMode(FenaPin, OUTPUT);
  pinMode(Fin1Pin, OUTPUT);
  pinMode(Fin2Pin, OUTPUT);
  pinMode(Fin3Pin, OUTPUT);
  pinMode(Fin4Pin, OUTPUT);
  pinMode(FenbPin, OUTPUT);

}

void loop() {
  if (Serial.available() > 0) {
    command = Serial.read();
    motor_stop();   //initialize with motors stopped
    //Change pin mode only if new command is different from previous.
    switch (command) {
      case 'F':
        //Checks for obstacle since forward command is received.
        distFront = readDistanceF();
        if (distFront < distLimit)
        {
          //Override the controller since obstacle is detected.
          override();
          distFront = readDistanceF();
          Serial.print("A");
          Serial.print(";");
          Serial.print(distFront); //send distance to MIT App
          Serial.print(";");
          Serial.print(0, DEC); //2nd value...dummy variable
          Serial.println(";");
        }
        else
        {
          //No obstacle----> move forward.
          drive_forward();
        }
        break;
      case 'B':
        distBack = readDistanceB();
        if (distBack < distLimit)
        {
          motor_stop();
          delay(500);
          drive_forward();
          delay(300);
          motor_stop();
          distBack = readDistanceB();
          Serial.print("R");
          Serial.print(";");
          Serial.print(distBack); //send distance to MIT App
          Serial.print(";");
          Serial.print(0, DEC); //2nd value...dummy variable
          Serial.println(";");
        }
        else
          drive_backward();
        break;
      case 'L':
        turn_left();
        break;
      case 'R':
        turn_right();
        break;
      case 'O' :
        drive_open();
        break;
      case 'C'  :
        drive_close();
        break;

      case 'T':     //reads distance
        {
          distFront = readDistanceF();
          Serial.print("A");
          Serial.print(";");
          Serial.print(distFront); //send distance to MIT App
          Serial.print(";");
          Serial.print(0, DEC); //2nd value...dummy variable
          Serial.println(";");
        }
        break;
    }
    //Read battery voltage every 10sec.
    currentMillis = millis();
    if (currentMillis - (previousMillis) > (interval)) {
      previousMillis = currentMillis;
      voltage1 = (analogRead(A4) * 5.0 / 1024.0) * 2.0;
      perVolt1 = (voltage1 * 100) / maxBattery1;
      if      (perVolt1 <= 75)                {
        level1 = 0;
      }
      else if (perVolt1 > 75 && perVolt1 <= 80) {
        level1 = 1;
      }
      else if (perVolt1 > 80 && perVolt1 <= 85) {
        level1 = 2;
      }
      else if (perVolt1 > 85 && perVolt1 <= 90) {
        level1 = 3;
      }
      else if (perVolt1 > 90 && perVolt1 <= 95) {
        level1 = 4;
      }
      else if (perVolt1 > 95)                 {
        level1 = 5;
      }

      voltage2 = (analogRead(A6) * 5.0 / 1024.0) * 2.0; // 6.0V
      perVolt2 = (voltage2 * 100) / maxBattery2;
      if      (perVolt2 <= 75)                {
        level2 = 0;
      }
      else if (perVolt2 > 75 && perVolt2 <= 80) {
        level2 = 1;
      }
      else if (perVolt2 > 80 && perVolt2 <= 85) {
        level2 = 2;
      }
      else if (perVolt2 > 85 && perVolt2 <= 90) {
        level2 = 3;
      }
      else if (perVolt2 > 90 && perVolt2 <= 95) {
        level2 = 4;
      }
      else if (perVolt2 > 95)                 {
        level2 = 5;
      }

      Serial.print("B");
      Serial.print(";");
      Serial.print(level1);
      Serial.print(";");
      Serial.print(level2);
      Serial.println(";");
    }
  }//serial avail ends here..

  //Checks for obstacle since forward command is received.
  distFront = readDistanceF();
  if (distFront < distLimit)
  {
    //Override the controller since obstacle is detected.
    override();
    distFront = readDistanceF();
    Serial.print("A");
    Serial.print(";");
    Serial.print(distFront); //send distance to MIT App
    Serial.print(";");
    Serial.print(0, DEC); //2nd value...dummy variable
    Serial.println(";");
  }
  //check backwards..
  distBack = readDistanceB();
  if (distBack < distLimit)
  {
    motor_stop();
    delay(500);
    drive_forward();
    delay(300);
    motor_stop();
    distBack = readDistanceB();
    Serial.print("R");
    Serial.print(";");
    Serial.print(distBack); //send distance to MIT App
    Serial.print(";");
    Serial.print(0, DEC); //2nd value...dummy variable
    Serial.println(";");
  }




}

// --------------------------------------------------------------------------- Drive

void motor_stop()
{
  analogWrite(FenaPin, 0);
  analogWrite(FenbPin, 0);
  analogWrite(RenaPin, 0);
  analogWrite(RenbPin, 0);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, LOW);
  delay(25);
}

void drive_forward()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);

}

void drive_backward()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);

}

void turn_left()
{
  analogWrite(FenaPin, 180);
  analogWrite(FenbPin, 180);
  analogWrite(RenaPin, 120);
  analogWrite(RenbPin, 120);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);

}

void turn_right()
{
  analogWrite(FenaPin, 180);
  analogWrite(FenbPin, 180);
  analogWrite(RenaPin, 120);
  analogWrite(RenbPin, 120);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);
}
void drive_open()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);
}
void drive_close()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);
}


//Returns distance value from ultrasonic sensor
int readDistanceF()
{
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  pulsetime = pulseIn(echo, HIGH);
  return pulsetime / 58.2; //magic
}
int readDistanceB()
{
  digitalWrite(trig2, LOW);
  delayMicroseconds(2);
  digitalWrite(trig2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig2, LOW);
  pulsetime = pulseIn(echo2, HIGH);
  return pulsetime / 58.2; //magic
}



void override()
{
  /*Note: The time it takes for the rover to turn depends in the
    motors you use...so that will be fixed during debugging.*/

  //Stop receiving data from the user.
  Serial.end();
  delay(10);

  //Obstacle avoidance maneuver and all that jazz..
  motor_stop();
  delay(500);
  drive_backward();
  delay(300);
  motor_stop();
  delay(10);
  scanLeft();
  delay(100);
  scanRight();
  if (distLeft > distRight && distLeft > distLimit)
  {
    turn_left();
    delay(500);
    motor_stop();
  }
  else if (distRight > distLeft && distRight > distLimit)
  {
    turn_right();
    delay(500);
    motor_stop();
  }
  else
  {
    drive_backward();
    delay(300);
    motor_stop();
  }

  //Restart the receiver.
  Serial.begin(9600);
}

void scanLeft()
{
  Serial.println("Scanning Left....");
  delay(10);
  turn_left();  //turns left to scan
  delay(300);
  motor_stop();
  delay(10);
  distLeft = readDistanceF();
  //Serial.print("Left is ");
  //Serial.println(distLeft); //scan
  delay(100);
  turn_right();   //turns to the centre again
  delay(300);
  motor_stop();
}

void scanRight()
{
  Serial.println("Scanning Right....");
  delay(10);
  turn_right();
  delay(300);
  motor_stop();
  delay(10);
  distRight = readDistanceF();
  //Serial.print("Right is ");
  //Serial.println(distRight);
  delay(100);
  turn_left();
  delay(300);
  motor_stop();
}

have fun.. ~q

akisv · January 24, 2024, 12:56am

Thank you so much for taking the time to do this, I will test it and let you know !!

akisv · January 24, 2024, 1:41am

Unfortunately it doesnt work. It doesnt even take commands and just works if i put my hand near one of the sensors and just dodges it by it self..

qubits-us · January 24, 2024, 1:46am

really?? stinkers..
obviously you have it up on blocks, so the wheels just spin and you're using your hands to make sure it stops before actually setting it free..
try not using the override() and just do a motot_stop() instead..
and while it's up on the blocks, test the sensors make sure they respond properly and you don't have something backwards..
~q

akisv · January 24, 2024, 1:56am

Okay i will try that. thanks for taking the time to respond !!

akisv · January 24, 2024, 1:57am

Its for my graduation project and i am seriously burnt out on trying to make it work. I've tried so many alternatives and thats the best i've wrote and it's still not fully functional...I'll keep working on it i guess.

qubits-us · January 24, 2024, 2:05am

That's cool, took me a while to debug mine too, no worries..
Sometimes a break helps too..
You know, don't quite follow your sensor code..
complete-guide-for-ultrasonic-sensor-hc-sr04
test each component separately..

good luck.. ~q

xfpd · January 24, 2024, 2:11am

Yes, that is true... but that was not the point. The comparison you did use the same global (PulseTime)... which is changed by both sensors... and the division looks like it would be zero most of the time... as you described.

akisv · January 24, 2024, 4:05pm

So how could i fix this, do you have any suggestions ? Thanks in advance!!!

qubits-us · January 24, 2024, 5:04pm

not so sure..
simmed the sensors, seem to be working..
Mega 2 Echos
try running just the sensors and verify their operating properly..
~q

xfpd · January 24, 2024, 6:29pm

I put your code into a simulator...

When the REAR sensor detects an object within 20cm, the motor sequence is:

RIGHT
STOP
RIGHT
STOP

Rdist

When the FRONT sensor detects and object within 20cm, the motor sequence is:

LEFT
REVERSE
FORWARD
FORWARD
REVERSE
LEFT

Fdist

Files from WOKWI.COM

sketch.ino

int RenaPin = 13;
int Rin1Pin = 22;
int Rin2Pin = 24;
int Rin3Pin = 26;
int Rin4Pin = 28;
int RenbPin = 12;

int FenaPin = 11;
int Fin1Pin = 30;
int Fin2Pin = 32;
int Fin3Pin = 34;
int Fin4Pin = 36;
int FenbPin = 10;

char command;

//Initializing all variables

//Variables where distances measured are stored.
int distFront;
int distLeft;
int distRight;
int distBack;
//Minimum distance of 20cm from any obstacle.
int distLimit = 20;

//HCSR04 Variables
int trig = 6;
int trig2 = 2;
int echo =  7;
int echo2 = 3;
unsigned long pulsetime = 0;

//Calculate Battery Level1. Arduino
const float maxBattery1 = 8.0;
int perVolt1;                 // Percentage variable
float voltage1 = 0.0;         // Read battery voltage
int level1;
//Calculate Battery Level2. Motor
const float maxBattery2 = 6.0;
int perVolt2;                 // Percentage variable
float voltage2 = 0.0;         // Read battery voltage
int level2;


long previousMillis = -1000 * 10;
long interval = 1000 * 10;
unsigned long currentMillis;

// --------------------------------------------------------------------------- Setup
void setup() {
  Serial.begin(9600);
  pinMode(trig, OUTPUT);
  pinMode(trig2, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(echo2, INPUT);

  // Setup motors

  pinMode(RenaPin, OUTPUT);
  pinMode(Rin1Pin, OUTPUT);
  pinMode(Rin2Pin, OUTPUT);
  pinMode(Rin3Pin, OUTPUT);
  pinMode(Rin4Pin, OUTPUT);
  pinMode(RenbPin, OUTPUT);

  pinMode(FenaPin, OUTPUT);
  pinMode(Fin1Pin, OUTPUT);
  pinMode(Fin2Pin, OUTPUT);
  pinMode(Fin3Pin, OUTPUT);
  pinMode(Fin4Pin, OUTPUT);
  pinMode(FenbPin, OUTPUT);

}

void loop() {
  if (Serial.available() > 0) {
    command = Serial.read();
    motor_stop();   //initialize with motors stopped
    //Change pin mode only if new command is different from previous.
    switch (command) {
      case 'F':
        //Checks for obstacle since forward command is received.
        distFront = readDistanceF();
        if (distFront < distLimit)
        {
          //Override the controller since obstacle is detected.
          override();
          distFront = readDistanceF();
          Serial.print("A");
          Serial.print(";");
          Serial.print(distFront); //send distance to MIT App
          Serial.print(";");
          Serial.print(0, DEC); //2nd value...dummy variable
          Serial.println(";");
        }
        else
        {
          //No obstacle----> move forward.
          drive_forward();
        }
        break;
      case 'B':
        distBack = readDistanceB();
        if (distBack < distLimit)
        {
          motor_stop();
          delay(500);
          drive_forward();
          delay(300);
          motor_stop();
          distBack = readDistanceB();
          Serial.print("R");
          Serial.print(";");
          Serial.print(distBack); //send distance to MIT App
          Serial.print(";");
          Serial.print(0, DEC); //2nd value...dummy variable
          Serial.println(";");
        }
        else
          drive_backward();
        break;
      case 'L':
        turn_left();
        break;
      case 'R':
        turn_right();
        break;
      case 'O' :
        drive_open();
        break;
      case 'C'  :
        drive_close();
        break;

      case 'T':     //reads distance
        {
          distFront = readDistanceF();
          Serial.print("A");
          Serial.print(";");
          Serial.print(distFront); //send distance to MIT App
          Serial.print(";");
          Serial.print(0, DEC); //2nd value...dummy variable
          Serial.println(";");
        }
        break;
    }
    //Read battery voltage every 10sec.
    currentMillis = millis();
    if (currentMillis - (previousMillis) > (interval)) {
      previousMillis = currentMillis;
      voltage1 = (analogRead(A4) * 5.0 / 1024.0) * 2.0;
      perVolt1 = (voltage1 * 100) / maxBattery1;
      if      (perVolt1 <= 75)                {
        level1 = 0;
      }
      else if (perVolt1 > 75 && perVolt1 <= 80) {
        level1 = 1;
      }
      else if (perVolt1 > 80 && perVolt1 <= 85) {
        level1 = 2;
      }
      else if (perVolt1 > 85 && perVolt1 <= 90) {
        level1 = 3;
      }
      else if (perVolt1 > 90 && perVolt1 <= 95) {
        level1 = 4;
      }
      else if (perVolt1 > 95)                 {
        level1 = 5;
      }

      voltage2 = (analogRead(A6) * 5.0 / 1024.0) * 2.0; // 6.0V
      perVolt2 = (voltage2 * 100) / maxBattery2;
      if      (perVolt2 <= 75)                {
        level2 = 0;
      }
      else if (perVolt2 > 75 && perVolt2 <= 80) {
        level2 = 1;
      }
      else if (perVolt2 > 80 && perVolt2 <= 85) {
        level2 = 2;
      }
      else if (perVolt2 > 85 && perVolt2 <= 90) {
        level2 = 3;
      }
      else if (perVolt2 > 90 && perVolt2 <= 95) {
        level2 = 4;
      }
      else if (perVolt2 > 95)                 {
        level2 = 5;
      }

      Serial.print("B");
      Serial.print(";");
      Serial.print(level1);
      Serial.print(";");
      Serial.print(level2);
      Serial.println(";");
    }
  }//serial avail ends here..

  //Checks for obstacle since forward command is received.
  distFront = readDistanceF();
  if (distFront < distLimit)
  {
    //Override the controller since obstacle is detected.
    override();
    distFront = readDistanceF();
    Serial.print("A");
    Serial.print(";");
    Serial.print(distFront); //send distance to MIT App
    Serial.print(";");
    Serial.print(0, DEC); //2nd value...dummy variable
    Serial.println(";");
  }
  //check backwards..
  distBack = readDistanceB();
  if (distBack < distLimit)
  {
    motor_stop();
    delay(500);
    drive_forward();
    delay(300);
    motor_stop();
    distBack = readDistanceB();
    Serial.print("R");
    Serial.print(";");
    Serial.print(distBack); //send distance to MIT App
    Serial.print(";");
    Serial.print(0, DEC); //2nd value...dummy variable
    Serial.println(";");
  }




}

// --------------------------------------------------------------------------- Drive

void motor_stop()
{
  analogWrite(FenaPin, 0);
  analogWrite(FenbPin, 0);
  analogWrite(RenaPin, 0);
  analogWrite(RenbPin, 0);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, LOW);
  delay(25);
}

void drive_forward()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);

}

void drive_backward()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);

}

void turn_left()
{
  analogWrite(FenaPin, 180);
  analogWrite(FenbPin, 180);
  analogWrite(RenaPin, 120);
  analogWrite(RenbPin, 120);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);

}

void turn_right()
{
  analogWrite(FenaPin, 180);
  analogWrite(FenbPin, 180);
  analogWrite(RenaPin, 120);
  analogWrite(RenbPin, 120);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);
}
void drive_open()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, LOW);
  digitalWrite(Rin2Pin, HIGH);
  digitalWrite(Rin3Pin, HIGH);
  digitalWrite(Rin4Pin, LOW);

  digitalWrite(Fin1Pin, HIGH);
  digitalWrite(Fin2Pin, LOW);
  digitalWrite(Fin3Pin, LOW);
  digitalWrite(Fin4Pin, HIGH);
}
void drive_close()
{
  analogWrite(FenaPin, 255);
  analogWrite(FenbPin, 255);
  analogWrite(RenaPin, 255);
  analogWrite(RenbPin, 255);
  digitalWrite(Rin1Pin, HIGH);
  digitalWrite(Rin2Pin, LOW);
  digitalWrite(Rin3Pin, LOW);
  digitalWrite(Rin4Pin, HIGH);

  digitalWrite(Fin1Pin, LOW);
  digitalWrite(Fin2Pin, HIGH);
  digitalWrite(Fin3Pin, HIGH);
  digitalWrite(Fin4Pin, LOW);
}


//Returns distance value from ultrasonic sensor
int readDistanceF()
{
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  pulsetime = pulseIn(echo, HIGH);
  return pulsetime / 58.2; //magic
}
int readDistanceB()
{
  digitalWrite(trig2, LOW);
  delayMicroseconds(2);
  digitalWrite(trig2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig2, LOW);
  pulsetime = pulseIn(echo2, HIGH);
  return pulsetime / 58.2; //magic
}



void override()
{
  /*Note: The time it takes for the rover to turn depends in the
    motors you use...so that will be fixed during debugging.*/

  //Stop receiving data from the user.
  Serial.end();
  delay(10);

  //Obstacle avoidance maneuver and all that jazz..
  motor_stop();
  delay(500);
  drive_backward();
  delay(300);
  motor_stop();
  delay(10);
  scanLeft();
  delay(100);
  scanRight();
  if (distLeft > distRight && distLeft > distLimit)
  {
    turn_left();
    delay(500);
    motor_stop();
  }
  else if (distRight > distLeft && distRight > distLimit)
  {
    turn_right();
    delay(500);
    motor_stop();
  }
  else
  {
    drive_backward();
    delay(300);
    motor_stop();
  }

  //Restart the receiver.
  Serial.begin(9600);
}

void scanLeft()
{
  Serial.println("Scanning Left....");
  delay(10);
  turn_left();  //turns left to scan
  delay(300);
  motor_stop();
  delay(10);
  distLeft = readDistanceF();
  //Serial.print("Left is ");
  //Serial.println(distLeft); //scan
  delay(100);
  turn_right();   //turns to the centre again
  delay(300);
  motor_stop();
}

void scanRight()
{
  Serial.println("Scanning Right....");
  delay(10);
  turn_right();
  delay(300);
  motor_stop();
  delay(10);
  distRight = readDistanceF();
  //Serial.print("Right is ");
  //Serial.println(distRight);
  delay(100);
  turn_left();
  delay(300);
  motor_stop();
}

diagram.json

{
  "version": 1,
  "author": "Anonymous maker",
  "editor": "wokwi",
  "parts": [
    { "type": "wokwi-arduino-mega", "id": "mega", "top": 58.2, "left": -406.8, "attrs": {} },
    {
      "type": "wokwi-led",
      "id": "led1",
      "top": -58.8,
      "left": -65.8,
      "rotate": 270,
      "attrs": { "color": "red", "flip": "1" }
    },
    {
      "type": "wokwi-led",
      "id": "led2",
      "top": -78,
      "left": -65.8,
      "rotate": 270,
      "attrs": { "color": "red", "flip": "1" }
    },
    {
      "type": "wokwi-led",
      "id": "led3",
      "top": -97.2,
      "left": -65.8,
      "rotate": 270,
      "attrs": { "color": "red", "flip": "1" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo1",
      "top": -67.2,
      "left": -105.6,
      "attrs": { "text": "RenA" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo2",
      "top": -86.4,
      "left": -105.6,
      "attrs": { "text": "Rin1" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo3",
      "top": -48,
      "left": -105.6,
      "attrs": { "text": "Rin2" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo4",
      "top": -86.4,
      "left": 67.2,
      "attrs": { "text": "Rin3" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo5",
      "top": -48,
      "left": 67.2,
      "attrs": { "text": "Rin4" }
    },
    {
      "type": "wokwi-led",
      "id": "led4",
      "top": -58.8,
      "left": 25.8,
      "rotate": 90,
      "attrs": { "color": "red", "flip": "" }
    },
    {
      "type": "wokwi-led",
      "id": "led5",
      "top": -97.2,
      "left": 25.8,
      "rotate": 90,
      "attrs": { "color": "red", "flip": "" }
    },
    {
      "type": "wokwi-led",
      "id": "led6",
      "top": -78,
      "left": 25.8,
      "rotate": 90,
      "attrs": { "color": "red", "flip": "" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo6",
      "top": -67.2,
      "left": 67.2,
      "attrs": { "text": "RenB" }
    },
    {
      "type": "wokwi-led",
      "id": "led7",
      "top": -174,
      "left": -65.8,
      "rotate": 270,
      "attrs": { "color": "red", "flip": "1" }
    },
    {
      "type": "wokwi-led",
      "id": "led8",
      "top": -193.2,
      "left": -65.8,
      "rotate": 270,
      "attrs": { "color": "red", "flip": "1" }
    },
    {
      "type": "wokwi-led",
      "id": "led9",
      "top": -212.4,
      "left": -65.8,
      "rotate": 270,
      "attrs": { "color": "red", "flip": "1" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo7",
      "top": -182.4,
      "left": -105.6,
      "attrs": { "text": "FenA" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo8",
      "top": -201.6,
      "left": -105.6,
      "attrs": { "text": "Fin1" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo9",
      "top": -163.2,
      "left": -105.6,
      "attrs": { "text": "Fin2" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo10",
      "top": -201.6,
      "left": 67.2,
      "attrs": { "text": "Fin3" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo11",
      "top": -163.2,
      "left": 67.2,
      "attrs": { "text": "Fin4" }
    },
    {
      "type": "wokwi-led",
      "id": "led10",
      "top": -174,
      "left": 25.8,
      "rotate": 90,
      "attrs": { "color": "red", "flip": "" }
    },
    {
      "type": "wokwi-led",
      "id": "led11",
      "top": -212.4,
      "left": 25.8,
      "rotate": 90,
      "attrs": { "color": "red", "flip": "" }
    },
    {
      "type": "wokwi-led",
      "id": "led12",
      "top": -193.2,
      "left": 25.8,
      "rotate": 90,
      "attrs": { "color": "red", "flip": "" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo12",
      "top": -182.4,
      "left": 67.2,
      "attrs": { "text": "FenB" }
    },
    {
      "type": "wokwi-hc-sr04",
      "id": "ultrasonic1",
      "top": -228.9,
      "left": -311.3,
      "attrs": { "distance": "17" }
    },
    { "type": "wokwi-hc-sr04", "id": "ultrasonic2", "top": -94.5, "left": -311.3, "attrs": {} },
    {
      "type": "wokwi-text",
      "id": "legendservo13",
      "top": -201.6,
      "left": -355.2,
      "attrs": { "text": "Fdist" }
    },
    {
      "type": "wokwi-text",
      "id": "legendservo14",
      "top": -67.2,
      "left": -355.2,
      "attrs": { "text": "Rdist" }
    }
  ],
  "connections": [
    [ "mega:13", "led2:A", "green", [ "v-28.8", "h258.6", "v-57.6" ] ],
    [ "mega:22", "led3:A", "green", [ "h26.6", "v-75.7", "h0", "v-38.4" ] ],
    [ "mega:24", "led1:A", "green", [ "h26.6", "v-114.25", "h-9.6" ] ],
    [ "mega:26", "led5:A", "green", [ "v1.05", "h26.6", "v-57.6", "h38.4", "v-86.4" ] ],
    [ "mega:28", "led4:A", "green", [ "v1.15", "h26.6", "v-67.2", "h38.4", "v-38.4" ] ],
    [ "mega:12", "led6:A", "green", [ "v-28.8", "h288", "v-57.6" ] ],
    [ "mega:11", "led8:A", "green", [ "v-28.8", "h239.6", "v-134.4" ] ],
    [ "mega:30", "led9:A", "green", [ "v1", "h26.6", "v-259.2", "h-9.6" ] ],
    [ "mega:32", "led7:A", "green", [ "v1.1", "h26.6", "v-192", "h0", "v-9.6" ] ],
    [ "mega:34", "led11:A", "green", [ "v1.2", "h26.6", "v-96", "h38.4", "v-153.6" ] ],
    [ "mega:36", "led10:A", "green", [ "v1.3", "h26.6", "v-105.6", "h38.4", "v-115.2" ] ],
    [ "mega:10", "led12:A", "green", [ "v-28.8", "h268.5", "v-134.4" ] ],
    [ "mega:GND.1", "led1:C", "black", [ "v-19.2", "h297.4", "v-58" ] ],
    [ "mega:GND.1", "led2:C", "black", [ "v-19.2", "h297.4", "v-77.2" ] ],
    [ "mega:GND.1", "led3:C", "black", [ "v-19.2", "h297.4", "v-96.4" ] ],
    [ "mega:GND.1", "led7:C", "black", [ "v-19.2", "h297.4", "v-134.8" ] ],
    [ "mega:GND.1", "led8:C", "black", [ "v-19.2", "h297.4", "v-154" ] ],
    [ "mega:GND.1", "led9:C", "black", [ "v-19.2", "h297.4", "v-173.2" ] ],
    [ "mega:GND.1", "led4:C", "black", [ "v-19.2", "h297.4", "v-57.6" ] ],
    [ "mega:GND.1", "led6:C", "black", [ "v-19.2", "h297.4", "v-76.8" ] ],
    [ "mega:GND.1", "led5:C", "black", [ "v-19.2", "h297.4", "v-96" ] ],
    [ "mega:GND.1", "led10:C", "black", [ "v-19.2", "h297.4", "v-134.4" ] ],
    [ "mega:GND.1", "led12:C", "black", [ "v-19.2", "h297.4", "v-153.6" ] ],
    [ "mega:GND.1", "led11:C", "black", [ "v-19.2", "h297.4", "v-172.8" ] ],
    [ "mega:6", "ultrasonic1:TRIG", "green", [ "v-38.4", "h82", "v-144", "h-105.2" ] ],
    [ "mega:2", "ultrasonic2:TRIG", "green", [ "v-57.6", "h-61.2" ] ],
    [ "mega:7", "ultrasonic1:ECHO", "green", [ "v-38.4", "h92", "v-144", "h-95.2" ] ],
    [ "mega:3", "ultrasonic2:ECHO", "green", [ "v-57.6", "h-41.7" ] ],
    [ "mega:5V.1", "ultrasonic2:VCC", "red", [ "v-47", "h-194.2" ] ],
    [ "mega:5V.1", "ultrasonic1:VCC", "red", [ "v-47", "h-88.6", "v-124.8", "h-105.6" ] ],
    [ "mega:GND.1", "ultrasonic1:GND", "black", [ "v-19.2", "h172.6", "v-172.8", "h-94.8" ] ],
    [ "mega:GND.1", "ultrasonic2:GND", "black", [ "v-19.2", "h95.8", "v-48" ] ]
  ],
  "dependencies": {}
}

akisv · January 24, 2024, 10:04pm

Thank you so much for even taking the time to do this!!!!

xfpd · January 24, 2024, 10:19pm

Use the simulator to help with your scanning and moving (it's a free site).

Your turns could be refined. Turning RIGHT moving forward can be done in three ways:

Skid right - left motor forward, right motor off (stop/skid)
Skid left - left motor off (stop/skid), right motor reverse
Rotate right - left motor forward, right motor reverse

Same thing with turning left moving forward, left moving backward and right moving backward.

akisv · January 24, 2024, 10:33pm

I'll check it out !

xfpd · January 24, 2024, 10:38pm

Here is a simple, 2-wheel simulation, showing the steering...

Topic		Replies	Views
Car chasis with HC-SR04 sensor Programming Questions	25	6005	May 5, 2021
HC-SR04 ultrasonic sensor help plz Programming Questions	7	3473	May 5, 2021
[solved] Multiple Ultrasonic sensors and logic in programing them Programming Questions	6	987	May 5, 2021
Programming a robot with HC-SR04 ultrasonic sensor (beginner) Programming Questions	7	788	May 5, 2021
Problem with ultrasonic sensor Robotics	15	5776	May 6, 2021

I am building an arduino rc car with 2 ultra sonic sensors (HC-SR04)

Related Topics