Let's Make Robots!

easing servo movements

Makes your servo moves in a smooth way

 

 

 

 

 

 

 

 

 

 

 

 

 

I’ve been working on this for a while and finally I have the courage to post the results.


I’m also testing the pololu micro serial servo controller in the hope I could achieve better results. One good thing the pololu controller has, is the ability to set the speed of the movement, I guess it is a good plus to have this extra control. And using this feature together with the filter you get really cool and smooth movements, check the end of the video.


The point I don’t like in this experience is that the robot shakes a lot, but that is due to the weak support. I will have to arrange a solution to void so much shakiness.

The low pass filter below is what makes the desaceleration while the value reaches the destination point. I’ve posted the full code below the video.

filter: 0.05; // 1 = no filter // has to be between 0.01 and 1
destinationValue = 1000;
destinationValueFiltered = destinationValueFiltered * (1.0-filter) + destinationValue * filter;

 

----------------------------------------------------------------------------------------------------------------------------

Links section for further development:

Robert Penner's Easing Equations:
http://robertpenner.com/easing/

Visualizers of easing equations:
http://www.robertpenner.com/easing/easing_demo.html

http://www.gizma.com/easing/

Processing Easing
http://processing.org/learning/basics/easing.html

Robert Penner's easing equations demo in Processing:
http://jesusgollonet.com/processing/pennerEasing/

Animation & Shaping:
http://www.megamu.com/processing/shapetween/

-------------------------------------------------------

Arduino Forum Topics:

Converting Easing Functions ActionScript -> Wiring
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1187650321

----------------------------------------------------------------------------------------------------------------------------

 

 

 

 

Code to move one servo directly connected to the Arduino:
// easing servo movements with low pass filter
// the servo signal must be connected directly to the arduino pin 2

// Time interval, this executes one piece of code from time to time
// Download Metro lybrary and instructions:
// http://www.arduino.cc/playground/Code/Metro
#include <Metro.h>
int duration = 3500; // duration of the interval in miliseconds
Metro intervaller = Metro (duration);

// MegaServo library
// download and instructions:
// http://www.arduino.cc/playground/Code/MegaServo
#include <MegaServo.h>
MegaServo servos;

// servos minimum and maximum position
#define MIN_POS 800 // the minuimum pulse width for your servos
#define MAX_POS 2200 // maximum pulse width for your servos

// servo pin
#define s0_pin 2

// variables to hold new destination positions
int d0; // destination0
int d0_sh; // destination0 to be smoothed

// the filter to be aplied
// this value will be multiplied by "d0" and added to "d0_sh"
float filtro = 0.05; // 0.01 to 1.0

// setup runs once, when the sketch starts
void setup() {
// initialize serial comunication
Serial.begin(9600);

// set servo pin
servos.attach(s0_pin);
}

// main program loop, executes forever after setup()
void loop() {

// check the time interval, if it reaches limit "duration" goes back to one
// and runs the code
if (intervaller.check() == 1) {

// calculate a new random position between max and min values
d0 = random(MIN_POS, MAX_POS);

// resets interval with a new random duration
intervaller.interval(random(500,2000));
}

// smooth the destination value
d0_sh = d0_sh * (1.0-filtro) + d0 * filtro;

// assign new position to the servo
servos.write(d0_sh);

// delay to make the servo move
delay(25);
}

 

And this is the code to use with the pololu micro serial servo controller:

// easing servo movements with a low pass filter and Pololu Micro Serial Servo Controller

// Time interval, this executes one piece of code from time to time
// Download Metro lybrary and instructions:
// http://www.arduino.cc/playground/Code/Metro
#include <Metro.h>
int duration = 3500; // duration of the interval in miliseconds
Metro intervaller = Metro (duration);

// servos minimum and maximum position
#define MIN_POS 500 // the minuimum pulse width for your servos
#define MAX_POS 5500 // maximum pulse width for your servos

// variables to hold new destination positions
int d0; // destination0
int d0_sh; // destination0 to be smoothed

// the filter to be aplied
// this value will be multiplied by "d0" and added to "d0_sh"
float filtro = 0.05; // 0.01 to 1.0

// set servo speed, goes from 1 to 127
int servoSpeed = 120;

