I need help with my final year project

Question

I am building a project using bluetooth control as well as an ir sensor. The logic behind my program is when I press a key I want my robot to execute a series of program and while the program is executing if an obstacle is detected I need the motors to stop and then continue from the same point after the obstacle is removed. I am uploading my code for ref.

#include <Servo.h>
#include <AFMotor.h>
#include <Servo.h>
int obstaclePin = 12;
int obstacle = LOW;
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
Servo myservo1;
Servo myservo2; 
int pos = 0;

char bt='S';
void setup()
{
  Serial.begin(9600);
  pinMode(obstaclePin,INPUT);
  myservo1.attach(10);
  myservo2.attach(9);
  motor1.setSpeed(100);
  motor2.setSpeed(100);
  motor3.setSpeed(100);
  motor4.setSpeed(100);
  Stop();
}


void loop() {
 bt=Serial.read();
  if(bt=='F')
{
  forward();
  delay(2000);
  backward();
  delay(2000);
  left();
  delay(2000);
  right();
  delay(2000);
  Stop();
}

if(bt=='B')
{
forward();
 delay(8000);
 left();
 delay(1500);
 ServoControl();
 delay(500);
 backward();
 delay(1000);
 right();
 delay(3000);
 forward();
 delay(8000);
 Stop();
}

if(bt=='L')
{
 forward();
 delay(10000);
 left();
 delay(1500);
 ServoControl();
 delay(500);
 backward();
 delay(1000);
 right();
 delay(3000);
 forward();
 delay(10000);
 Stop();
}

if(bt=='R')
{
 forward();
 delay(12000);
 left();
 delay(1500);
 ServoControl();
 delay(500);
 backward();
 delay(1000);
 right();
 delay(3000);
 forward();
 delay(12000);
 Stop();
}

if(bt=='S')
{
 Stop(); 
}
if(bt=='O')
{
  servoOpen();
}
if(bt=='C')
{
  servoCLose();

}

}
void forward()
{
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
}

void backward()
{
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);
}
void left()
{
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(RELEASE);
  motor4.run(RELEASE);
}
void right()
{
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
}
void Stop()
{
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  motor3.run(RELEASE);
  motor4.run(RELEASE);
}
void servoOpen()
{
  for (pos = 0; pos <= 100; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo1.write(pos);
    myservo2.write(pos);             
  }
}


void servoCLose()
{
  
  for (pos = 100; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo1.write(pos);
    myservo2.write(pos);
  }
}
void ServoControl()
{
  for (pos = 0; pos <= 100; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo1.write(pos);
    myservo2.write(pos);
    delay(40);                       // waits 15ms for the servo to reach the position
  }
  for (pos = 100; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo1.write(pos);
    myservo2.write(pos);            
    delay(40);                       // waits 15ms for the servo to reach the position
  }
}
void obstacleD()
{
  obstacle = digitalRead(obstaclePin);
   if(obstacle == HIGH)
  {
    Stop();
  }
}

I don't know where to add the obstacle detection so it would work like an interrupt. the main problem I was experiencing was that whenever I tried running the code on my Arduino it would completely change the logic and noting would work even if I put the command in the serial monitor until an obstacle was detected. I'm a newbie at programming and using Arduino.

Answer 1

Your main problem is, that you are using many blocking calls, especially delay(). During that the Arduino cannot do anything else. You need to use a non-blocking coding style, doing multiple things one after another in fast succession. Here the style from the BlinkWithoutDelay example using millis() is very useful. There are lots of tutorials about that online (on this site or many others), so I will not go into detail, how that works.

---

Quick fix: If you just need a quick fix and the code structure of your project is not relevant for your grade, you can do the following:

Replace the obstacleD() function with one, that will also wait for a specific time and also check the IR sensor during that time. Something like this:

void obstacleD(unsigned long wait, void (*function)(void)){
    unsigned long start_time = millis();
    unsigned long already_waited = 0;
    while(millis()-start_time < wait-already_waited){
        if(digitalRead(obstaclePin)){
            already_waited += millis()-start_time;
            Stop();
            while(digitalRead(obstaclePin));
            start_time = millis();
            function();
        }
    }
}

Then in the rest of your code instead of delay(1000) (or similar) you are calling

obstacleD(1000, forward);

The first parameter is the waiting time, the second one is the function, that was lastly called (since we need to recall it, if an obstacle is found and then removed). We are noting the start time of the function into start_time. The first while loop will loop, until we waited as long as provided (difference between current time and start time will then be greater than the wait time minus the time we already waited). In that loop we are checking the IR sensor pin. If it reads HIGH, we add the elapsed time to our already_waited variable, stop and then loop while the IR sensor pin stays high. When the pin goes LOW again we leave this inner while loop, update to our new start_time and again execute the function to start the motors again. The code should continue the path like without an obstacle, even if multiple time an obstacle is placed during one path element.

Note, that the above code is not tested in any way. You might need to make minor changes.

---

Interrupt quick fix: You could implement the stopping on obstacle with an actual interrupt. Something like this:

void obstacle_detected(){
    while(digitalRead(obstaclePin));
}

void setup(){
    ...
    attachInterrupt(obstaclePin, obstacle_detected, RISING);
}

Though this is also not a good solution, since you totally block any other code, until the obstacle is removed, including receiving serial data (one byte is received by the hardware, but that has to be read by an interrupt. If another byte is send before that, the original byte is lost). Normally you would only set a flag in the ISR (interrupt service routine), but your main code isn't ready for using that flag correctly.

---

The good-structured-code way: To get the desired behavior with a good structured code, you need to completely restructure/rewrite your current code. As mentioned above you need to create a code, that does not block for long times, but only does things, if they need to be done at that time, and otherwise execute other code. You need to learn the usage of millis().

Also your code would greatly benefit from a Finite State Machine (FSM), as Majenko already wrote in a comment. Here you first draw a diagram with your complete behavior structure, divided up into states, that do exactly one thing. The states are connected by state transitions (arrows from one state to another) (and we can even repeat a state by drawing an arrow from a state back to itself). Then you can convert that into a code structure using a state variable (integer or enum) and a switch statement, where each case has the code for one state. You can look at my answer to this question for more information. Or you can search for the term "finite state machine" or "FSM" on this site or google (for google you maybe should add "arduino" to the search query to get relevant results).

All in all you would create a code, that handles the wanted behavior without any hardware interrupts (not really needed here) and which is easily extendable, without getting problems with the program flow.

---

Note:

The for loop in servoCLose() is totally unnecessary. You don't have any delay to slow it down, so it will go to the end position in way less time, than the motor needs to even start moving. Just use Servo.write() one single time there with the desired angle. That is enough, unless you want it to move slowly. Then you would need a way to delay between the writes (either delay() or with millis(), depending on your then overall approach).