Code Rewrite.. Functions slow things down.

Projects Programming
1

I have been slowly adding things to my little robot borrowing code from here and there and slowly learning to write my own. I have and Uno Rev 3 with and Adafruit motorshield and i have added a parallax Ping and recently a 16x2 LCd readout.

My problem is this. I have code that works and does exactly what i want it o do. basic collision avoidance with serial and LCD status display. The trouble is everytime I add a new piece I have to rearrange and make the code work all over. So i decided to get inline and start writing each piece as a function and Thought I had it figured out. The trouble is it seems to slow the code so much that it doesn't perform well anymore. I will post both sketches and any tips will be greatly appreciated.

This sketch works fine and no apparent delays.

//Woody Bot

#include <Ping.h>
#include <AFMotor.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x3f,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display

Ping ping = Ping(A3); //set which model ping your using

AF_DCMotor motor(1, MOTOR12_64KHZ); //Set motors to right frequency to match your servo.
// create motor #1, 64KHz pwm 
 
AF_DCMotor motor2(2, MOTOR12_64KHZ); 
// create motor #2, 64KHz pwm 

int count = 0;

void setup() {  
  pinMode(13, OUTPUT);
    
  Serial.begin(9600);    // start serial communication at 9600 baud
  
  lcd.init();  // initialize the lcd 
  lcd.backlight();
  lcd.print("   Woody Bot");// Print a message to the LCD.
  lcd.setCursor(0,1);
  lcd.print("  Version 1.4"); //Print on line 2 of the lcd
  delay(3000);
  
  pinMode(A0, INPUT_PULLUP); // make pin A0 an input with internal pullup
 
  motor.setSpeed(255); // set the speed 0f the servos
  motor2.setSpeed(130);
       
  while (digitalRead(A0) == HIGH){   // Pause until the button is pressed
  lcd.setCursor(0,1);
  lcd.print("  Press Start!    ");
  }
} 
 
void loop() 
{ 
  if(count == 0) {
    for(int i = 0; i<5; i++) {    // do this 5 times
    ping.fire();
    
    int inches = ping.inches();  // where is everything?
    int cm = ping.centimeters();
    int mseconds = ping.microseconds();
    
    Serial.print(inches);  // print values out on serial monitor
    Serial.print(" , ");
    Serial.print(cm);
    Serial.print(" , ");
    Serial.println(mseconds);
    delay(100);
    
    if(inches <= 6) {    // if the Woody Bot gets too close...
    
    lcd.setCursor(0,1);
    lcd.print("   Turn Left    ");
            
    motor.run(BACKWARD); 
    motor2.run(FORWARD); 
    delay(300);
  }else{
      
    lcd.setCursor(0,1);
    lcd.print("   ^^^Forward^^^   ");
    
    motor.run(FORWARD); 
    motor2.run(FORWARD); 
    count = 1;
    }
  }
}
   if(count == 1) {
     for(int i = 0; i<5; i++) {  // do this 5 times
         
     ping.fire();
    
     int inches = ping.inches();   // where are we???
     int cm = ping.centimeters();
     int mseconds = ping.microseconds();
    
     Serial.print(inches);  // print on the serial monitor
     Serial.print(" , ");
     Serial.print(cm);
     Serial.print(" , ");
     Serial.println(mseconds);

     motor.run(FORWARD); //Forward
     motor2.run(FORWARD); 
    
     delay(100);
    
    if(inches <= 6) {    // if the Woody Bot is too close, turn the other way
            
    lcd.setCursor(0,1);
    lcd.print("   Turn Left    ");
    
    motor.run(BACKWARD); 
    motor2.run(FORWARD); 
    delay(300);
     
  }else{
     lcd.setCursor(0,1);
     lcd.print("    Forward     ");
    
     motor.run(FORWARD); 
     motor2.run(FORWARD);
       }
     }
   }
}

This is my rewritten code trying to put as much as I can in functions. I have tried a dozen variations and keep having problems. The Ping is definitely not activating at 6 inches away. It sometimes gets stuck in the turn left loop. It almost appears that the code is slowed way down and delaying the functions. but I have a feeling I am not formatting something correctly.

//Code for the Woody Bot, with Ping Sensor, and LCD2004 readout
//
//Kevin "Woody" Woodyard
//
//Check out http://woodystime.blogspot.com for more info and build guide.

#include <Ping.h>
#include <AFMotor.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x3f,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display

Ping ping = Ping(A3); //set which model ping your using

AF_DCMotor motor(1, MOTOR12_64KHZ); //Set motors to right frequency to match your servo.
// create motor #1, 64KHz pwm 
 
AF_DCMotor motor2(2, MOTOR12_64KHZ); 
// create motor #2, 64KHz pwm 

void setup() {  
     
  Serial.begin(9600);    // start serial communication at 9600 baud
  
  lcd.init();  // initialize the lcd 
  lcd.backlight();
  lcd.print("   Woody Bot");// Print a message to the LCD.
  lcd.setCursor(0,1);
  lcd.print("  Version 1.4"); //Print on line 2 of the lcd
  delay(3000);
  
  pinMode(A0, INPUT_PULLUP); // make pin A0 an input with internal pullup
 
  motor.setSpeed(255); // set the speed 0f the servos
  motor2.setSpeed(130);
  
  while (digitalRead(A0) == HIGH){   // Pause until the button is pressed
  lcd.setCursor(0,1);
  lcd.print("  Press Start!    ");
  }
} 

