Arduino: void loop() gives unexpected result

Question

I am developing a simple robot (with 4 wheeled base) which moves forward and detects an object through distance sensor, picks it with an arm. Object is a box with some sort of handle from upward direction, hence arm is placed below the, so called, handle of object and lifts it upward. The task I want to achieve that the robot should move forward and place the object in a container (it is also a physical object) placed at some specified location.

The robot moves with 4 DC motors present in wheels of its base and arm is controlled by a separate DC motor.

What I want is that:

The robot should move forward until it detects the object.
When object is detected then it should stop the the wheels and start the arm to lift the object upward.
Then it should move forward until it detects the second object i.e. the container.
When second object (container) is detected then the wheels should be stopped and arm should be activated to put the object in the container.

For this I have written the following code

#define mp1 3
#define mp2 4
#define m2p1 5
#define m2p2 6

#define echoPin 7 // Echo Pin
#define trigPin 8 // Trigger Pin
#define LEDPin 13 // Onboard LED

#define armPin1 9  // Pin 1 of arm
#define armPin2 10  // Pin 2 of arm

int maximumRange = 200; // Maximum range needed
int minimumRange = 18; // Minimum range needed
long duration, distance; // Duration used to calculate distance

int first = 0;

void setup()
{
  Serial.begin(9600);

 //Setting the pins of motors
 pinMode(mp1, OUTPUT);
 pinMode(mp2, OUTPUT);
 pinMode(m2p1, OUTPUT);
 pinMode(m2p2, OUTPUT);

 //Setting the pins of distance sensor
 pinMode(trigPin, OUTPUT);
 pinMode(echoPin, INPUT);

 pinMode(LEDPin, OUTPUT); // Use LED indicator (if required)

 pinMode(armPin1, OUTPUT);
 pinMode(armPin2, OUTPUT);
}// end setup method

long calculateDistance(){
  //Code of distance sensor
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2); 

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(20); 

  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);

 //Calculate the distance (in cm) based on the speed of sound.
 return duration/58.2;
}

void loop()
{
  distance = calculateDistance();

  while(distance > minimumRange) {
    forward();
    distance = calculateDistance();
  }

  while(distance <= minimumRange && first == 0){
    stopMotors();

    pickObject();

    distance = calculateDistance();
  }

  while(distance > minimumRange) {
    forward();
    distance = calculateDistance();
  }

  while(distance <= minimumRange && first == 1){
    stopMotors();

    putObject();

    distance = calculateDistance();
  }// end second while copied 

  first = 1;

}// end loop function

void pickObject(){
  digitalWrite(armPin1, LOW);
  digitalWrite(armPin2, HIGH);
}

void putObject(){
  digitalWrite(armPin1, HIGH);
  digitalWrite(armPin2, LOW);
}

void stopMotors(){
  digitalWrite(mp1, LOW);
  digitalWrite(mp2, LOW);
  digitalWrite(m2p1, LOW);
  digitalWrite(m2p2, LOW);
}

void forward(){
  digitalWrite(mp1, LOW);
  digitalWrite(mp2, HIGH);
  digitalWrite(m2p1, HIGH);
  digitalWrite(m2p2, LOW);
}

But the code does not work as I want it to do. It actually moves the arm in downward direction. It may be because I have not understood the flow of loop() function.

Can anyone tell what is the problem in my code and what should be my code to get the desired result?

Answer 1

My original answer agreed with the one by @Bmoore in its diagnosis, but I now believe that your algorithm in loop() is essentially correct. Here's what happens:

1. We're in the first iteration, first==0. You enter the while(distance > minimumRange), which you stay in until the condition breaks. distance is now <= minimumRange and first == 0.
2. You enter while(distance <= minimumRange && first == 0). You stay there, constantly stopping the drive engines and taking measurements, until the object has clearance from the ultrasound sensor. distance is now > minimumRange and first == 0.
3. You enter while(distance > minimumRange). You stay in it until the condition breaks. distance is now <= minimumRange and first == 0.
4. You don't enter while(distance <= minimumRange && first == 1) because first == 0.
5. You set first = 1 and end the iteration.
6. You're now in the second iteration. Again, distance is now <= minimumRange and first == 1. You don't enter the first three whiles because of that.
7. You enter while(distance <= minimumRange && first == 1) and stay in that while until the heat death of the Universe (or until your Arduino's power runs out, whichever first).

So, the algorithm is solid. Therefore, there must be something wrong elsewhere. Here's what I see is suspect:

• pickObject() and putObject() are reversed. You stay in the second while. This can combine with an invalid starting position: if your robot can move its arm upwards and downwards from the starting state, then maybe something's wrong with your assumptions.
• for whatever reason, distance is always less than minimumRange after it encounters the first object (sensor fault, to short of a wait between trig pulse and echo, etc.) . You then stay in the first iteration, in the second while. Note that this requires the previous error condition to be consistent with observation.

There are also several things that are not exactly correct in your code:

• digitalWrite() latches, i.e. if you set in on LOW or HIGH on a pin, the value will stay set until you change it. You keep writing those values unnecessarily.
• as the other answers point out, loop() is a... loop. It repeats itself. You should rewrite your whiles into ifs (updating the conditions of course).
• you're also making your algorithm dependent on a busy-waiting approach by keeping measuring distance constantly until the arm moves, sending pulses over and over again. Electrical energy is not free, especially not on an embedded platform. You should add a delay() in those cases.

---

There may be also other malfunctions as well (I certainly hope you're not stalling your arm-driving engine, since you never actually stop it!).

Therefore, in general, you should solve such problems like so:

1. Add Serial.Write() statements to your code, make them informative, describe which steps has occurred and with what values of crucial parameters.
2. Connect your Arduino to the IDE via USB, or use a logging shield that e.g works with an SD card.
3. Based on those logs, determine where the fault in the algorithm is.

Using logging in that manner will save you many headaches.

Answer 2

Here is your problem:

The loop function runs constantly, every time it ends it starts right back up. You set first to 1, but you never re set it to 0, so every loop it goes through first is 1.

while(distance <= minimumRange && first == 1){
    stopMotors();

    putObject();

    distance = calculateDistance();
  }// end second while copied 

  first = 1;

Answer 3

The loop method goes on and on, over and over again until you run out of battery or turn your robot off.

I don't have your robot so it is difficult to test your code, but maybe, with all due respect, your approach is not exactly the right one.

For my robots I go behavior-based. Every loop (hundreds of times per second) the robot asks questions about the environnement and about its internal state. Using the answers, it chooses the right behavior.

The methods you are using are a good start to behavior-based robotic. Here are some slides explaining all of this: behavior-based robotic.

Hope it helps !

Answer 4

The loop function runs constantly, every time it ends it starts right back up. You set first to 1, but you never re set it to 0, so every loop it goes through first is 1.

while(distance <= minimumRange && first == 1){
    stopMotors();

    putObject();

    distance = calculateDistance();
}// end second while copied 

first = 1;