Let's Make Robots!

Smooth Head Rotation

Rotates a head along a mathematical function
  • Programming language:
AttachmentSize
AngleLookupTable.xls145 KB

Did you ever tried to rotate the robot head and found the movement nervous?
   This tip can help to smooth movements out. 

Did you ever looked at a robot and found its movements human like?
   This tip can tell you how you can humanize your robot's moves. 
 

Motivation

Most of us one day have the challenge to rotate something from one angle to another. It might be a head, arm, finger, tail or whatever you have to move. Most of us initially go on and write a for-loop that counts from 0° to i.e. 180° with an increment of 1 and write the counter to the servo, i.e. [.., servo.write(42°), servo.write(43°), ...]. This is fine. A classic linear movement.

In this tip we want to go one step further and use a non-linear movement to make the move smooth. You also get an idea what mathematical function is used to create the data for you.

 

Introduction

The movement must start from zero velocity and go to zero velocity. There you find an intrinsically nature of the mathematical function we are looking for. It must be near zero in the beginning and near zero in the end. 

One function that has this nature is the Sinus Square velocity over time function. This function is used alot in feedback control systems. It accelerates progressive in the first third, continues with almost no acceleration in the second third and then decelerates regressive in the last third.

Using this Sinus Square function makes sense in almost every move your robot makes. It not only looks and sounds better than the linear function but is also more gentle to the servo gears. The gears grip and "slowly" start to rotate.

If you want to find out more about this topic then have a look into the further readings at the end of this tip.

 

The Smooth Head Rotation

Start your Arduino IDE and copy the sample code from the end of this page. Paste it into the C editor and upload it to the microcontroller.

When you now observe the servo movement you see, that it does exactly what I mentioned before. It accelerates smooth. No sudden full speed but a smooth beginning of a movement. Then constant speed. Then decelerates smooth back to zero velocity at 180°.

Now play around with the timerInMillies that your ears hear the smoothness too. 

 

The Electronics

Plug the servo to pin 6 and the led to pin 13.

Use pin 6 when you want to use the code below.

 

The Math

Skip this chapter if you just want to have a smooth head rotation and don't bother about the calculus behind it.

 

The sinus square function we use is in velocity over time. That is the V(t) function. We want to set angles (distances) and not velocities therefore we need to apply some calculus. To calculate the way over time values the V(t) function must be integrated to a W(t) function (W for way). The W(t) function must then be scaled, stretched and parametrized.

This function is going to give us the angle for a given t and the amount of sampling points x. t goes from 0° to 180°. x is the amount of sampling points you want. With the sampling point amound you deside, how many steps the movement must make from start to end, that is i.e. if you go from 1° to 45° in 30 steps you need to set the x to 30.

As we now have the analytical function we need to translate it into a Excel forumla to then calculate each sampling point.

 

The Discreete Profile

To use the integrated sinus square function in your C code you need to have a discreete profile. This data is going to be in a lookup table so the servo controller can use the data to tell the servo to what angle it needs to go next.

Using this function you can use a spreadsheet table to calculate the values.

I use a Google Doc Table to create the data, to get all the nessessary angles. Excel is good to. Use this forumla to calculate the angles from 0° to 45° in 100 steps:

=Round( 100 / Pi() * ( ( Pi() * A1) / 45 - cos( ( Pi() * A1 ) / 45 ) * sin( ( Pi() * A1 ) / 45 ) ) , 0 )


The 100 can be changed to that many steps that are useful to you. The 45° can be changed to what angle the move should go to.

Here more generic with the t value in the A column and the amount of samples in the $D$2 and the end angle in the cell $E$2

=Round( $D$2 / Pi() * ( ( Pi() * A1 ) / $E$2 - cos( ( Pi() * A1 ) / $E$2 ) * sin( ( Pi() * A1 ) / $E$2 ) ) , 0 )

Use this formula for the other way around:

=Round( 180 - ( $D$2 / Pi() * ( ( Pi() *  A1 ) / $E$2 - cos( ( Pi() *  A1 ) / $E$2 ) * sin( ( Pi() *  A1 ) / $E$2 ) ) ) , 0 )


The Software