void loop() {
      Scan();
      int inches = ping.inches();   // where are we???
      Forward();
        if(inches <= 6) {    // if the Woody Bot gets too close...
          Left();
        }else{
           Forward();
        }
}
void Scan(){
    for(int i = 0; i<5; i++)    // do this 5 times
    ping.fire();
    }
    
void Forward(){ //Run both motors forward
    lcd.setCursor(0,1);
    lcd.print("   ^^^Forward^^^   ");
    
    motor.run(FORWARD); 
    motor2.run(FORWARD); 
    delay(100);
    }
    
void Left(){ //Run Motors opposite to rotate left.
    lcd.setCursor(0,1);
    lcd.print("   Turn Left    ");
            
    motor.run(BACKWARD); 
    motor2.run(FORWARD); 
    delay(300);
    }
    
void Right(){ //Run Motors opposite to rotate right.
    lcd.setCursor(0,1);
    lcd.print("   Turn Right    ");
            
    motor.run(FORWARD); 
    motor2.run(BACKWARD); 
    delay(300);
    }

Thanks

WOody

2

You might want to have a look at the NewPing library at Arduino Playground - Ping Library

The author mentions that it doesn't delay a full second if there is no echo in range.

3

The functions are not slowing you down. You've altered the logic in the process of rearranging them.
With functions your program becomes much cleaner though and thus is much easier to fix.

Try something like this (and maybe the NewPing library suggested above):

//Code for the Woody Bot, with Ping Sensor, and LCD2004 readout
//
//Kevin "Woody" Woodyard
//
//Check out http://woodystime.blogspot.com for more info and build guide.

#include <Ping.h>
#include <AFMotor.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>

#define THRESHOLD 6

LiquidCrystal_I2C lcd(0x3f, 16, 2);  // set the LCD address to 0x27 for a 16 chars and 2 line display
Ping ping = Ping(A3);

//Set motors to right frequency to match your servo.
AF_DCMotor motor(1, MOTOR12_64KHZ);
AF_DCMotor motor2(2, MOTOR12_64KHZ); 

void setup()
{
  pinMode(A0, INPUT_PULLUP);

  motor.setSpeed(255);
  motor2.setSpeed(130);

  lcd.init();
  lcd.backlight();

  sayHello();
} 

void loop()
{
  if (scan() <= THRESHOLD)    // if the Woody Bot gets too close...
    turnLeft();
  else
    goForward();
}

void sayHello()
{
  lcd.print("   Woody Bot");
  lcd.setCursor(0,1);
  lcd.print("  Version 1.4");
  delay(3000);

  // Pause until the button is pressed
  while (digitalRead(A0) == HIGH)
  {
    lcd.setCursor(0,1);
    lcd.print("  Press Start!    ");
  }
}

double scan()
{
  ping.fire();
  return ping.inches();
}

void goForward()
{
  motor.run(FORWARD); 
  motor2.run(FORWARD); 

  lcd.setCursor(0,1);
  lcd.print("   ^^^Forward^^^   ");

  delay(100);
}

void turnLeft()
{
  motor.run(BACKWARD); 
  motor2.run(FORWARD); 

  lcd.setCursor(0,1);
  lcd.print("   Turn Left    ");

  delay(300);
}

void turnRight()
{
  motor.run(FORWARD); 
  motor2.run(BACKWARD); 

  lcd.setCursor(0,1);
  lcd.print("   Turn Right    ");

  delay(300);
}
4

int2str,

Thanks your Mods worked pefrect. I wasn't sure you could put a function in Setup or anywhere else for that matter, Still learning that. SO it appears i wasn't too far off , but it was really late last night and my eyes were hurtin so I asked for help.

I see the major change is the Ping scan. My way i borrowed from another sketch. If I read it right it was pinging 5 times then returning the results. The code I borrowed that from originally had a servo scanning the ping back and forth. I am not doing that. Your way is using double and scanning just once. Can you tell me what the differences are so I understand in the future?

I will work on adding the NewPing Library. I seem to have better luck on a lot of things using older libraries, not sure why. Maybe more docs and my inexperience.

5

Just an FYI I know now why I didn't use the NewPing Library. The Example is setup using the 4 pin Ping style sensors. I have the 3 pin version. I have a couple of the other ones but this one is already mounted on the bot. So I didn't take the time to figure it out.

6

Sarconastic:
I see the major change is the Ping scan. My way i borrowed from another sketch. If I read it right it was pinging 5 times then returning the results. The code I borrowed that from originally had a servo scanning the ping back and forth. I am not doing that. Your way is using double and scanning just once. Can you tell me what the differences are so I understand in the future?

If you fix the indentation on your original code, you'll see that it was calling ping.fire() once and then checked the result and set the motors. It did the complete process 5 times. When you had initially re-written the program, you just fired a pulse 5 times in a row and then only looked at the last result. There is no need for that. Just ping once and evaluate the result.

ping.inches() returns a double. That's why I changed the return parameter of the scan() function. Since it now calls fire() and returns the result.

7

That explains the delay I was getting then. Thanks for the detailed Explanation it will help me in the future.

The first sketch I had created by pulling snippets from everywhere, and didn't really pay attention to format our layout. If it worked I was happy. I am trying now to get better at it. The old DOS days are helping but with code I don't fully understand sometime it can be difficult to understand the proper layout. Every day is a bit better though.

Thanks again