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?
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:
first==0. You enter the while(distance > minimumRange), which you stay in until the condition breaks. distance is now <= minimumRange and first == 0.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.while(distance > minimumRange). You stay in it until the condition breaks. distance is now <= minimumRange and first == 0.while(distance <= minimumRange && first == 1) because first == 0.first = 1 and end the iteration.distance is now <= minimumRange and first == 1.
You don't enter the first three whiles because of that.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.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.loop() is a... loop. It repeats itself. You should rewrite your whiles into ifs (updating the conditions of course).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:
Serial.Write() statements to your code, make them informative, describe which steps has occurred and with what values of crucial parameters.Using logging in that manner will save you many headaches.