For simplicity this sketch turns only the head from 0° to 180° and then stops. To re-run the sketch press the Arduino reset button.

This example uses Timer2. As you might know I don't like the delay() function. The Timer2 lets me run a timer that calls the move() function periodically after a duration. This keeps the main-loop free to do the behaviors and stay reactive.

Further you find a lot of functions in the code. This is due the Single Level of Abstraction Principle. And an intension revealing method-name I like a lot more than // comments . So methods can be used to make self-explaining code. 

A second example is in the Appendix A. The second example uses the delay and no timer.

/* 
 * Smooth servo rotation using a sinus square function.
 * more infos: http://letsmakerobots.com/node/31697
 * created by NilsB 
 */

#include <MsTimer2.h>
#include <Servo.h> 

const int timerInMillies = 60;
const int countSinusSquareLookupTableEntries = 34;
const int sinusSquareLookupTable[] = {
  0,0,0,0,1,2,
  5,8,12,17,24,31,
  40,49,60,71,82,93,
  105,116,126,136,145,153,
  160,166,171,174,177,179,
  180,180,180,180
};

int currentValueIndex = 0;
const int movmentIndicatorPin = 13;

Servo headServo;

/*
* This is the function that gets called periodically.
*/
void move()
{
  moveServoTo(angle());

  indicateMovement();
  
  incrementOrStop();
}

void moveServoTo(int angle){
  Serial.println(angle);
  headServo.write(angle); 
}

int angle(){
  return sinusSquareLookupTable[currentValueIndex];
}

void indicateMovement()
{
  static boolean output = HIGH;
  
  digitalWrite(movmentIndicatorPin, output);
  output = !output;
}

void incrementOrStop(){
  currentValueIndex++;
  if(currentValueIndex == countSinusSquareLookupTableEntries - 1){
    stopMove();
  }
}

void stopMove(){
  MsTimer2::stop();
  headServo.detach();
}

void setupMovementIndicator(){
  Serial.begin(9600);
  pinMode(movmentIndicatorPin, OUTPUT);
}

void setupTimer2(){
  MsTimer2::set(timerInMillies, move);
  MsTimer2::start();
}

void setupServo(){  
  headServo.attach(6);
  headServo.write(0); 
}

void setup(){
  setupMovementIndicator();
  setupServo();  
  setupTimer2();
}

/*
* The loop method is empty and can be used to
* read sensor data and feed the behaviors.
*/
void loop(){;}

Comparison to the Linear Ramp

In the comments was asked about the ramp function. The ramp function is used to make linear movements. The first part of the move the velocity increases by a fixed increment delta. In the middle part the velocity stays the same. In the third part the velocity decreases again to zero.

When we compare the velocity over time V(t) and acceleration over time a(t) diagrams we see that the Sinus Square V(t) from this tip has a acceleration transition. The ramp V(t) has a peak. This peak is hearable. You hear noise from the power transmission of the motor to the gears. On every orange dot you should hear this noise. This may slowly destroy the gear since with every move there are four times the gears crash into each other... While for our little light microservos that carry a ultrasonic range sensor only this crashes are not so dramatic. But when you build a heavier head like a steampunk metal head or even a plastic head with microcontroller and sensors the mass inertia starts to come in and Newton's Laws of Physics.

But I must agree compound ramp functions are very easy to understand and applicable without mathematics. It's your choice to smoothen things and accept higher level complexity or less smooth with lower complexity.

Sinus Square Lookup Tables

Here you have some useful lookup tables. You find one that goes from 0° to 180° and the other way around. And the same with a higher precision.

