code seems to pause for no reason

[drewship]

I would like this code to take the inputs from two stationary ping sonar sensors and steer a car to follow another car in front of it. The car has two continuous rotation servos to move the car and one pan servo to steer the car. When the code runs, the serial monitor shows the sensor values and the continuous rotation servos operate correctly. When an object is placed several feet in front of the sensors the servos stop moving and the sensors stop pinging. After a period of time, the sensors will start pinging and the servos will start turning. can someone help determine why this is happening instead of the sensors and servos operating continuously to steer thee car as desired.

Code: [Select]

#include <Servo.h> //include Servo library

long duration, inchesRight, inchesLeft, cm, cmRight, cmLeft, durationLeft, durationRight; //time it takes to recieve PING))) signal

long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}


//rightMotor stops at 1388
//leftMotor stops at 1586
const int RForward = 400; //speed value of drive servo, value 500 CW, 2500 CCW for full speed rotation
const int RBackward = 2400; //speed value of drive servo, value 500 CW, 2500 CCW for full speed rotation
const int LForward = 1733; //the servos must rotate in opposite directions in order to drive in the same direction
const int pingPinLeft = 6;  //signal pin on parallax ping sensor connected to this arduino pin "remember the ping sensor is 5V, not source"
const int pingPinRight = 7;
const int dangerThresh = 10; //about 30 inches threshold for obstacles (in cm), this was 10 by default
const int dangerReverseThresh = 25; //about 10 inches threshold for obstacles (in cm), this was 10 by default
int leftDistance, rightDistance, distanceFwd, chkDistanceFwd; //distances on either side
int delayTime = 2;
int pos = 90;    // variable to store the servo position


Servo panMotor; //servo that pans the ping sensor
Servo leftMotor;  //continuous rotation servo that drives the left wheel
Servo rightMotor; //continuous rotation servo that drives the right wheel

void pingLeft()

{ 
 
  pinMode(pingPinLeft, OUTPUT);
  digitalWrite(pingPinLeft, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPinLeft, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPinLeft, LOW);
  pinMode(pingPinLeft, INPUT);
  durationLeft = pulseIn(pingPinLeft, HIGH);

 
}

void pingRight()

{ 


  pinMode(pingPinRight, OUTPUT);
  digitalWrite(pingPinRight, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPinRight, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPinRight, LOW);
  pinMode(pingPinRight, INPUT);
  durationRight = pulseIn(pingPinRight, HIGH);

 
}


void forward()
{
 
  rightMotor.writeMicroseconds(RForward); //move forward   
  leftMotor.writeMicroseconds(LForward); //move forward

}

void bothWheelStop()
{
 
  rightMotor.writeMicroseconds(1388);
  leftMotor.writeMicroseconds(1586);
 
}

void turnLeft() //if left is less obstructed
{
 

  panMotor.write(180);
  delay(100); //run motors this many milliseconds
  bothWheelStop(); 
}

void turnRight() //if right is less obstructed
{
 

   panMotor.write(0);
  delay(100); //run motors this many milliseconds
  bothWheelStop();
   
}

void setup()
{
 
  durationLeft = 0;
  durationRight = 0;
  cmRight = 0;
  cmLeft = 0;
  inchesRight = 0;
  inchesLeft = 0;
  chkDistanceFwd = 0;
  rightDistance = 0;
  leftDistance = 0;
  rightMotor.writeMicroseconds(1388);
  leftMotor.writeMicroseconds(1586);
  rightMotor.attach(10); //signal wire on right countinuous rotation "drive" servo connected to this arduino pin
  leftMotor.attach(8);  //signal wire on left continuous rotation "drive servo connected to this arduino pin
  panMotor.attach(11); //signal wire on pan servo for ping sensor connected to this arduino pin
  panMotor.write(pos); //set PING))) pan to center "90 by default"
  Serial.begin(9600);
}

