Ask your Expert: Nico teWinkel - Software Engineer who specialises in firmware, C and Java.

Hello Nico!

There seems to be a problem in my code, that arise from both the servo and the sound libraries trying to use together pin 11 timer.

I have tried to replace the Servo.h library with ServoTimer2.h library, but it did not help.

Can you please take a look and tell me if you know of any solution to this problem?

What happens with the robot itself, is that when the tank is close enough to a wall and the distance function kicks in the sound sample, the servos stop working and just make these funny uncontroled movement each time the sound sample is played.

Here is the code:

#include <ServoTimer2.h>

#include <PCM.h> // This is the sound library

const unsigned char sample[] PROGMEM = {

  126, 127, 128, 128, 128, 128, 128, 127 //  .......Here comes a very long array of numbers, that describe the sound sample and i took them off, for the sake of readablilty.

}; //This is the sound sample

  #define trigPin 13

#define echoPin 12

char val; // variable to receive data from the serial port

ServoTimer2 wheel1;

ServoTimer2 wheel2;

ServoTimer2 camera_servo1;

ServoTimer2 camera_servo2;

char mem=' '; //remember the last menu choice on the BlueControl app

int camera_servo1_mem_servo1=1500;  // remember the camera servo1 last angle so they will not exceed their limits

int camera_servo2_mem_servo2=1500;  // remember the camera servo1 last angle so they will not exceed their limits

int led =  10;    // LED connected to digital pin 13

int led_count = 225; // LED minimum dim value brfore it starts lighting

int light_saver_timer=0; // Count 3.5 minutes with no action to shut light and save battery

void setup() {

  Serial.begin(9600);       // start serial communication at 9600bps

  pinMode(trigPin, OUTPUT); //initiate ultrasound sensor trig pin

  pinMode(echoPin, INPUT);  //initiate ultrasound sensor echi pin

  pinMode(led, OUTPUT); //initiate led pin

  pinMode(11,OUTPUT);  //initiate speaker pin

  digitalWrite(11,LOW); //initiate speaker pin

}

void loop() {

  long duration, distance; // Next lines deal with finding and calculating ultrasound distance sensor

  digitalWrite(trigPin, LOW);  // Added this line

  delayMicroseconds(2); // Added this line

  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10); // Added this line

  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);

  distance = (duration/2) / 29.1;

  if (millis()-light_saver_timer > 210000){ // Count 3.5 minutes with no action to shut light and save battery

    led_count=225;

    light_saver_timer=millis();

    analogWrite(led,led_count);

  }

  if( Serial.available() ){       // if data is available to read

     val = Serial.read();         // read it and store it in 'val'

  }

  //Next lines save the last value that was chosen in the BlueControl app menu

  if( val == 'a') 

         {

            mem='a';

            val=' ';

         }

   else if( val == 'c')

          {

            mem='c';

            val=' ';

          }

  else if( val == 'd')

          {

            mem='d';

            val=' ';

          }

          else if( val == 'e')

          {

            mem='e';

            val=' ';

          }

else if( val == 'U' && mem == 'a' )

      {

      light_saver(); 

      drive_forward_long(); 

      val=' ';

     }

   else if(val == 'U' && mem == 'c' )

     {

      light_saver();

      drive_forward_short(); 

      val=' ';

     }

   else if( val == 'D' && mem == 'a' )              

  {

    light_saver();

    drive_backward_long();

    val=' ';

  }

   else if( val == 'D' && mem == 'c' )              

  {

    light_saver();

    drive_backward_short();

    val=' ';

  }

   else if( val == 'R' )              

  {

    light_saver();

    turn_right(); 

     val=' ';

  }

   else if( val == 'L' )             

  {

    light_saver();

    turn_left();

     val=' ';

  }

  else if( val == 'C' )             

  {

    light_saver();

    motor_stop();

     val=' ';

  }

else if( val == 'U' && mem == 'd' )

      {

      light_saver(); 

      camera_servo_up(); 

      val=' ';

     }

else if( val == 'D' && mem == 'd' )

      {

      light_saver(); 

      camera_servo_down(); 

      val=' ';

     }

   else if(val == 'U' && mem == 'e' )

     {

       light_saver();

        if ( led_count < 255)

   {

    led_count=led_count+2;

    analogWrite(led, led_count);

   }

      val=' ';

     }  

      else if(val == 'D' && mem == 'e' )

     {

        light_saver();

        if ( led_count > 225)

   {

    led_count=led_count-2;

    analogWrite(led, led_count);

   }

     val=' ';

     }

    else if (distance <= 41){  //Next lines play the sound sample in different frequencies, according to disdtance measured by ultra-sound sensor

       startPlayback(sample, sizeof(sample));

  if (distance <=41 && distance >=27) delay(8500);

     else if (distance <=27 && distance >=20) delay(5250);

      else if (distance <=20 && distance >=17) delay(2440);

       else if (distance <=17 && distance >=12) delay(1500);

        else if (distance <=12 && distance >=8) delay(1080);

         else if (distance <=8 && distance >=5) delay(740);

          else if (distance <=5 && distance >=3) delay(530);

           else if (distance <=4 && distance >=2) delay(340);

            else if (distance <=2 && distance >=0) delay(180);

  }

}

void motor_stop(){

   wheel1.attach(6);

   wheel2.attach(5);

   wheel1.attach(6);

   wheel2.attach(5);

   wheel1.write(1458);  // set servo to mid-point

   wheel2.write(1418);

}

void drive_forward_long(){

  wheel1.attach(6);

wheel2.attach(5);

wheel1.write(2000); 

wheel2.write(1000);

delay(4000);

wheel1.write(1458);  // set servo to mid-point

wheel2.write(1418);

}

void drive_forward_short(){

wheel1.attach(6);

wheel2.attach(5);

wheel1.write(2000); 

wheel2.write(1000);

delay(800);

wheel1.write(1458);  // set servo to mid-point

wheel2.write(1418);

}

void drive_backward_long(){

wheel1.attach(6);

wheel2.attach(5);

wheel1.write(1000); 

wheel2.write(2000);

delay(4000);

wheel1.write(1458);  // set servo to mid-point

wheel2.write(1418);

}

void drive_backward_short(){

wheel1.attach(6);

wheel2.attach(5);

wheel1.write(1000); 

wheel2.write(2000);

delay(800);

wheel1.write(1458);  // set servo to mid-point

wheel2.write(1418);

}

void turn_left(){

wheel1.attach(6);

wheel2.attach(5);

wheel1.write(1000); 

wheel2.write(1000);

delay(150);

wheel1.write(1458);  // set servo to mid-point

wheel2.write(1418);

}

void turn_right(){

wheel1.attach(6);

wheel2.attach(5);

wheel1.write(2000); 

wheel2.write(2000);

delay(150);

wheel1.write(1458);  // set servo to mid-point

wheel2.write(1418);

}

void camera_servo_up(){

camera_servo1.attach(8);

camera_servo2.attach(9);

if (camera_servo1_mem_servo1>800 && camera_servo2_mem_servo2<2200)

{

   camera_servo1_mem_servo1=camera_servo1_mem_servo1+30;

   camera_servo2_mem_servo2=camera_servo2_mem_servo2-30;

   camera_servo1.write(camera_servo1_mem_servo1);

   camera_servo2.write(camera_servo2_mem_servo2);

}

}

void camera_servo_down(){

   camera_servo1.attach(8);

   camera_servo2.attach(9);

if (camera_servo1_mem_servo1>800 && camera_servo2_mem_servo2<2200)

{

   camera_servo1_mem_servo1=camera_servo1_mem_servo1-30;

   camera_servo2_mem_servo2=camera_servo2_mem_servo2+30;

   camera_servo1.write(camera_servo1_mem_servo1);

   camera_servo2.write(camera_servo2_mem_servo2);

}

}

void light_saver() {

   light_saver_timer=millis();

}

Hi Roi,

I think the issue is with your delays:

  if (distance <=41 && distance >=27) delay(8500);

delay makes the program stop and just sit there, and while it's doing that none of your other code runs (except interrupts, which may be what keeps the sound going).

I'm not sure what the purpose of the delays are, but maybe you can do that in some other way that still allows the robot to respond to other things?

Cheers,

-Nico

Hello Nico!

The purpose of this section in the code, is that when the robot gets closer to the wall, the frequency of playback of the audio sample, will become higher. So the robot will say "Left" "Left" "Left"...etc or "Right" "Right" "Right"...etc at a higher or a lower frequency, according to the distance and direction of an obstacles on its sideways.

What would be the best way to implement this functionality in the code, without casuing this delay problem?

BTW, i had a problem when i was trying to implement the sections of the code, that deal with the ultra-sound sensor, when trying to implement it for 2 sensros and i was able to make only 1 sensor react to objects at various distances.

Do you have any clue what am i doing wrong about it?

Here is the original code, that i found here:

http://www.instructables.com/id/Simple-Arduino-and-HC-SR04-Example/step3/Upload-the-sketch/

And which on it, i tried to double many elements, so 2 sensors would be functional, but to no avail:

Here is the code, after trying to adapt it to work with 2 sensors:

/*

HC-SR04 Ping distance sensor]

VCC to arduino 5v GND to arduino GND

Echo to Arduino pin 13 Trig to Arduino pin 12

Red POS to Arduino pin 11

Green POS to Arduino pin 10

560 ohm resistor to both LED NEG and GRD power rail

More info at: http://goo.gl/kJ8Gl

Original code improvements to the Ping sketch sourced from Trollmaker.com

Some code and wiring inspired by http://en.wikiversity.org/wiki/User:Dstaub/robotcar

*/

#define trigPin 13

#define echoPin 12

#define trigPin1 7

#define echoPin1 4

//#define led 11

//#define led2 10

void setup() {

  Serial.begin (9600);

  pinMode(trigPin, OUTPUT);

  pinMode(echoPin, INPUT);

  pinMode(trigPin1, OUTPUT);

  pinMode(echoPin1, INPUT);

  //pinMode(led, OUTPUT);

  //pinMode(led2, OUTPUT);

}

void loop() {

  long duration, distance, duration1, distance1;

  digitalWrite(trigPin, LOW);  // Added this line

  delayMicroseconds(4); // Added this line

  digitalWrite(trigPin1, LOW);  // Added this line

  delayMicroseconds(4); // Added this line

  digitalWrite(trigPin, HIGH);

  //  delayMicroseconds(1000); - Removed this line

  delayMicroseconds(20); // Added this line

  digitalWrite(trigPin1, HIGH);

  //  delayMicroseconds(1000); - Removed this line

  delayMicroseconds(20); // Added this line

  digitalWrite(trigPin, LOW);

  digitalWrite(trigPin1, LOW);

  duration = pulseIn(echoPin, HIGH);

  duration1 = pulseIn(echoPin1, HIGH);

  distance = (duration/2) / 29.1;

  distance1 = (duration1/2) / 29.1;

  //if (distance < 4) {  // This is where the LED On/Off happens

   // digitalWrite(led,HIGH); // When the Red condition is met, the Green LED should turn off

// digitalWrite(led2,LOW);

//}

  //else {

   // digitalWrite(led,LOW);

   // digitalWrite(led2,HIGH);

// }

  if (distance >= 200 || distance <= 0){

    Serial.println("Out of range LEFT");

  }

  else {

    Serial.println("LEFT");

    Serial.print(distance);

    Serial.println(" cm");

  }

   delay(1000);

  if (distance1 >= 200 || distance1 <= 0){

    Serial.println("Out of range RIGHT");

  }

  else {

    Serial.println("RIGHT");

    Serial.print(distance1);

    Serial.println(" cm");

  }

  delay(1000);

}

Thanks a lot,

Roi.

Hello Nico!

The purpose of this section in the code, is that when the robot gets closer to the wall, the frequency of playback of the audio sample, will become higher. So the robot will say "Left" "Left" "Left"...etc or "Right" "Right" "Right"...etc at a higher or a lower frequency, according to the distance and direction of an obstacles on its sideways.

What would be the best way to implement this functionality in the code, without casuing this delay problem?

BTW, i had a problem when i was trying to implement the sections of the code, that deal with the ultra-sound sensor, when trying to implement it for 2 sensros and i was able to make only 1 sensor react to objects at various distances.

Do you have any clue what am i doing wrong about it?

Here is the original code, that i found here:

http://www.instructables.com/id/Simple-Arduino-and-HC-SR04-Example/step3/Upload-the-sketch/

And which on it, i tried to double many elements, so 2 sensors would be functional, but to no avail:

Here is the code, after trying to adapt it to work with 2 sensors:

/*

HC-SR04 Ping distance sensor]

VCC to arduino 5v GND to arduino GND

Echo to Arduino pin 13 Trig to Arduino pin 12

Red POS to Arduino pin 11

Green POS to Arduino pin 10

560 ohm resistor to both LED NEG and GRD power rail

More info at: http://goo.gl/kJ8Gl

Original code improvements to the Ping sketch sourced from Trollmaker.com

Some code and wiring inspired by http://en.wikiversity.org/wiki/User:Dstaub/robotcar

*/

#define trigPin 13

#define echoPin 12

#define trigPin1 7

#define echoPin1 4

//#define led 11

//#define led2 10

void setup() {

  Serial.begin (9600);

  pinMode(trigPin, OUTPUT);

  pinMode(echoPin, INPUT);

  pinMode(trigPin1, OUTPUT);

  pinMode(echoPin1, INPUT);

  //pinMode(led, OUTPUT);

  //pinMode(led2, OUTPUT);

}

void loop() {

  long duration, distance, duration1, distance1;

  digitalWrite(trigPin, LOW);  // Added this line

  delayMicroseconds(4); // Added this line

  digitalWrite(trigPin1, LOW);  // Added this line

  delayMicroseconds(4); // Added this line

  digitalWrite(trigPin, HIGH);

  //  delayMicroseconds(1000); - Removed this line

  delayMicroseconds(20); // Added this line

  digitalWrite(trigPin1, HIGH);

  //  delayMicroseconds(1000); - Removed this line

  delayMicroseconds(20); // Added this line

  digitalWrite(trigPin, LOW);

  digitalWrite(trigPin1, LOW);

  duration = pulseIn(echoPin, HIGH);

  duration1 = pulseIn(echoPin1, HIGH);

  distance = (duration/2) / 29.1;

  distance1 = (duration1/2) / 29.1;

  //if (distance < 4) {  // This is where the LED On/Off happens

   // digitalWrite(led,HIGH); // When the Red condition is met, the Green LED should turn off

// digitalWrite(led2,LOW);

//}

  //else {

   // digitalWrite(led,LOW);

   // digitalWrite(led2,HIGH);

// }

  if (distance >= 200 || distance <= 0){

    Serial.println("Out of range LEFT");

  }

  else {

    Serial.println("LEFT");

    Serial.print(distance);

    Serial.println(" cm");

  }

   delay(1000);

  if (distance1 >= 200 || distance1 <= 0){

    Serial.println("Out of range RIGHT");

  }

  else {

    Serial.println("RIGHT");

    Serial.print(distance1);

    Serial.println(" cm");

  }

  delay(1000);

}

Thanks a lot,

Roi.

Hi Roi,

I don't have time to dig into your code, sorry (lots of deadlines this week). Maybe try posting this as a new discussion (marked as a question) in the Arduino forum, where more people can look into it for you. (www.element14.com/.../arduino, click "Start a Discussion" on the bar beside the orange "Join this group" button).

For an alternative to using delay, you could try something like millis() to keep track of time, as is done in this tutorial:

http://www.arduino.cc/en/Tutorial/BlinkWithoutDelay

Or maybe the TimeAlarms library will help:

http://www.pjrc.com/teensy/td_libs_TimeAlarms.html

I'm wondering if you could set it to run the sound at desired intervals just using alarmRepeat.

Cheers,

-Nico