Arduino, Smooth RC Servo Motor Control



When using the arduino Servo.h library, you normally use myServo.write(), but you can use myServo.writeMicroseconds().  This lets you write out the controlling squarewave period in microseconds.  The rc servo standard is typically 1000us <-> 2000us, with 1500us as neutral position.  This means that you have a resolution of ~2000 individual steps, rather than the 180 (degree) steps the normal write() would provide - and therefore, the potential for significantly smoother servo motor movements. 

Check out: http://arduino.cc/en/Reference/ServoWriteMicroseconds

Here is the test code I have used to good effect so far, I have been using servo motors with an extended range of uSeconds.  Currently using one pot to set the target position, and the second pot to change the ramping/smoothing fraction.  Ramping/smoothing is done with a technique called easing (see here) which means continually adding a fraction-of-the-difference between target and current position.  See below, very simple code, only one variable to adjust smoothing.

The last point to mention is that if you want to benefit from writing out a higher resolution of steps, you need to write out the control signal much quicker!  I found the Serial.print statements to considerably slow the code down.



#include <Servo.h>

// Using floats as the servo library can handle this and
// it makes for smoother progression through position
// values.
float current_pos;
float target_pos;

// One variable to control smoothing.
float easing = 0.7;

// Motors
Servo l_motor;
Servo r_motor;

void setup() {

   // Serial seriously slows this code down... commented out
   //Serial.begin(9600);
 
   // Start the code at middle-ish
   // position
   current_pos = 1500;
 
   // pin/connector specific!
   l_motor.attach(4);
   r_motor.attach(5);
}

void loop() {
   int pot;   // target position
   int pot2; // easing fraction
   float diff; // difference of position

   // Read in pot, scale appropriately
   // to set uSecond position value
   // in range 600:2400
   pot = analogRead(A2);
   pot = map( pot, 0,1023, 0, 1800);
   pot = 2400 - pot;
 
   // Read in pot 2, used to create
   // a fraction to scale movement.
   pot2 = analogRead(A3);
   pot2 = map( pot2, 0, 1023, 0,1000);
   easing = (float)pot2;
   easing /= 1000;
 
   // Type cast to float.
   target_pos = (float)pot;

   // Work out the required travel.
   diff = target_pos - current_pos;
 
   // Debug prints
   // slow, so commented out
   /*
   Serial.print("Current: ");
   Serial.print( current_pos );
   Serial.print(", Target: ");
   Serial.print( target_pos );
   Serial.print(", Difference: ");
   Serial.print( diff );
   Serial.print(", Easing: ");
   Serial.print( easing );
   Serial.print(", L: ");
   Serial.print( current_pos );
   Serial.print(", R: ");
   Serial.print( 600 + (2400 - current_pos));
   Serial.println(" ");
   */
 
 
   // Avoid any strange zero condition
   if( diff != 0.00 ) {
      current_pos += diff * easing;
   } else {
      // if diff == 0, we don't need to move!
   }
 
   // Write out motor values.
   // Right motor is offset by 600, and the opposite of left.
   l_motor.writeMicroseconds( (int)current_pos );
   r_motor.writeMicroseconds( (int)(600 + (2400 - current_pos)) );
 
   // Delay, need to play with this, use millis()
   delay(20);
 
}