void loop()
{
 
 

  pingRight();
  rightDistance = microsecondsToCentimeters(durationRight);
 
  pingLeft();
  leftDistance = microsecondsToCentimeters(durationLeft);

 
  // convert the time into a distance
  inchesLeft = microsecondsToInches(durationLeft);
  cmLeft = microsecondsToCentimeters(durationLeft);
   inchesRight = microsecondsToInches(durationRight);
  cmRight = microsecondsToCentimeters(durationRight);
 

  Serial.print(inchesRight);        //Prints to Serial Monitor
  Serial.print("inRight, ");        //Prints to Serial Monitor
  Serial.print(cmRight);            //Prints to Serial Monitor
  Serial.print("cmRight");          //Prints to Serial Monitor
  Serial.println();            //Prints to Serial Monitor
 
  Serial.print(inchesLeft);        //Prints to Serial Monitor
  Serial.print("inLeft, ");        //Prints to Serial Monitor
  Serial.print(cmLeft);            //Prints to Serial Monitor
  Serial.print("cmLeft");          //Prints to Serial Monitor
  Serial.println();            //Prints to Serial Monitor


  Serial.print("  dangerThresh="); //displays value of dangerThresh to validate operation
  Serial.print(dangerThresh);
  Serial.print("  rightDistance="); 
  Serial.print (rightDistance);
  Serial.print("  leftDistance=");
  Serial.print (leftDistance); 
 


  if ((rightDistance > dangerThresh) && (leftDistance > dangerThresh)) //if target distance is greater than programmed default, go straight
  {
   
    forward();
  }
 

  else if ((leftDistance > rightDistance) && (leftDistance > dangerThresh)) //if target distance is greater than programmed default AND if target is closer on the RIGHT side...turn RIGHT to face target
  { 
   
    turnRight();
  }
 

  else if ((rightDistance > leftDistance) && (rightDistance > dangerThresh))  //if target distance is greater than programmed default AND if target is closer on the LEFT side...turn LEFT to face target
  { 
   
    turnLeft();
  }
 

  else if ((rightDistance <= dangerThresh) || (leftDistance <= dangerThresh)) //if target is <= minimum programmed distance. STOP
  {
   
    bothWheelStop();
  }
 

}


[PeterH]

I guess that might be because it's executing this code:

Code: [Select]


if ((rightDistance <= dangerThresh) || (leftDistance <= dangerThresh)) //if target is <= minimum programmed distance. STOP
  {
   
    bothWheelStop();
  }


You've got some Serial.println() statements there - what do they tell you is going on when this problem happens?

[drewship]

That piece of code should only work when the object in front is 10 cm or less. I am experiencing this when the distance is greater than 10 cm.

[PeterH]

That piece of code should only work when the object in front is 10 cm or less. I am experiencing this when the distance is greater than 10 cm.

'Should'? What do the println statements tell you?

[AWOL]

Code: [Select]

