Question about software and PING

Well, it is Sat Morning, actually noon for me Saturday. I have worked this AM with my code.

I finally got to the “Ah!hah!” moment on the subroutines and I totally understand how this works now, thanks to all of the kind people on this forum. I have the program very close to where I want it. But, I have a few kinks to knock out of it.

When i place the robot in a Hallway, it turns left and right and cannot go straight. This is because the PING is not allowing it to get more than a foot and a half to two feet from an object. I need to get upto 3" close to the object before turning. I have tried all morning on this, and I am to the point where I MUST ask.
Thanks, My code:
``#

include <MegaServo.h>
   MegaServo myservo;
// Begin Robot Code
int micVal;
int cdsVal;
int irLval;  // Left IR
int irCval;  // Center IR
int irRval;  // Right IR
int irRRval; // Rear IR
int i;   // Generic Counter
int x;  // Generic Counter
int PLval;  // Pulse Width for Left Servo
int PRval;  // Pulse Width for Right Servo
int cntr;  // Generic Counter Used for Determining amt. of Object Detections
int counter; // Generic Counter
int clrpth;  // amt. of Milliseconds Of Unobstructed Path
int objdet;  // Time an Object was Detected
int task;  // Routine to Follow for Clearest Path
int pwm;  // Pulse Width for Pan Servo
boolean add;  // Whether to Increment or Decrement PW Value for Pan Servo
int distance;  // Distance to Object Detected via Ultrasonic Ranger
int oldDistance;  // Previous Distance Value Read from Ultrasonic Ranger

float scale = 1.9866666666666666666666666666667;  // *Not Currently Used*

int LeftPin = 6;  // Left Servo
int RightPin = 9;  // Right Servo
int PingServoPin = 5;  // Pan Servo
int irLPin = 0;            // Analog 0; Left IR
int irCPin = 1;            // Analog 1; Center IR
int irRPin = 2;            // Analog 2; Right IR
int irRRPin =3;            // Analog 3; Rear IR
int ultraSoundSignal = 7; // Ultrasound signal pin
int val = 0;              // Used for Ultrasonic Ranger
int ultrasoundValue = 0;  // Raw Distance Val
int oldUltrasoundValue;  // *Not used*
int pulseCount;        // Generic Counter
int timecount = 0; // Echo counter
int ledPin = 13; // LED connected to digital pin 13
int pos = 0;
int SPKR = 4;

#define BAUD 9600
#define CmConstant 1/29.034

