Servorator

[Note: This post has been updated to reflect changes to the library’s API]

Setting a servo to a new position causes the servo to move to that position as fast as its little motor can manage. That generally results in whiplash like control. The better solution is to incrementally move to the new position in smaller steps. This way you can create smoother, more controlled movements.

I was hoping that there was an existing Arduino library to provide such control but, alas, I was unable to find one. Being the software engineer that I am I decided to create my own. I’m all about systems. The result is the Servorator library.

Screen Shot 2013-12-26 at 12.22.40 AM

SS_Servorator Library on GitHub

In it’s current form Servorator is very basic. It only lets you set the speed of each servo. Ultimately I want it to handle acceleration, de-acceleration and support for “trimming” – as there is quite a bit of variation between individual servos with regard to their range of movement.

Still, the library is useful as is, and it is easy to use.

First step is to import the lib into the Arduino IDE. Go to GitHub repo and download the zip file. Then import the library into Arduino.

In your sketch you need to include the library so:


#import <SS_Servorator.h>

You can then create an instance of SS_Servorator:


#define NUM_SERVOS  6

SS_Servorator sr(NUM_SERVOS);

When you create an instance you pass in the number of servos to control. You could create multiple instances – if for some reason you needed to manage groups of servos – but one instant is usually all you would need.

For each instance you create you need to provided a servo handling routine. Here is an example of such a routine:


// the servo handler for Servorator
void update_servo( SS_Index index, SS_Angle angle, void *data)
{
  // SS_Angle is in 1000ths of a degree
  servo[index].write( angle/1000);
}

Servorator will call this routine whenever it needs to update the position of a servo. The routine should make the appropriate call to whatever servo interface you are using. In the example we are calling the Arduino’s Servo library write function. Note that the angle passed into the handler is in 1000ths of degrees, meaning it has a range of 0..180000, but the Servo lib uses only degrees so we need to divide by 1000 first to convert;

You need to register the update routine with Servorator at set up using the following call:


void setup()
{
  :
  // register servo handler
  sr.setServoHandler( update_servo, NULL);
  :
}

The second parameter is a pointer to void. Whatever pointer you pass will in turn be passed through to the update routine in the ‘data’ param. You use that param if you need to associate some data with the instance callback.

For the update routine to get called you need to regularly call Servorator’s ‘service’ function:


// main loop
void loop()
{
  :
  sr.service();
  :
}

If service is not called frequently then servo movement will become erratic and slowed.

Here are all the code pieces in one sketch:


// include needed librarys
#include
#include <SS_Servorator.h>

#define NUM_SERVOS 6

Servo servo[NUM_SERVOS];
SS_Servorator sr(NUM_SERVOS);

// the servo handler for Servorator
void update_servo( SS_Index index, SS_Angle angle, void *data)
{
  // SS_Angle is in tenths of a degree
  servo[index].write( angle/1000);
}

void setup()
{

  // assign PWM pins to servos
  servo[0].attach(3);
  servo[1].attach(5);
  servo[2].attach(6);
  servo[3].attach(9);
  servo[4].attach(10);
  servo[5].attach(11);

  // register servo handler
  sr.setServoHandler( update_servo, NULL);

}

// main loop
void loop()
{
  :
  sr.service();
  :
}

The example uses the Arduino’s Servo library to control servos on pins 3, 5, 6, 9, 10 and 11.

So how do we actually control the servos now? Where you we specify what angles to move them to?

Instead of calling the servo controls directly, we tell Servorator what we want to happen and it, in turn, calls the servo update handler to perform the servo changes. To set servo 0 to 175 degrees we do the following:


    sr.setServoTargetAngle( 0, SS_DEGREES(175));

The first parameter is the servo number. Servo numbers start from index 0 and go up. So, in our example above, the range of servo indexes are from 0 to 5.

The second param is the angle to move the servo to. Servorator deals in 1000ths of degrees. The SS_DEGREES() macro helps make code a little more readable. Instead of using 175000 1000ths of a degree we can say SS_DEGREES(175). You can also type SS_DEGREES(175.5) but keep in mind that Servorator is only accurate to a 1000th of a degree so SS_DEGREES(175.572987) is identical to SS_DEGREES(175.572)

You can read the current and target servo angles using:


    SS_Angle current = sr.getServoAngle( 0 );
    SS_Angle target = sr.getServoTargetAngle( 0 );

‘target’ is the angle set in the last call to setServoTargetAngle(). ‘current’ is the angle the servo is currently at.

The speed at which the servos turn is set using:


    sr.setServoMaxVelocity( 0, SS_DEGREES(60));

setServoMaxVelocity() lets you set the speed of a servo in 1000ths of degrees per second. The above code sets the speed of servo 0 to 60 degrees per second.

The library’s example sketch – SimepleServo.ino – shows how all this works together. It uses pins 3, 5, 6, 9, 10 and 11 to oscillate servos back and forth 90 degrees at various speeds, with pin 3 (servo 0) being fastest and pin 11 (servo 5) being slowest.


// include needed librarys
#include <Servo.h>
#include <SS_Servorator.h>

#define NUM_SERVOS 6

Servo servo[NUM_SERVOS];
SS_Servorator sr(NUM_SERVOS);

// the servo handler for Servorator
void update_servo( SS_Index index, SS_Angle angle, void *data)
{
  // SS_Angle is in 1000th of a degree
  servo[index].write( angle/1000);
}

void setup()
{

  Serial.begin(9600);
  // assign PWM pins to servos
  servo[0].attach(3);
  servo[1].attach(5);
  servo[2].attach(6);
  servo[3].attach(9);
  servo[4].attach(10);
  servo[5].attach(11);

  // register servo handler
  sr.setServoHandler( update_servo, NULL);

  // set all servos at 45 degrees
  // servo 0 is fastest
  SS_Velocity vel = SS_FAST_RATE;
  for ( int i=0; i<NUM_SERVOS;i++)
  {
    sr.setServoTargetAngle(i, SS_DEGREES(45));
    sr.setServoMaxVelocity( i, vel );
    vel = vel / 2 ;
  }

}

// main loop
void loop()
{
  // make servos ping-pong between 45 and 135 degrees
  for ( int i=0; i<NUM_SERVOS;i++)
  {
    SS_Angle angle = sr.getServoAngle(i);

    if ( angle <= SS_DEGREES(45))
    {
      sr.setServoTargetAngle(i, SS_DEGREES(135));
    }
    else if ( angle >= SS_DEGREES(135))
    {
      sr.setServoTargetAngle(i, SS_DEGREES(45));
    }
  }
  // sr.service() needs to be called regularly so that
  // the servos are updated via update_servo()
  sr.service();
}

Finally there is an option to set to servo update frequency:


  sr.setUpdateInterval( 20 );

this option lets you control the frequency of servo updates by specifying the number of milliseconds between up dates. The default is 20 ms which is 50 Hz. Keep in mind that the service() call must be called at least at the same rate, or higher, as the update frequency. If you set the update interval to 20 ms but only call service() once a second then the update frequency is once a second.

There is still a lot more I want to do with Servorator. For me it’s been a great introduction to creating Arduino libraries and I’m one more step closer to getting my robotic arm smoothly animated. My next task is building a sequencer…

Advertisements

Comments welcome

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s