void setup()
{
 
  durationLeft = 0;
  durationRight = 0;
  cmRight = 0;
  cmLeft = 0;
  inchesRight = 0;
  inchesLeft = 0;
  chkDistanceFwd = 0;
  rightDistance = 0;
  leftDistance = 0;
This is a waste of typing; they're already all zero.

[DuaneB]

And how are you powering the servos ? Check the two links in my signature for a possible issue with your setup that would cause the behavior you are seeing.

Duane B

[drewship]

Thanks for your replies, here is what i have done and what i have found. I removed the part of code in setup that set the variables to zero, I commented out the if statement at the end that stopped the servos and ran the program with no objects within four feet of the ping sensors. Below are the following serial monitor results for about fifteen seconds. The results that are zero are the result of the ping sensors not pinging (light did not flash on either sensor). The servos are powered by a separate 4AA battery pack.


22 inRight, 58 cmRight
23 inLeft, 60 cmLeft
 dangerThresh=10  rightDistance=58  leftDistance=60
23 inRight, 59 cmRight
24 inLeft, 61 cmLeft
 dangerThresh=10  rightDistance=59  leftDistance=61
0 inRight, 0 cmRight
52 inLeft, 133 cmLeft
 dangerThresh=10  rightDistance=0  leftDistance=133
8 inRight, 22 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=22  leftDistance=0
81 inRight, 208 cmRight
28 inLeft, 72 cmLeft
 dangerThresh=10  rightDistance=208  leftDistance=72
81 inRight, 207 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=207  leftDistance=0
64 inRight, 165 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=165  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=0  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=0  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=0  leftDistance=0
5 inRight, 13 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=13  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=0  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=0  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=0  leftDistance=0
1 inRight, 3 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=3  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=0  leftDistance=0
6 inRight, 16 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=16  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
 dangerThresh=10  rightDistance=0  leftDistance=0


When only using the first if statement used to run the servos and adding a 100ms delay between pings, i get the following results with nothing within 4 feet of the sensors.

12 inRight, 32 cmRight
53 inLeft, 135 cmLeft
  dangerThresh=10  rightDistance=32  leftDistance=135
77 inRight, 197 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=197  leftDistance=0
81 inRight, 208 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=208  leftDistance=0
82 inRight, 209 cmRight
25 inLeft, 64 cmLeft
  dangerThresh=10  rightDistance=209  leftDistance=64
82 inRight, 209 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=209  leftDistance=0
81 inRight, 208 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=208  leftDistance=0
33 inRight, 85 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=85  leftDistance=0
44 inRight, 113 cmRight
53 inLeft, 135 cmLeft
  dangerThresh=10  rightDistance=113  leftDistance=135
63 inRight, 162 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=162  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=0  leftDistance=0
82 inRight, 210 cmRight
19 inLeft, 48 cmLeft
  dangerThresh=10  rightDistance=210  leftDistance=48
2 inRight, 5 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=5  leftDistance=0
20 inRight, 53 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=53  leftDistance=0
82 inRight, 209 cmRight
1 inLeft, 4 cmLeft
  dangerThresh=10  rightDistance=209  leftDistance=4
0 inRight, 0 cmRight
52 inLeft, 133 cmLeft
  dangerThresh=10  rightDistance=0  leftDistance=133
114 inRight, 291 cmRight
52 inLeft, 134 cmLeft
  dangerThresh=10  rightDistance=291  leftDistance=134
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=0  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=0  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=0  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=0  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=0  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=0  leftDistance=0
0 inRight, 0 cmRight
0 inLeft, 0 cmLeft
  dangerThresh=10  rightDistance=0  leftDistance=0

[PeterH]

That shows that pingLeft() and pingRight() are sometimes producing an answer of zero.

I suggest you write a simplified sketch that only calls one of those functions at regular intervals and prints the results, and test it until you are sure it produces reliable answers under all conditions.

I can't see from your output - can you confirm that the sketch does actually keep running at the same speed and it only the ping results which are going wrong?

I see that you are using delayMicroseconds() with quite small values to trigger the ping. Where did those values come from? The online reference says this about delayMicroseconds():

This function works very accurately in the range 3 microseconds and up. We cannot assure that delayMicroseconds will perform precisely for smaller delay-times.

[drewship]

Here is my updated code. This code currently pings each sensor every three ms while activating the continuous motion servos until either one or both sensors detects an object within the dangerThresh which is currently set at ten cm. Once the object is greater than dangerThresh, the continuous rotation servos will start up again. so now my problem seems to be that the pan servo does not turn right or left as directed by the if statements for Right and Left respectively. Thanks for your help so far.

Code: [Select]

#include <Servo.h> //include Servo library

long duration, inchesRight, inchesLeft, cm, cmRight, cmLeft, durationLeft, durationRight; //time it takes to recieve PING))) signal

long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}


//rightMotor stops at 1388
//leftMotor stops at 1586
const int RForward = 400; //speed value of drive servo, value 500 CW, 2500 CCW for full speed rotation
const int RBackward = 2400; //speed value of drive servo, value 500 CW, 2500 CCW for full speed rotation
const int LForward = 1733; //the servos must rotate in opposite directions in order to drive in the same direction
const int pingPinLeft = 6;  //signal pin on parallax ping sensor connected to this arduino pin "remember the ping sensor is 5V, not source"
const int pingPinRight = 7;
const int dangerThresh = 10; //about 30 inches threshold for obstacles (in cm), this was 10 by default
const int dangerReverseThresh = 25; //about 10 inches threshold for obstacles (in cm), this was 10 by default
int leftDistance, rightDistance, distanceFwd, chkDistanceFwd; //distances on either side
int delayTime = 2;
int pos = 90;    // variable to store the servo position


Servo panMotor; //servo that pans the ping sensor
Servo leftMotor;  //continuous rotation servo that drives the left wheel
Servo rightMotor; //continuous rotation servo that drives the right wheel

void pingLeft()

{ 
 
  pinMode(pingPinLeft, OUTPUT);
  digitalWrite(pingPinLeft, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPinLeft, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPinLeft, LOW);
  pinMode(pingPinLeft, INPUT);
  durationLeft = pulseIn(pingPinLeft, HIGH);

 
}

void pingRight()