void setup() {
  myservo.attach(11);
  pos = 40;
  myservo.write(pos);
  delay(150);
  Serial.begin(9600);
  pinMode(SPKR, OUTPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(LeftPin, OUTPUT);
  pinMode(RightPin, OUTPUT);
  pinMode(PingServoPin, OUTPUT);
  pinMode(irLPin, INPUT);
  pinMode(irCPin, INPUT);
  pinMode(irRPin, INPUT);
  pinMode(irRRPin, INPUT);
    // Beep! 
  {
    for (int i = 0; i<500; i++) {  // generate a 1KHz tone for 1/2 second
      digitalWrite(SPKR, HIGH);
      delayMicroseconds(500);
      digitalWrite(SPKR, LOW);
      delayMicroseconds(500);
    } 
  for(i = 0; i < 20; i++) {
    digitalWrite(PingServoPin, HIGH);
    delayMicroseconds(655 * 2);
    digitalWrite(PingServoPin, LOW);
    delay(20);
  }
  ultrasoundValue = 600;
  i = 0;      
}
}
void loop()
{
  Look();
  Go();
}
void Look() {
  irLval = analogRead(irLPin);
  irCval = analogRead(irCPin);
  irRval = analogRead(irRPin);
  if(irLval > 200) {
    BrowUP();
     PLval = 850;
    PRval = 820;
    x = 5;
    cntr = cntr + 1;
    clrpth = 0;
    objdet = millis();
  }
  else if(irCval > 200) {
    BrowUP(); 
    PLval = 850;
    PRval = 820;
    x = 10;
    cntr = cntr + 1;
    clrpth = 0;
    objdet = millis();
  }
  else if(irRval > 200) {
    BrowUP();
    //delay(150);
    //BrowDOWN();  
    PLval = 650;
    PRval = 620;
    x = 5;
    cntr = cntr + 1;
    clrpth = 0;
    objdet = millis();
  }
  else {
    x = 1;
    PLval = 850;
    PRval = 620;
    counter = counter + 1;
    clrpth = (millis() - objdet);
    if(add == true) {
      pwm = pwm + 50;
    }
    else if(add == false) {
      pwm = pwm - 50;
    }
    if(pwm < 400) {
      pwm = 400;
      add = true;
    }
    if(pwm > 950) {
      pwm = 950;
      add = false;
    }
    digitalWrite(PingServoPin, HIGH);
    delayMicroseconds(pwm * 2);
    digitalWrite(PingServoPin, LOW);
    delay(20);
    readPing();
    if(ultrasoundValue < 500) {
      cntr = cntr + 1;
      switch(pwm) {
      case 400:
        x = 7;
        PLval = 650;
        PRval = 650;
        Go();
        break;
      case 500:
        x = 10;
        PLval = 650;
        PRval = 650;
        Go();
        break;
      case 600:
        x = 14;
        PLval = 850;
        PRval = 850;
        Go();
        break;
      case 700:
        x = 10;
        PLval = 850;
        PRval = 850;
        Go();
        break;
      case 950:
        x = 7;
        PLval = 850;
        PRval = 850;
        Go();
        break;
      }
    }
  }
  if(cntr > 25 && clrpth < 2000) {
    clrpth = 0;
    cntr = 0;
    Scan();
  }
}
void Go() {
  for(i = 0; i < x; i++) {
    digitalWrite(LeftPin, HIGH);
    delayMicroseconds(PLval * 2);
    digitalWrite(LeftPin, LOW);
    digitalWrite(RightPin, HIGH);
    delayMicroseconds(PRval * 2);
    digitalWrite(RightPin, LOW);
    delay(20);
  }
}
void readPing() {  // Get Distance from Ultrasonic Ranger
  timecount = 0;
  val = 0;
  pinMode(ultraSoundSignal, OUTPUT); // Switch signalpin to output
  digitalWrite(ultraSoundSignal, LOW); // Send low pulse
  delayMicroseconds(2); // Wait for 2 microseconds
  digitalWrite(ultraSoundSignal, HIGH); // Send high pulse
  delayMicroseconds(5); // Wait for 5 microseconds
  digitalWrite(ultraSoundSignal, LOW); // Holdoff
  pinMode(ultraSoundSignal, INPUT); // Switch signalpin to input
  val = digitalRead(ultraSoundSignal); // Append signal value to val
  while(val == LOW) { // Loop until pin reads a high value
    val = digitalRead(ultraSoundSignal);
  }

  while(val == HIGH) { // Loop until pin reads a high value
    val = digitalRead(ultraSoundSignal);
    timecount = timecount +1;            // Count echo pulse time
  }
  ultrasoundValue = timecount; // Append echo pulse time to ultrasoundValue

  // Lite up LED if any value is passed by the echo pulse
   if(timecount > 0){
    digitalWrite(ledPin, HIGH);
  }
} 
void Scan() {   // Scan for the Clearest Path
  oldDistance = 10;
  task = 0;
  for(i = 1; i < 5; i++) {
    switch(i) {
    case 1:
      //Serial.println("Pos. 1");
      pwm = 1125;    ///  incr. by 100 from 1085
      break;
    case 2:
      //Serial.println("Pos. 2");
      pwm = 850; //// increased by 100 from 850
      break;
    case 3:
      //Serial.println("Pos. 3");
      pwm = 400;
      break;
    case 4:
      //Serial.println("Pos. 4");
      pwm = 235;
      break;
    }
    for(pulseCount = 0; pulseCount < 20; pulseCount++) {  // Adjust Pan Servo and Read USR
      digitalWrite(PingServoPin, HIGH);
      delayMicroseconds(pwm * 2);
      digitalWrite(PingServoPin, LOW);
      readPing();
      delay(20);
    }
    distance = ((float)ultrasoundValue * CmConstant);   // Calculate Distance in Cm
    if(distance > oldDistance) {  // If the Newest distance is longer, replace previous reading with it
      oldDistance = distance;
      task = i;   // Set task equal to Pan Servo Position
    }
  }
  distance = 50;  // Prevents Scan from Looping
  switch(task) {   // Determine which task should be carried out
  case 0:  // Center was clearest
    x = 28;
    PLval = (850);
    PRval = (850);
    Go();
    break;
  case 1:  // 90 degrees Left was Clearest
    x = 14;
    PLval = (650);
    PRval = (650);
    Go();
    break;
  case 2:  // 45 degrees left
    x = 7;
    PLval = (650);
    PRval = (650);
    Go();
    break;
  case 3:  // 45 degrees right
    x = 7;
    PLval = (850);
    PRval = (850);
    Go();
    break;
  case 4:  // 90 degrees right
    x = 14;
    PLval = (850);
    PRval = (850);
    Go();
    break;
  }
}

void BrowUP()
{
  pos = 0;
  myservo.write(pos);
  delay(500);
  BrowDOWN();
  return;
}
void BrowDOWN()
{
  pos = 40;
  myservo.write(pos);
  return;
}

// End Robot Code

Saturday PM now. I finally figured out where to change it so the robot would come close, but not touch. Now, everything is working perfect that I had tried to accomplish this session, except, now that it is doing more 'Thinkin', it is terribly slow. I still have to add some eyes, photosensors, and some other items. I now question whether the Arduino is going to be fast enough to make it. I could also use mulitple Arduinos. I am very pleased with everything else. I would like to make it faster. So, if you can look at my code and tell me some ways to make it faster, that would be nice. I will try on this end and let you know what I have come up with. I mean, it runs acceptible, but just barely. Would like for it to run faster.

Thanks!

;D

I'm sure the arduino is plenty fast. You'll need to look at getting rid of some of your delay() calls. I'm guessing that delay(500) is the root of most of the slowness.

I have the delay so when I put the brow UP, it stays up a little while (1/2 SEC). I changed some of the delays from 250 to 50. I will look closer. Thanks.

You see, I still want to put on two photocells, A PIR, and Microphone and more. I am afraid this arduino is getting filled up as far as thinking. Hope not.

This is about as optimized as I can get it and the brows still work:

#include <MegaServo.h>
   MegaServo myservo;
// Begin Robot Code
//int micVal;
//int cdsVal;
int irLval;  // Left IR
int irCval;  // Center IR
int irRval;  // Right IR
int i;   // Generic Counter
int x;  // Generic Counter
int PLval;  // Pulse Width for Left Servo
int PRval;  // Pulse Width for Right Servo
int cntr;  // Generic Counter Used for Determining amt. of Object Detections
int counter; // Generic Counter
int clrpth;  // amt. of Milliseconds Of Unobstructed Path
int objdet;  // Time an Object was Detected
int task;  // Routine to Follow for Clearest Path
int pwm;  // Pulse Width for Pan Servo
boolean add;  // Whether to Increment or Decrement PW Value for Pan Servo
int distance;  // Distance to Object Detected via Ultrasonic Ranger
int oldDistance;  // Previous Distance Value Read from Ultrasonic Ranger

float scale = 1.9866666666666666666666666666667;  // *Not Currently Used*

int LeftPin = 6;  // Left Servo
int RightPin = 9;  // Right Servo
int PingServoPin = 5;  // Pan Servo
int irLPin = 0;            // Analog 0; Left IR
int irCPin = 1;            // Analog 1; Center IR
int irRPin = 2;            // Analog 2; Right IR
int ultraSoundSignal = 7; // Ultrasound signal pin
int val = 0;              // Used for Ultrasonic Ranger
int ultrasoundValue = 0;  // Raw Distance Val
int oldUltrasoundValue;  // *Not used*
int pulseCount;        // Generic Counter
int timecount = 0; // Echo counter
int ledPin = 13; // LED connected to digital pin 13
int pos = 0;
int SPKR = 4;

#define BAUD 9600
#define CmConstant 1/29.034

void setup() {
  myservo.attach(11);
  pos = 40;
  myservo.write(pos);
  delay(150);
  myservo.detach();
  Serial.begin(9600);
  pinMode(SPKR, OUTPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(LeftPin, OUTPUT);
  pinMode(RightPin, OUTPUT);
  pinMode(PingServoPin, OUTPUT);
  pinMode(irLPin, INPUT);
  pinMode(irCPin, INPUT);
  pinMode(irRPin, INPUT);
    // Beep! 
  {
    for (int i = 0; i<500; i++) {  // generate a 1KHz tone for 1/2 second
      digitalWrite(SPKR, HIGH);
      delayMicroseconds(400);
      digitalWrite(SPKR, LOW);
      delayMicroseconds(400);
    } 
  for(i = 0; i < 20; i++) {
    digitalWrite(PingServoPin, HIGH);
    delayMicroseconds(655 * 2);
    digitalWrite(PingServoPin, LOW);
    delay(20);
  }
  ultrasoundValue = 600;
  i = 0;      
}
}
void loop()
{
  Look();
  Go();
}
void Look() {
  irLval = analogRead(irLPin);
  irCval = analogRead(irCPin);
  irRval = analogRead(irRPin);
  if(irLval > 200) {
    BrowUP();
     PLval = 850;
    PRval = 820;
    x = 5;
    cntr = cntr + 1;
    clrpth = 0;
    objdet = millis();
  }
  else if(irCval > 200) {
    BrowUP(); 
    PLval = 850;
    PRval = 820;
    x = 10;
    cntr = cntr + 1;
    clrpth = 0;
    objdet = millis();
  }
  else if(irRval > 200) {
    BrowUP();
    PLval = 650;
    PRval = 620;
    x = 5;
    cntr = cntr + 1;
    clrpth = 0;
    objdet = millis();
  }
  else {
    x = 1;
    PLval = 850;
    PRval = 620;
    counter = counter + 1;
    clrpth = (millis() - objdet);
    if(add == true) {
      pwm = pwm + 50;
    }
    else if(add == false) {
      pwm = pwm - 50;
    }
    if(pwm < 400) {
      pwm = 400;
      add = true;
    }
    if(pwm > 950) {
      pwm = 950;
      add = false;
    }
    digitalWrite(PingServoPin, HIGH);
    delayMicroseconds(pwm * 2);
    digitalWrite(PingServoPin, LOW);
    delay(20);
    readPing();
    if(ultrasoundValue < 150) { // this value is the closeness of PING will get.
      cntr = cntr + 1;
      switch(pwm) {
      case 400:
        x = 7;
        PLval = 650;
        PRval = 650;
        Go();
        break;
      case 500:
        x = 10;
        PLval = 650;
        PRval = 650;
        Go();
        break;
      case 600:
        x = 14;
        PLval = 850;
        PRval = 850;
        Go();
        break;
      case 700:
        x = 10;
        PLval = 850;
        PRval = 850;
        Go();
        break;
      case 950:
        x = 7;
        PLval = 850;
        PRval = 850;
        Go();
        break;
      }
    }
  }
  if(cntr > 25 && clrpth < 2000) {
    clrpth = 0;
    cntr = 0;
    Scan();
  }
}
void Go() {
  for(i = 0; i < x; i++) {
    digitalWrite(LeftPin, HIGH);
    delayMicroseconds(PLval * 2);
    digitalWrite(LeftPin, LOW);
    digitalWrite(RightPin, HIGH);
    delayMicroseconds(PRval * 2);
    digitalWrite(RightPin, LOW);
    delay(20);
  }
}
void readPing() {  // Get Distance from Ultrasonic Ranger
  timecount = 0;
  val = 0;
  pinMode(ultraSoundSignal, OUTPUT); // Switch signalpin to output
  digitalWrite(ultraSoundSignal, LOW); // Send low pulse
  delayMicroseconds(2); // Wait for 2 microseconds
  digitalWrite(ultraSoundSignal, HIGH); // Send high pulse
  delayMicroseconds(5); // Wait for 5 microseconds
  digitalWrite(ultraSoundSignal, LOW); // Holdoff
  pinMode(ultraSoundSignal, INPUT); // Switch signalpin to input
  val = digitalRead(ultraSoundSignal); // Append signal value to val
  while(val == LOW) { // Loop until pin reads a high value
    val = digitalRead(ultraSoundSignal);
  }
  while(val == HIGH) { // Loop until pin reads a high value
    val = digitalRead(ultraSoundSignal);
    timecount = timecount +1;            // Count echo pulse time
  }
  ultrasoundValue = timecount; // Append echo pulse time to ultrasoundValue
  // Lite up LED if any value is passed by the echo pulse
   if(timecount > 0){
    digitalWrite(ledPin, HIGH);
  }
} 
void Scan() {   // Scan for the Clearest Path
  oldDistance = 30;
  task = 0;
  for(i = 1; i < 5; i++) {
    switch(i) {
    case 1:
      //Serial.println("Pos. 1");
      pwm = 1125;    ///  incr. by 100 from 1085
      break;
    case 2:
      //Serial.println("Pos. 2");
      pwm = 850; //// increased by 100 from 850
      break;
    case 3:
      //Serial.println("Pos. 3");
      pwm = 400;
      break;
    case 4:
      //Serial.println("Pos. 4");
      pwm = 235;
      break;
    }
    for(pulseCount = 0; pulseCount < 20; pulseCount++) {  // Adjust Pan Servo and Read USR
      digitalWrite(PingServoPin, HIGH);
      delayMicroseconds(pwm * 2);
      digitalWrite(PingServoPin, LOW);
      readPing();
      delay(20);
    }
    distance = ((float)ultrasoundValue * CmConstant);   // Calculate Distance in Cm
    if(distance > oldDistance) {  // If the Newest distance is longer, replace previous reading with it
      oldDistance = distance;
      task = i;   // Set task equal to Pan Servo Position
    }
  }
  distance = 50;  // Prevents Scan from Looping
  switch(task) {   // Determine which task should be carried out
  case 0:  // Center was clearest
    x = 28;
    PLval = (850);
    PRval = (850);
    Go();
    break;
  case 1:  // 90 degrees Left was Clearest
    x = 14;
    PLval = (650);
    PRval = (650);
    Go();
    break;
  case 2:  // 45 degrees left
    x = 7;
    PLval = (650);
    PRval = (650);
    Go();
    break;
  case 3:  // 45 degrees right
    x = 7;
    PLval = (850);
    PRval = (850);
    Go();
    break;
  case 4:  // 90 degrees right
    x = 14;
    PLval = (850);
    PRval = (850);
    Go();
    break;
  }
}

void BrowUP()
{
  myservo.attach(11);
  pos = 0;
  myservo.write(pos);
  delay(500);
  BrowDOWN();
  return;
}
void BrowDOWN()
{
  myservo.attach(11);
  pos = 40;
  myservo.write(pos);
  delay(120);
  myservo.detach();
  return;
}

// End Robot Code

any suggestions are welcome.

Thanks!

Just keep in mind that is a full 1/2 second that the Arduino is doing nothing. Take a look at the blink without delay example on the site to give you some ideas.

It's all comming from the brows , it seems. I may have two versions one with brows operating and one without brows. But, the brows are soooo cool. I also want to add photocells to make it a photovore and photophobe, and PIR for burglar alarm, and microphone so it can hear me. A color sensor, and maybe a video camera. But, if they slow it down like this, we are going to be talking another cpu.

Did you check out the blink without delay example?

Thanks, Digger, I will try this.

Question 4-U, Digger. Are you saying that when I want to raise my Eyebrows for 1-2 seconds instead of delay(1000, or 2000) I can somehow use this code to do this and not stop the whole PING process that the function BrowsUP(); is called?

There are several places in my code which I have to say delay 50,150,500, etc.

How exactly would I go about that? Would I place this code within my subroutine and instead of saying delay, say long interval , etc?

Give me another example using my situation if you have time.

This really sounds good because my robot was running FAST without the browsUP and BrowsDown subroutines.

Thank You all so much for your time. This has got to be the GREATEST forum on the whole internet!!!!

There are several places in my code which I have to say delay 50,150,500, etc

The trick is to separate the actions (switching digital lines, reading inputs etc) from the delays, and remember that everything executes in a big loop. Every time you have a delay, you need to think about not tying the processor up just watching the clock until the desired amount of time has elapsed, but put that part of the code in a state so that each time it is called, it looks to see if the time has elapsed. If it has, you go set the state to the next action and execute it. If the time hasn't elapsed, just remain in that state until it has.

I think I understand, but could you give me an example or point to a place on arduino.cc so I can see this. I have a hard head and I am new to c language.

Thanks!

This example (below) shows how to monitor a bucket of water without delaying. It flashes a LED while filling, and not using delays. For you, you would just have more states. You can use switch instead of if… else… if, but it often increases code size.

You can also have inner states to each state. For example, STATE_SHOW_EMOTION would show whatever emotion is currently in process, and EMOTION_STATE_STARTBROW can transition to EMOTION_STATE_WAITFORBROW, to EMOTION_STATE_FINISHBROW, etc… Each state checks millis() to see if the required time has expired, and if so, moves to the next state.

int iLastLED=LOW;
int iLastLedSwitch = 0;

void loop()
{
if(iState==STATE_LOWWATERINBUCKET)
{
   // set the relay to start pumping.
   digitalWrite(13, HIGH);
   iState=STATE_FILLING_BUCKET;
}
if (iState==STATE_FILLING_BUCKET)
{
   // check the bucket level.
   if(analogRead(7)>500)
  {
     // ok, bucket is full.
     iState=STATE_BUCKET_FULL_ANNOUNCE;
  }
   // show that progress is being made.
   if(millis()-iLastLedSwitch >500)
   {
      digitalWrite(12, iLastLED);
      iLastLed=!iLastLed;
     iLastLedSwitch = millis();
   }
}
if(iState==STATE_BUCKET_FULL_ANNOUNCE)
{
   // turn off the LED, in case it was left on.
   digitalWrite(12, LOW);
  // stop the relay - don't pump more in.
   digitalWrite(13, LOW);
   Serial.print("The bucket is now full.  Go ahead...");
   iState=STATE_CHECK_WATER_LEVEL_NORMAL;
}
if(iState==STATE_CHECK_WATER_LEVEL_NORMAL)
{
   // check the water level.
   if(analogRead(7)<200)
   {
       iState = STATE_LOWWATERINBUCKET;
      Serial.print("Starting bucket filling...  Water detected low.");
   }
}
}

Thank you, spinlock!

I have tried several experiments, but It has went over my head. Maybe another day I will grasp it.