tyfrom 0° to 180°inversefrom 0° to 90°inversefrom 90° to 180°inversefrom 90° to 135°inversefrom 90° to 45°inverse
00018009090180901359045
10.000203066018009090180901359045
20.00162423018009090180901359045
30.005480108018009090180901359045
40.012984347018009090180901359045
50.02534615018009090180901359045
60.043768802018009090180911348946
70.069448205018009090180911348946
80.103571418018009090180921338847
90.147315212018018991179921338847
100.201844639018018991179931328748
110.26831161018018991179941318649
120.347853489018018991179951308550
130.441591714018028892178961298451
140.550630425117928892178971288352
150.676055122117938793177991268154
160.8189313381179387931771001258055
170.9803033491179486941761021237857
181.1611928921179486941761041217659
191.3625979281179585951751061197461
201.5854914212178684961741081177263
211.8308201562178783971731101157065
222.0995035852178882981721121136867
232.3924327022178981991711131126768
242.710468967317710801001701151106570
253.054443245317711791011691171086372
263.425154804317712781021681191066174
273.823370334417613771031671211045976
284.249823017417615751051651231025778
294.705211633517516741061641251005580
305.19019970651751872108162126995481
315.70541469961741971109161128975283
326.25144724861742169111159129965184
336.82885044271732268112158130955085
347.4381391571732466114156131944986
358.07978939581722664116154132934887
368.75423776991712862118152133924788
379.46188090891712961119151133924788
3810.203075101703159121149134914689
3910.97813537111693357123147134914689
4011.78733606121683555125145135904590
4112.63090954131673753127143135904590
4213.50904638141663951129141135904590
4314.42189506141664149131139    
4415.36956176151654347133137    
4516.35211024161644545135135    
4617.36956176171634743137133    
4718.42189506181624941139131    
4819.50904638201605139141129    
4920.63090954211595337143127    
5021.78733606221585535145125    
5122.97813537231575733147123    
5224.203075241565931149121    
5325.46188091251556129151119    
5426.75423777271536228152118    
5528.07978939281526426154116    
5629.43813915291516624156114    
5730.82885044311496822158112    
5832.25144725321486921159111    
5933.7054147341467119161109    
6035.19019971351457218162108    
6136.70521163371437416164106    
6238.24982302381427515165105    
6339.82337033401407713167103    
6441.4251548411397812168102    
6543.05444324431377911169101    
6644.71046897451358010170100    
6746.39243274613481917199    
6848.099503584813282817298    
6949.830820165013083717397    
7051.585491425212884617496    
7153.362597935312785517595    
7255.161192895512586417694    
7356.980303355712386417694    
7458.818931345912187317793    
7560.676055126111987317793    
7662.550630436311788217892    
7764.441591716411688217892    
7866.347853496611489117991    
7968.268311616811289117991    
8070.201844647011089117991    
8172.147315217210889117991    
8274.103571427410690018090    
8376.069448217610490018090    
8478.04376887810290018090    
8580.025346158010090018090    
8682.01298435829890018090    
8784.00548011849690018090    
8886.00162423869490018090    
8988.000203078892        
90909090        
9191.999796939288        
9293.998375779486        
9395.994519899684        
9497.987015659882        
9599.9746538510080        
96101.956231210278        
97103.930551810476        
98105.896428610674        
99107.852684810872        
100109.798155411070        
101111.731688411268        
102113.652146511466        
103115.558408311664        
104117.449369611763        
105119.323944911961        
106121.181068712159        
107123.019696712357        
108124.838807112555        
109126.637402112753        
110128.414508612852        
111130.169179813050        
112131.900496413248        
113133.607567313446        
114135.28953113545        
115136.945556813743        
116138.574845213941        
117140.176629714040        
118141.75017714238        
119143.294788414337        
120144.809800314535        
121146.294585314634        
122147.748552814832        
123149.171149614931        
124150.561860815129        
125151.920210615228        
126153.245762215327        
127154.538119115525        
128155.79692515624        
129157.021864615723        
130158.212663915822        
131159.369090515921        
132160.490953616020        
133161.578104916218        
134162.630438216317        
135163.647889816416        
136164.630438216515        
137165.578104916614        
138166.490953616614        
139167.369090516713        
140168.212663916812        
141169.021864616911        
142169.79692517010        
143170.53811911719        
144171.24576221719        
145171.92021061728        
146172.56186081737        
147173.17114961737        
148173.74855281746        
149174.29458531746        
150174.80980031755        
151175.29478841755        
152175.7501771764        
153176.17662971764        
154176.57484521773        
155176.94555681773        
156177.2895311773        
157177.60756731782        
158177.90049641782        
159178.16917981782        
160178.41450861782        
161178.63740211791        
162178.83880711791        
163179.01969671791        
164179.18106871791        
165179.32394491791        
166179.44936961791        
167179.55840831800        
168179.65214651800        
169179.73168841800        
170179.79815541800        
171179.85268481800        
172179.89642861800        
173179.93055181800        
174179.95623121800        
175179.97465391800        
176179.98701571800        
177179.99451991800        
178179.99837581800        
179179.99979691800        

 