{ 


  pinMode(pingPinRight, OUTPUT);
  digitalWrite(pingPinRight, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPinRight, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPinRight, LOW);
  pinMode(pingPinRight, INPUT);
  durationRight = pulseIn(pingPinRight, HIGH);

 
}


void forward()
{
 
  rightMotor.writeMicroseconds(RForward); //move forward   
  leftMotor.writeMicroseconds(LForward); //move forward

}

void bothWheelStop()
{
 
  rightMotor.writeMicroseconds(1388);
  leftMotor.writeMicroseconds(1586);
 
}

void turnLeft() //if left is less obstructed
{
 

  panMotor.write(180);
  delay(100); //run motors this many milliseconds
  bothWheelStop(); 
}

void turnRight() //if right is less obstructed
{
 

   panMotor.write(0);
  delay(100); //run motors this many milliseconds
  bothWheelStop();
   
}

void setup()
{
 

  rightMotor.writeMicroseconds(1388);
  leftMotor.writeMicroseconds(1586);
  rightMotor.attach(10); //signal wire on right countinuous rotation "drive" servo connected to this arduino pin
  leftMotor.attach(8);  //signal wire on left continuous rotation "drive servo connected to this arduino pin
  panMotor.attach(11); //signal wire on pan servo for ping sensor connected to this arduino pin
  panMotor.write(pos); //set PING))) pan to center "90 by default"
  Serial.begin(9600);
}

void loop()
{
 
 

  pingRight();
  rightDistance = microsecondsToCentimeters(durationRight);
  delay(300);
  pingLeft();
  leftDistance = microsecondsToCentimeters(durationLeft);
  delay(300);
 
  // convert the time into a distance
  inchesLeft = microsecondsToInches(durationLeft);
  cmLeft = microsecondsToCentimeters(durationLeft);
   inchesRight = microsecondsToInches(durationRight);
  cmRight = microsecondsToCentimeters(durationRight);
 

  Serial.print(inchesRight);        //Prints to Serial Monitor
  Serial.print("inRight, ");        //Prints to Serial Monitor
  Serial.print(cmRight);            //Prints to Serial Monitor
  Serial.print("cmRight");          //Prints to Serial Monitor
  Serial.println();            //Prints to Serial Monitor
 
  Serial.print(inchesLeft);        //Prints to Serial Monitor
  Serial.print("inLeft, ");        //Prints to Serial Monitor
  Serial.print(cmLeft);            //Prints to Serial Monitor
  Serial.print("cmLeft");          //Prints to Serial Monitor
  Serial.println();            //Prints to Serial Monitor


  Serial.print("  dangerThresh="); //displays value of dangerThresh to validate operation
  Serial.print(dangerThresh);
  Serial.print("  rightDistance="); 
  Serial.print (rightDistance);
  Serial.print("  leftDistance=");
  Serial.print (leftDistance); 
 


  if ((rightDistance > dangerThresh) && (leftDistance > dangerThresh)) //if target distance is greater than programmed default, go straight
  {
   
    forward();
  }
 

  else if ((leftDistance > rightDistance) && (rightDistance > dangerThresh)) //if target distance is greater than programmed default AND if target is closer on the RIGHT side...turn RIGHT to face target
  { 
   
    turnRight();
  }
 

  else if ((rightDistance > leftDistance) && (leftDistance > dangerThresh))  //if target distance is greater than programmed default AND if target is closer on the LEFT side...turn LEFT to face target
  { 
   
    turnLeft();
  }
 

  else if ((rightDistance <= dangerThresh) || (leftDistance <= dangerThresh)) //if target is <= minimum programmed distance. STOP
  {
   
    bothWheelStop();
  }
 

}


[UKHeliBob]

Let's look at a small section of your code

Code: [Select]

void loop()
{
  pingRight();
  rightDistance = microsecondsToCentimeters(durationRight);
  delay(300);
  pingLeft();
  leftDistance = microsecondsToCentimeters(durationLeft);
  delay(300);
Presumably the pingRight and pingLeft functions are meant to measure the distances right and left, but if you look at either of them they do not move the pan servo.

[PeterH]

Suggest you put a print statement in each of those 'if' blocks so that you can tell what it thinks it is doing - also add a final 'else' to tell you when none of those conditions are true. Since you already print out the values used in those conditions, it should now be obvious what it is doing and why.