// setup runs once, when the sketch starts
void setup() {
// initialize serial comunication
Serial.begin(9600);

// set servo pin and speed
servoSetSpeed(0, servoSpeed);
}

// main program loop, executes forever after setup()
void loop() {

// check the time interval, if it reaches limit "duration" goes back to one
// and runs the code
if (intervaller.check() == 1) {

// calculate a new random position between max and min values
d0 = random(MIN_POS, MAX_POS);

// resets interval with a new random duration
intervaller.interval(random(500,3000));
}

// smooth the destination value
d0_sh = d0_sh * (1.0-filtro) + d0 * filtro;

// assign new position to the servo
put(0,d0_sh);

// delay to make the servo move
delay(25);
}


// functions from this forum topic:
// http://forum.pololu.com/viewtopic.php?f=16&t=745&start=60&st=0&sk=t&sd=a

void put(int servo, int angle)
{
//servo is the servo number (typically 0-7)
//angle is the absolute position from 500 to 5500

unsigned char buff[6];

unsigned int temp;
unsigned char pos_hi,pos_low;

//Convert the angle data into two 7-bit bytes
temp=angle&0x1f80;
pos_hi=temp>>7;
pos_low=angle & 0x7f;

//Construct a Pololu Protocol command sentence
buff[0]=0x80; //start byte
buff[1]=0x01; //device id
buff[2]=0x04; //command number
buff[3]=servo; //servo number
buff[4]=pos_hi; //data1
buff[5]=pos_low; //data2

//Send the command to the servo controller
for(int i=0;i<6;i++){
Serial.print(buff[i],BYTE);
}

}

void servoSetSpeed(int servo, int speed){
//servo is the servo number (typically 0-7)
//speed is servo speed (1=fastest, 127=slowest)
//set speed to zero to turn off speed limiting

unsigned char buff[5];
unsigned char speedcmd;

speedcmd=speed&0x7f;//take only lower 7 bits of speed

buff[0]=0x80;//start byte
buff[1]=0x01;//device id
buff[2]=0x01;//command number
buff[3]=servo;//servo number
buff[4]=speed;//data1

for(int i=0;i<5;i++){
Serial.print(buff[i],BYTE);
}
}

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.
I also got a question, what is "d0_sh"'s initial value? (i looked at the first program)

The "d0" is a variable to store the destination point wich is randomly calculated, the "d0_sh" is a variable where the smooth filter will be applied.  "_sh" is for smooth :D

No no...i meant, what's its initial value? I mean... how can you calculate d0_sh if it equals to something which has d0_sh in it? It must have got an initial value somewhere right?

When you declare "int var;" and you don't assign any value to it, it will have 0 as an initial value.

If you have something like this:

filter: 0.05; // 1 = no filter // has to be between 0.01 and 1
d0 = 1000;
d0_sh = d0_sh * (1.0-filter) + d0 * filter;      

and this goes like:

d0_sh = 0 * (1.0 - 0.05) + 1000 * 0.05;     // d0_sh = 50
d0_sh = 50 * (1.0 - 0.05) + 1000 * 0.05;   // d0_sh = 99.05

143
186
........
974
976

and it goes until gets (almost) to 1000 (still working on it)

I hope it is more clear to you.

oh, didn't know it was 0 by default, thanks for the explanation!
Fixed an error on the first code example and added comments.

Excellent walkthrough! I`ve been looking for a way to get more out of my attiny2313 servo driver. This looks like just what I need.

I wonder how easy it would be to add acceleration and decceleration to the start and end of servo movements?

Thanks   :)

I also want to implement more ways to control movement, all the "easing" equations have been worked around in Flash ActionScript and Processing, check the links I've added. The research goes on  ;)

I would love to see this equations translated to Arduino code  :)

http://robertpenner.com/easing/easing_demo.html

I can easily use an equation where the origin point is lower than the destination point, but can't do the reverse.. my math skills aren't high enough for that..


EDIT: will post a new video tomorrow with good news   :D

Is that because the interpolation requires a negative increment in one axis but not the other? I've done this with several bots in RISC assembler. van Rijn is probably the best example.

My solution was to use four very similar code segments. There are only four (general) directions. Going towards any of: -x,-y; -x,+y; x,-y or x,y. I feel this was a bit of a cheat, but it works.

TriOmniWheeler does it in three axes, but that's a different story. You don't want to go there.