Further Readings

http://www.wolframalpha.com/input/?i=plot%28sin%28t%29%5E2%2Ct%2C0%2CPi%2F2%29
   plot( sin( t )^2, t, 0,Pi/2 )

http://www.wolframalpha.com/input/?i=plot%28integrate%28sin%28t%29%5E2%29%2Ct%2C0%2CPi%29
   plot( integrate( sin( t )^2 ), t, 0, Pi)

http://www.wolframalpha.com/input/?i=%281%2F2*%28t%2F180*Pi+-+Sin%28t%2F180*Pi%29*cos%28t%2F180*Pi%29%29%29%2F1.57*180
   (1/2*(t/180*Pi - Sin(t/180*Pi)*cos(t/180*Pi)))/1.57*180

http://www.wolframalpha.com/input/?i=x%2FPi+*+%28%28%CF%80+t%29%2F180-cos%28%28%CF%80+t%29%2F180%29+sin%28%28%CF%80+t%29%2F180%29%29
   x/Pi * ((π t)/180-cos((π t)/180) sin((π t)/180))

http://books.google.ch/books?id=_VfzuBute1cC&lpg=PA183&ots=JmxoR_MWsV&dq=sinus%20quadrat%20regelungstechnik&hl=de&pg=PA183#v=onepage&q&f=false
   "Regelungstechnik. Erweiterungen der Regelungsstruktur."

http://letsmakerobots.com/node/28278
   "This tutorial shows the use of timers and interrupts for Arduino boards."

http://arduino.cc/playground/Main/MsTimer2
   "MsTimer2 is a small and very easy to use library to interface Timer2 on the ATmega168/328"

http://www.scribd.com/doc/49262022/Clean-Code-Cheat-Sheet-V1-3
   "Clean Code Development Cheat Cheat"

 

Appendix A:

This is an alternative implementation that uses the delay() function in the main loop.

 

/* 
 * Smooth servo rotation using a sinus square function.
 * ATTENTION: THIS SKETCH USES THE DELAY FUNCTION
 * More infos: http://letsmakerobots.com/node/31697
 * created by NilsB 
 */

#include <Servo.h> 

const int timerInMillies = 20;
const int countSinusSquareLookupTableEntries = 181;
const int sinusSquareLookupTable[] = {
0,0,0,0,0,0,0,0,0,
0,0,0,0,0,1,1,1,1,
1,1,2,2,2,2,3,3,3,
4,4,5,5,6,6,7,7,8,
9,9,10,11,12,13,14,14,15,
16,17,18,20,21,22,23,24,25,
27,28,29,31,32,34,35,37,38,
40,41,43,45,46,48,50,52,53,
55,57,59,61,63,64,66,68,70,
72,74,76,78,80,82,84,86,88,
90,92,94,96,98,100,102,104,106,
108,110,112,114,116,117,119,121,123,
125,127,128,130,132,134,135,137,139,
140,142,143,145,146,148,149,151,152,
153,155,156,157,158,159,160,162,163,
164,165,166,166,167,168,169,170,171,
171,172,173,173,174,174,175,175,176,
176,177,177,177,178,178,178,178,179,
179,179,179,179,179,180,180,180,180,
180,180,180,180,180,180,180,180,180,
180
};

const int movmentIndicatorPin = 13;

Servo headServo;

void setup(){
  setupMovementIndicator();
  setupServo();  
}

void loop(){
  move();
  wait();
}

/*
* This is the function that gets called periodically.
*/
void move()
{
  for(int angleIndex = 0; angleIndex < countSinusSquareLookupTableEntries; angleIndex++){
    moveServoTo(angle(angleIndex));
    indicateMovement();
    wait();
  }
  
  waitLong();
  for(int angleIndex = countSinusSquareLookupTableEntries-1; angleIndex >= 0; angleIndex--){
    moveServoTo(angle(angleIndex));
    indicateMovement();
    wait();
  }
  
  waitLong();
}

