Pages: [1]   Go Down
Author Topic: code seems to pause for no reason  (Read 816 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 43
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
#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();
  }
 

}
Logged

UK
Offline Offline
Shannon Member
****
Karma: 223
Posts: 12631
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Code:
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?
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Offline Offline
Newbie
*
Karma: 0
Posts: 43
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

UK
Offline Offline
Shannon Member
****
Karma: 223
Posts: 12631
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 302
Posts: 26250
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
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.
Logged

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

Dubai, UAE
Offline Offline
Edison Member
*
Karma: 22
Posts: 1675
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Logged


Offline Offline
Newbie
*
Karma: 0
Posts: 43
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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


« Last Edit: April 05, 2013, 07:09:57 pm by drewship » Logged

UK
Offline Offline
Shannon Member
****
Karma: 223
Posts: 12631
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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():
Quote
This function works very accurately in the range 3 microseconds and up. We cannot assure that delayMicroseconds will perform precisely for smaller delay-times.
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Offline Offline
Newbie
*
Karma: 0
Posts: 43
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
#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();
  }
 

}
Logged

East Anglia (UK)
Online Online
Faraday Member
**
Karma: 114
Posts: 4248
May all of your blinks be without delay()
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Let's look at a small section of your code
Code:
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.
Logged

Please do not send me PMs asking for help.  Post in the forum then everyone will benefit from seeing the questions and answers.

UK
Offline Offline
Shannon Member
****
Karma: 223
Posts: 12631
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Pages: [1]   Go Up
Jump to: