Servo sound scanning

I’m sure this is not a new problem but I’ve blown up my brain trying to resolve it today.

I purchased a Zumo bot and with a little imagination, bolts and wiring I’ve been able to hijack some code that uses a sonic sensor to detect an object immediately in front of the bot, change direction and carry on it’s way.

#include <ZumoMotors.h>
#include <Pushbutton.h> 
#include <ZumoBuzzer.h>
#include <Servo.h>  // servo library
#include <avr/pgmspace.h>

#define FORWARD_SPEED     100
#define LED_PIN 13
#define LED_PINA 5

int ledStateA = LOW;             // ledState used to set the LED
int ledStateB = LOW;
    int position;
  long duration, inches, cm;

long previousMillis = 0;
long interval = 300;

const int trigPin = 2;
const int echoPin = 11;

ZumoBuzzer buzzer;
Servo servo1;  // servo control object

Pushbutton button(ZUMO_BUTTON);   

ZumoMotors motors;

void fireup()
  buzzer.playNote(NOTE_E(5), 100, 15);
  buzzer.playNote(NOTE_A(5), 100, 15);
  digitalWrite(5, HIGH);
  //digitalWrite(6, HIGH);

void setup()

  pinMode(LED_PIN, OUTPUT);
  pinMode(LED_PINA, OUTPUT);


   digitalWrite(LED_PIN, HIGH);
   motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);


void loop()

  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(trigPin, OUTPUT);
  digitalWrite(trigPin, LOW);
  digitalWrite(trigPin, HIGH);
  digitalWrite(trigPin, LOW);
  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.

  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);
  // convert the time into a distance
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);

  if(cm < 8){      // run backwards and spin ;

      // play note A5



long microsecondsToInches(long microseconds)
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second).  This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See:
  return microseconds / 74 / 2;
long microsecondsToCentimeters(long microseconds)
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;

void collider()
//  buzzer.playNote(NOTE_A(5), 200, 15);ZumoBuzzer buzzer;
  digitalWrite(LED_PIN, LOW);   motors.setSpeeds(-FORWARD_SPEED, -FORWARD_SPEED);   delay(100);   for (int speed = 0; speed >= -200; speed--)

void alive()
  unsigned long currentMillis = millis();

  if(currentMillis - previousMillis > interval) {
    // save the last time you blinked the LED 
    previousMillis = currentMillis;   

    // if the LED is off turn it on and vice-versa:
    if (ledStateA == LOW)
      {ledStateA = HIGH;
       ledStateB = LOW;}
      {ledStateA = LOW;
       ledStateB = HIGH;}
    // set the LED with the ledState of the variable:
       digitalWrite(5, ledStateA);

It works for me, it has some extra stuff in there and it needs some tidying up.

Stage two is to make that sonic sensor pan side to side on a servo.

I have tried to incorporate this code to do that:


Example sketch 08


  Sweep a servo back and forth through its full range of motion.

  A "servo", short for servomotor, is a motor that includes 
  feedback circuitry that allows it to be commanded to move to
  specific positions. This one is very small, but larger servos
  are used extensively in robotics to control mechanical arms,
  hands, etc. You could use it to make a (tiny) robot arm,
  aircraft control surface, or anywhere something needs to be
  moved to specific positions.

Hardware connections:

  The servo has a cable attached to it with three wires.
  Because the cable ends in a socket, you can use jumper wires
  to connect between the Arduino and the servo. Just plug the
  jumper wires directly into the socket.
  Connect the RED wire (power) to 5 Volts (5V)
  Connect the WHITE wire (signal) to digital pin 9
  Connect the BLACK wire (ground) to ground (GND)

  Note that servos can use a lot of power, which can cause your
  Arduino to reset or behave erratically. If you're using large
  servos or many of them, it's best to provide them with their
  own separate 5V supply. See this Arduino Forum thread for info:

This sketch was written by SparkFun Electronics,
with lots of help from the Arduino community.
This code is completely free for any use.
Visit to learn about the Arduino.

Version 2.0 6/2012 MDG

// If we had to write a sketch to control a servo from scratch,
// it would be a lot of work. Fortunately, others have done the
// hard work for you. We're going to include a "library"
// that has the functions needed to drive servos.

// A library is an set of additional functions you can add to
// your sketch. Numerous libraries are available for many uses,
// see for information
// on the standard libraries, and Google for others. When you're
// using a new part, chances are someone has written a library
// for it.

#include <Servo.h>  // servo library

// Once you "include" a library, you'll have access to those
// functions. You can find a list of the functions in the servo
// library at:
// Most libraries also have example sketches you can load from
// the "file/examples" menu.

// Now we'll create a servo "object", called myservo. You should
// create one of these for each servo you want to control. 
// You can control a maximum of twelve servos on the Uno 
// using this library. (Other servo libraries may let you
// control more). Note that this library disables PWM on
// pins 9 and 10!

Servo servo1;  // servo control object

void setup()
  // We'll now "attach" the servo1 object to digital pin 9.
  // If you want to control more than one servo, attach more
  // servo objects to the desired pins (must be digital).

  // Attach tells the Arduino to begin sending control signals
  // to the servo. Servos require a continuous stream of control
  // signals, even if you're not currently moving them.
  // While the servo is being controlled, it will hold its 
  // current position with some force. If you ever want to
  // release the servo (allowing it to be turned by hand),
  // you can call servo1.detach().


void loop()
  int position;
  // To control a servo, you give it the angle you'd like it
  // to turn to. Servos cannot turn a full 360 degrees, but you
  // can tell it to move anywhere between 0 and 180 degrees.

  // Change position at full speed:

  // Change position at a slower speed:

  // To slow down the servo's motion, we'll use a for() loop
  // to give it a bunch of intermediate positions, with 20ms
  // delays between them. You can change the step size to make 
  // the servo slow down or speed up. Note that the servo can't
  // move faster than its full speed, and you won't be able
  // to update it any faster than every 20ms.

  // Tell servo to go to 180 degrees, stepping by two degrees
  for(position = 35; position < 155; position += 2)
    servo1.write(position);  // Move to next position
    delay(20);               // Short pause to allow it to move

  // Tell servo to go to 0 degrees, stepping by one degree

  for(position = 155; position >= 35; position -= 2)
    servo1.write(position);  // Move to next position
    delay(20);               // Short pause to allow it to move

I can’t get the thing to work though.

The bot fires up, the servo turns a full left and that’s all she wrote.

Hi nlancaster

I have tried to incorporate this code to do that:

But at the moment, you are trying this servo program by itself as it is? You haven't combined the two programs?

How are you powering the servo? Can you post a link to details of the servo you are using.



Hi there - this is the servo

The servo works beautifully with the second piece of code. I want to attach the sonic sensor on top and have it sweep left and right. Can’t seem to figure out where to put that servo code though in the first piece.

As a test I placed it in the setup loop and it will do a full sweep left and right one time, so everything seems wired up right.

My first guess is the variables are being reset somehow?

That piece of code you posted causes the servo to sweep back and forth usingfor(position = 155; position >= 35; position -= 2)etc.

I suspect in your robot project you want code that behaves like this pseudo code

loop() {
   do stuff with the robot
   move the servo a small bit
   ping the distance

In other words, the movement of the servo should probably be integrated into the other code rather than all being done at once as in the demo code.

If you really do want to sweep the servo through the full arc while the robot waits for that to complete then you need to ping the distance immediately after servo1.write(position);


 // Tell servo to go to 180 degrees, stepping by two degrees
  for(position = 35; position < 155; position += 2)

If you are going to have useless comments, you MUST make them correct useless comments. Otherwise, they just piss readers off.

Yes by George, that's exactly it. Sweep, scan, sweep scan, etc until it sees a terrible obstacle such as a discarded toy from the children or myself.

I will double check that code for the comment that is identified. I confess much of this work was done under a delicious narcotic painkiller regime following sinus surgery the prior Friday. While it makes the pain almost unnoticeable it does also allow breaks in concentration, such as watching a juice commercial several times over because it make me laugh.

Thanks team - off to check code before this latest pill is fully absorbed.

// actually quite like the pills

I figured this out.

The Zumo robot uses a library that is shared with the standard servo library, basically the Timer is messed up.

A little bit of digging on the Zumo site and I found the workaround they suggested.