void moveServoTo(int angle){
  Serial.println(angle);
  headServo.write(angle); 
}

int angle(int index){
  return sinusSquareLookupTable[index];
}

void indicateMovement()
{
  static boolean output = HIGH;
  
  digitalWrite(movmentIndicatorPin, output);
  output = !output;
}

void stopMove(){
  headServo.detach();
}

void setupMovementIndicator(){
  Serial.begin(9600);
  pinMode(movmentIndicatorPin, OUTPUT);
}

void setupServo(){  
  headServo.attach(6);
  headServo.write(0); 
}

void wait(){
  delay(timerInMillies);
}

void waitLong(){
  delay(5*timerInMillies);
}

 

 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

I've collected this so I can read it again when I'm not half asleep. Very interesting post.

So. . .this could also be used for drive motors to ramp up to speed without a jerky start?

Instead of going from 0 to 180 and back, you'd want to scale it to the PWM lower and upper limits of 0 to 255. 

Unless you are using continuous rotation servos, in which case it'd be perfect as it is.

Yes. The sinus square can be used for this too. So your Houston would not shake due to mass inertia when it starts to move, and not shake when it makes a normal stop. In this tip I wanted to stay focused on servo rotations. But indeed the topic is not so narrow.

Heh, you're a mind reader NilsB.  Nothing would please me more than to have Houston's first "steps" be smooth and graceful.  I figured he was going to look like the shaking tower of robot.

OK. Question: Say I want to use this method with legged robots and all mevement is related to the servo center position. All movement angles are expressed as center+angle or center-angle. How do I get the index to start the movement from 90 (center) and move to 115 (center+25) for example? There must be something simple that eludes me at this hour...

As I understand your question you ask for the correct profile and the index strategy, right?

Find your profile. To get this profile I used the periodicity of the sinus square and squeezed the integral of it from 0° to 25°. The periodical sinus gives you now your data from -25° via 0° to 25°. Next I lifted this wave up to 65° by adding 65 to the formula. The result is visualized in the picture above. The vertical axis is angle, the horizontal axis is time/steps.

I have chosen 25 steps from 65° to 90° and 25 from 90° to 115°.

Here is the Google Doc Table formula:  A2 is a cell with one discreete x and the A column contains data [0...50]

=Round( 25 / Pi() * ( Pi() * A2 / 25 - Cos( Pi() * A2 / 25) * Sin( Pi() * A2 / 25)) + 65 ,0)

Find the index. So now you get a list of 50 samples. If you move from 90° to 115° and want to know each angle then use the (50/2 + index) to lookup the angle at the given index. If you go from 90° to 65° use the (50/2 - index) for lookup. I assume index in each loop [0..25].

Hi,

   I had not thought of this before reading your post, but this will also make the movement of small tracked robots more realistic, I plan to encorporate a simple 'easing' into my first robot so that it does not start and stop every move at full speed or full stop. It will hopefully look like something more substantial, as if it has weight and momentum.

Thanks for sharing

Duane.

rcarduino.blogspot.com

 

 

 

There are two formulas to allow accel/deaccel over time:

f(x) = (x^2)/(200*t/200) //accel
f(x) = (-((x-t)^2)/(200*t/200)+100 //deaccel

All that will need to be passed is time to target and target.
The function will calculate percentages of target and write them to a servo.

void smoothMove(float timeToTarget, int target, int servoToMove) {
int accel, deaccel;

for (int i = 0; i <= timeToTarget/2; i++) {
accel = (i^2)/(200*target/200);
analogWrite(servoToMove, accel * target);
}

for (int i = timeToTarget/2; i <= timeToTarget; i++) {
deaccel = (-((i-target)^2)/(200*target/200)+100;
analogWrite(servoToMove, deaccel * target);
}
}

I don't know if this will be better/worse/different. Just thought I would share.

PS: I only worked out the formulas on a graphing program. The curves looked right. I don't know if they would work.

PPS: I am going to have to rework all of this. I just is not coming out as I remember it doing.

Now how can we inject a Random function into this code to make 2 servos move smoothly but randomly in both directions ? the aim is to replicate human head movement in 2 axis?