DIY vacuum robot code - prevent from being stuck

Warning! Noob here!

I built a vacuum robot with this instructable.

I built this one with very basic tools and materials (and skills) as you can see in the video below.

So my problem is that I'm new to c++, and I am unable to modify the code in order to go backwards and turn from time to time instead of just going straight ahead. I need this function because it gets stuck a lot (due to my not so good bumper design). I know it will be less effective, but at least it won't be stuck in a chair or something.

I tried several modifications, for example adding some lines above the else if (!control) part of the sketch, but it just ignores the sensors and bumpers, unless they detect something in the end of the cycle (which is very very rare) It behaves like this with that code.

I thought I can achive what I want with a while loop, but it just doesn't want to work properly. Here's the sketch I put all my hope into. But it's useless.
EDIT: In case you cannot see the code under the link above, I post the code in a relpy, due to maximum caracter number.
Or here's an other link with the same code.

The last code I came up with has an additional counter that gives short commands, so the robot has a chance to dectect something (because the sensors can detect in the end of the cycle). Btw. it only detects the inputs of the bumpers, but not the sharp sensors . I don't paste that code here, because it is not the path I want to continue.

(In the future I want to add an accelerometer, that works as an additional sensor, and gives a turn command if it detects that the robot is stuck. I'm waiting for the part.)

So my question is: how can I make my robot to go backwards and turn from time to time instead of just going straight ahead AND while doing this, react to the inputs of senors real time.

Thank you for your help in advance!

Your link to create.arduino did not work for me. Just post your code here.

…R

PS … To make it easy for people to help you please modify your post and use the code button </> so your code looks like this and is easy to copy to a text editor.

In case you cannot see the code with the link, here’s the code:

//Code: VaccumCode
//Version: 2.0.1
//Author: Cesar Nieto refer to ces.nietor@gmail.com
//Last change: 19/05/2017
//Changes: Documentation added
#include <math.h> 
////////////PINS////////////////
//Distance Analog Sensors (Sharp)
const int SD1 = 0; //left front sensor
const int SD2 = 1; //right front sensor
const int SD3 = 2; //left side sensor
const int SD4 = 3; //right side sensor

//Battery Voltage input
const int battery = 4;

//IndicatorLED
const int led = 13;

//Fan output
const int fanmotor =  12;      // the number of the LED pin

// Motor1 Right
const int motor1Pin1 = 3;
const int motor1Pin2 = 5;

// Motor2 Left
const int motor2Pin1 = 6;
const int motor2Pin2 = 9;

//Bumper
const int bumper1 = 10;
const int bumper2 = 11;
const int bumper3 = 7;
const int bumper4 = 8;

///////////////Constants////////////////
const float voltageBatCharged = 12.68; // Voltage measured when battery fully charged //Change this
//PWM for the micro metal motors
const int pwmMax = 160;// for 12V  pwmMAx = 170, for 10V output  pwmMax = 140
const int pwmMin = 70;;// for 9V  pwmMin = 128
//Mínimun distance of the sensor
const int minSharp = 30;

// Variables will change:
int bumperState = 0;  // variable for reading the pushbutton status
boolean control = true;
int counter = 0; //   Prevents from being stuck
int bumperState2 = 0; // variable for reading the pushbutton status
boolean control2 = true;
int counter2 = 0; // Prevents from being stuck
int bumperState3 = 0; // variable for reading the pushbutton status
boolean control3 = true;
int counter3 = 0; // Prevents from being stuck
int bumperState4 = 0; // variable for reading the pushbutton status
boolean control4 = true;
int counter4 = 0; // Prevents from being stuck


//////////////CODE/////////////
void setup() {
  //Initialize outputs and inputs
  //Fan motor as output
  pinMode(fanmotor, OUTPUT);
  //Motor1
  pinMode(motor1Pin1, OUTPUT);
  pinMode(motor1Pin2, OUTPUT);
  //Motor2
  pinMode(motor2Pin1, OUTPUT);
  pinMode(motor2Pin2, OUTPUT);
  //LED
  pinMode(led, OUTPUT);
  //INPUTS
  // initialize the pushbutton inputs 
  //Bumper
  pinMode(bumper1, INPUT_PULLUP); 
  pinMode(bumper2, INPUT_PULLUP); 
  pinMode(bumper3, INPUT_PULLUP); 
  pinMode(bumper4, INPUT_PULLUP); 
  //Sensor
  pinMode(SD1, INPUT);
  pinMode(SD2, INPUT);
  pinMode(SD3, INPUT);
  pinMode(SD4, INPUT);
  //Batt
  pinMode(battery, INPUT);
  // Initialize serial
  Serial.begin(9600);    
  ///////////////////////////////Wait////////////////////////////////////////
  //Wait about 5 s and initialize fan if voltage ok
  waitBlinking(5,1); //5 seconds at 1 Hz
  //Crank (initialize the fan because the voltage drops when cranking)
  if(readBattery(battery)>12.1){
    digitalWrite(fanmotor, HIGH); //Turn the Fan ON
    delay(1000); //For 1000ms
  }
  else {
    //do nothing Convention
    }
} 
//////////Functions To Use //////////
void waitBlinking(int n, int frequency){
  //blink for n seconds at frequency hz
  for (int i=1; i <= n; i++){
    for(int j=1; j<=frequency; j++){
      digitalWrite(led, HIGH);   
      delay((1000/frequency)/2);   //Half time on            
      digitalWrite(led, LOW);   
      delay((1000/frequency)/2);   //Half time off
    }
   } 
}
double sdSHARP(int Sensor){
  //Returns the distance in cm
  double dist = pow(analogRead(Sensor), -0.857); // x to power of y
  return (dist * 1167.9);
}
void forwardMotors(int moveTime){  
  //Manipulate direction according the desired movement of the motors
   analogWrite(motor1Pin1, pwmMin); 
   analogWrite(motor1Pin2, 0); //PWM value wher 0 = 0% and 255 = 100%
   analogWrite(motor2Pin1, pwmMin); 
   analogWrite(motor2Pin2, 0); 
   delay(moveTime);
}
void rightMotors(int moveTime){ 
   analogWrite(motor1Pin1, 0); 
   analogWrite(motor1Pin2, pwmMin); 
   analogWrite(motor2Pin1, pwmMin);
   analogWrite(motor2Pin2, 0); 

   delay(moveTime);
}
void leftMotors(int moveTime){ 
   analogWrite(motor1Pin1, pwmMin); 
   analogWrite(motor1Pin2, 0); 
   analogWrite(motor2Pin1, 0);
   analogWrite(motor2Pin2, pwmMin+20); 
   delay(moveTime);
}
void backwardMotors(int moveTime){
   analogWrite(motor1Pin1, 0); 
   analogWrite(motor1Pin2, pwmMin+20);
   analogWrite(motor2Pin1, 0); 
   analogWrite(motor2Pin2, pwmMin+20); 
   delay(moveTime);
}
void stopMotors(){ 
   analogWrite(motor1Pin1, 0);
   analogWrite(motor1Pin2, 0); 
   analogWrite(motor2Pin1, 0); 
   analogWrite(motor2Pin2, 0); 
}
float  readBattery(int input){
  int readInput;
  float voltage;
  readInput = analogRead(input);
  voltage = (((readInput*4.9)/1000)*voltageBatCharged ) / 5; // resolution of analog input = 4.9mV per Voltage 
  Serial.print(" Battery= ");
  Serial.print(voltage);
  return voltage;
  } 
void batteryControl(int input){
  //Turn everything off in case the battery is low
  float v_battery;
  v_battery = readBattery(input);
  if(v_battery<=11.6){ //battery limit of discharge, Don't put this limit lower than  11.1V or you can kill the battery
    control = false;
    }
  else {
    //Do nothing Convention
    }
}
/////////////////////////////////////////////////MAIN CODE//////////////////////////////
void loop(){
  /*  
  Serial.print("SD1= ");
  Serial.print(sdSHARP(SD1));
  Serial.println();
  Serial.print("  SD2= ");
  Serial.print(sdSHARP(SD2));
  Serial.println();
  delay(200);*/
  bumperState = digitalRead(bumper1);
  bumperState2 = digitalRead(bumper2);
  bumperState3 = digitalRead(bumper3);
  bumperState4 = digitalRead(bumper4);
  //Keep the control of the battery automatically turn the fan off
  //If control = true the battery level is ok, otherwise the battery is low.
  batteryControl(battery); //modifies the variable control of the battery is low
  
  if (control){
    while (!sdSHARP(SD1)<=4.3 && !sdSHARP(SD2)<=4.3 && !bumperState==0 && !bumperState2==0 && !bumperState3==0 && !bumperState4==0);               {
      forwardMotors(5100); 
      backwardMotors(300);
      leftMotors(300);
      forwardMotors (4800);
      backwardMotors(400);
      rightMotors(350);
      forwardMotors(5000); 
      backwardMotors(280);
      rightMotors(320);
      forwardMotors (5200);
      backwardMotors(410);
      rightMotors(350);
      forwardMotors(4500); 
      backwardMotors(310);
      leftMotors(320);
      forwardMotors (4800);
      backwardMotors(400);
      rightMotors(350);
      forwardMotors (300);
    }
      
     
      
    
    digitalWrite(led, HIGH);
    
       if (sdSHARP(SD1)<=4.3 ){ 
      //If the distance between an object and the left front sensor is less than 4.3 cm or the bumper hits, it will move to the left
      if (counter ==2){ // prevent of being stuck on corners
        counter = 0;
        }
      else {
        //Do nothing Convention
    }
    
      forwardMotors(100); // approach a bit
      backwardMotors(500); // backward delay of 500ms
      leftMotors(300);
      counter = counter + 2;
      Serial.print("  Turn Left ");
      }
    else if (sdSHARP(SD2)<=4.3){ 
      //If the distance between an object and the right front sensor is less than 4.3 cm, it will move to the right
      if (counter ==1){
        counter = 0;
        }
      else{
        //Do nothing Convention
      }
      forwardMotors(100);
      backwardMotors(500);
      rightMotors(300);
      counter++;
      Serial.print("  Turn Right");
      }
    else if (bumperState==0){//BUMPER1
      counter = 0;
      backwardMotors(1500); //backward delay of 500ms
      leftMotors(500);
      Serial.print("  Turn Left ");
      }
    else if (bumperState2==0){//BUMPER2
      counter2 = 0;
      backwardMotors(1500); //backward delay of 500ms
      rightMotors(400);
      Serial.print("  Turn Right ");
      }
      
    else if (bumperState3==0){//BUMPER3
      counter3 = 0;
      backwardMotors(1500); //backward delay of 500ms
      leftMotors(500);
      Serial.print("  Turn Left ");
      }
    else if (bumperState4==0){//BUMPER4
      counter = 4;
      backwardMotors(1500); //backward delay of 500ms
      rightMotors(400);
      Serial.print("  Turn Right ");
      }
    else {
      if(counter==3){ //Corner
        leftMotors(1000);
        counter = 0;
        }
      else {
        forwardMotors(0);
        
      }
      Serial.print("  Move Forward");
      }
         
  }
  else if (!control){
    //If the battery is low, turn everything off
    digitalWrite(fanmotor, LOW); //Turn the Fan OFF
    stopMotors();
    Serial.print(" Low Battery! ");
    Serial.println();
    waitBlinking(1,3);  //blink as warning 3hz in a loop
    }
  Serial.println();
  }

If you want a responsive program then the first thing you need to do is remove ALL the calls to delay() as the Arduino can do nothing during a delay(). Unfortunately your program has a lot of them so there is a lot of work ahead of you.

Have a look at how millis() is used to manage timing without blocking in Several things at a time.

Actually, with a little thought and re-organization you should be able to do all the timing in one place - there is no need for each function to have its own timing code. It may even help with the transition from your existing code to the use of millis() if you first replace all the delay()s with a single delay() in loop(). Then you could more easily replace that single delay() with millis(). Note that when I say single delay() I do not mean that every delay() should be the same length.

...R

Thank you very much, I'll do some research about what you said, and try to solve the problem. I'll come back with updates.
Until then I'd be thankful for any other options as well.

Hello again!
I have to admit that I haven't done my research for what you suggested, but my accelerometer arrived and somehow it seems to be working now.
The accelerometer I am using is called 6DOF MPU-6050 3 Axis Gyro With Accelerometer Sensor Module For Arduino.
I used a code from an other project to integrate the accelerometer. That project is for a head mouse that clicks if the cursor stays inside a 1010 pixel square for 2 seconds. I raised this value to 2020 pixels, but maybe it was not a good idea, since I get a good amount of false positive readings from the accelerometer, while the robot is moving straight ahead. I replaced the click to an order to a desired movement.
Here’s that project (also awsome)
Here's the code I am currently using. It still needs some adjustments because of the false positive readings from the accelerometer, and for a yet unknown reason my left side bumper does not work (might be even contact issues because I don’t see anything in the serial terminal from that one.

The new part of the code is at the bottom.