# 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.
* 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;

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

indicateMovement();

incrementOrStop();
}

void moveServoTo(int angle){
Serial.println(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();
}

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

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

void setupServo(){
}

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.

 t y from 0° to 180° inverse from 0° to 90° inverse from 90° to 180° inverse from 90° to 135° inverse from 90° to 45° inverse 0 0 0 180 0 90 90 180 90 135 90 45 1 0.000203066 0 180 0 90 90 180 90 135 90 45 2 0.00162423 0 180 0 90 90 180 90 135 90 45 3 0.005480108 0 180 0 90 90 180 90 135 90 45 4 0.012984347 0 180 0 90 90 180 90 135 90 45 5 0.02534615 0 180 0 90 90 180 90 135 90 45 6 0.043768802 0 180 0 90 90 180 91 134 89 46 7 0.069448205 0 180 0 90 90 180 91 134 89 46 8 0.103571418 0 180 0 90 90 180 92 133 88 47 9 0.147315212 0 180 1 89 91 179 92 133 88 47 10 0.201844639 0 180 1 89 91 179 93 132 87 48 11 0.26831161 0 180 1 89 91 179 94 131 86 49 12 0.347853489 0 180 1 89 91 179 95 130 85 50 13 0.441591714 0 180 2 88 92 178 96 129 84 51 14 0.550630425 1 179 2 88 92 178 97 128 83 52 15 0.676055122 1 179 3 87 93 177 99 126 81 54 16 0.818931338 1 179 3 87 93 177 100 125 80 55 17 0.980303349 1 179 4 86 94 176 102 123 78 57 18 1.161192892 1 179 4 86 94 176 104 121 76 59 19 1.362597928 1 179 5 85 95 175 106 119 74 61 20 1.585491421 2 178 6 84 96 174 108 117 72 63 21 1.830820156 2 178 7 83 97 173 110 115 70 65 22 2.099503585 2 178 8 82 98 172 112 113 68 67 23 2.392432702 2 178 9 81 99 171 113 112 67 68 24 2.710468967 3 177 10 80 100 170 115 110 65 70 25 3.054443245 3 177 11 79 101 169 117 108 63 72 26 3.425154804 3 177 12 78 102 168 119 106 61 74 27 3.823370334 4 176 13 77 103 167 121 104 59 76 28 4.249823017 4 176 15 75 105 165 123 102 57 78 29 4.705211633 5 175 16 74 106 164 125 100 55 80 30 5.190199706 5 175 18 72 108 162 126 99 54 81 31 5.705414699 6 174 19 71 109 161 128 97 52 83 32 6.251447248 6 174 21 69 111 159 129 96 51 84 33 6.828850442 7 173 22 68 112 158 130 95 50 85 34 7.43813915 7 173 24 66 114 156 131 94 49 86 35 8.079789395 8 172 26 64 116 154 132 93 48 87 36 8.754237769 9 171 28 62 118 152 133 92 47 88 37 9.461880908 9 171 29 61 119 151 133 92 47 88 38 10.203075 10 170 31 59 121 149 134 91 46 89 39 10.97813537 11 169 33 57 123 147 134 91 46 89 40 11.78733606 12 168 35 55 125 145 135 90 45 90 41 12.63090954 13 167 37 53 127 143 135 90 45 90 42 13.50904638 14 166 39 51 129 141 135 90 45 90 43 14.42189506 14 166 41 49 131 139 44 15.36956176 15 165 43 47 133 137 45 16.35211024 16 164 45 45 135 135 46 17.36956176 17 163 47 43 137 133 47 18.42189506 18 162 49 41 139 131 48 19.50904638 20 160 51 39 141 129 49 20.63090954 21 159 53 37 143 127 50 21.78733606 22 158 55 35 145 125 51 22.97813537 23 157 57 33 147 123 52 24.203075 24 156 59 31 149 121 53 25.46188091 25 155 61 29 151 119 54 26.75423777 27 153 62 28 152 118 55 28.07978939 28 152 64 26 154 116 56 29.43813915 29 151 66 24 156 114 57 30.82885044 31 149 68 22 158 112 58 32.25144725 32 148 69 21 159 111 59 33.7054147 34 146 71 19 161 109 60 35.19019971 35 145 72 18 162 108 61 36.70521163 37 143 74 16 164 106 62 38.24982302 38 142 75 15 165 105 63 39.82337033 40 140 77 13 167 103 64 41.4251548 41 139 78 12 168 102 65 43.05444324 43 137 79 11 169 101 66 44.71046897 45 135 80 10 170 100 67 46.3924327 46 134 81 9 171 99 68 48.09950358 48 132 82 8 172 98 69 49.83082016 50 130 83 7 173 97 70 51.58549142 52 128 84 6 174 96 71 53.36259793 53 127 85 5 175 95 72 55.16119289 55 125 86 4 176 94 73 56.98030335 57 123 86 4 176 94 74 58.81893134 59 121 87 3 177 93 75 60.67605512 61 119 87 3 177 93 76 62.55063043 63 117 88 2 178 92 77 64.44159171 64 116 88 2 178 92 78 66.34785349 66 114 89 1 179 91 79 68.26831161 68 112 89 1 179 91 80 70.20184464 70 110 89 1 179 91 81 72.14731521 72 108 89 1 179 91 82 74.10357142 74 106 90 0 180 90 83 76.06944821 76 104 90 0 180 90 84 78.0437688 78 102 90 0 180 90 85 80.02534615 80 100 90 0 180 90 86 82.01298435 82 98 90 0 180 90 87 84.00548011 84 96 90 0 180 90 88 86.00162423 86 94 90 0 180 90 89 88.00020307 88 92 90 90 90 90 91 91.99979693 92 88 92 93.99837577 94 86 93 95.99451989 96 84 94 97.98701565 98 82 95 99.97465385 100 80 96 101.9562312 102 78 97 103.9305518 104 76 98 105.8964286 106 74 99 107.8526848 108 72 100 109.7981554 110 70 101 111.7316884 112 68 102 113.6521465 114 66 103 115.5584083 116 64 104 117.4493696 117 63 105 119.3239449 119 61 106 121.1810687 121 59 107 123.0196967 123 57 108 124.8388071 125 55 109 126.6374021 127 53 110 128.4145086 128 52 111 130.1691798 130 50 112 131.9004964 132 48 113 133.6075673 134 46 114 135.289531 135 45 115 136.9455568 137 43 116 138.5748452 139 41 117 140.1766297 140 40 118 141.750177 142 38 119 143.2947884 143 37 120 144.8098003 145 35 121 146.2945853 146 34 122 147.7485528 148 32 123 149.1711496 149 31 124 150.5618608 151 29 125 151.9202106 152 28 126 153.2457622 153 27 127 154.5381191 155 25 128 155.796925 156 24 129 157.0218646 157 23 130 158.2126639 158 22 131 159.3690905 159 21 132 160.4909536 160 20 133 161.5781049 162 18 134 162.6304382 163 17 135 163.6478898 164 16 136 164.6304382 165 15 137 165.5781049 166 14 138 166.4909536 166 14 139 167.3690905 167 13 140 168.2126639 168 12 141 169.0218646 169 11 142 169.796925 170 10 143 170.5381191 171 9 144 171.2457622 171 9 145 171.9202106 172 8 146 172.5618608 173 7 147 173.1711496 173 7 148 173.7485528 174 6 149 174.2945853 174 6 150 174.8098003 175 5 151 175.2947884 175 5 152 175.750177 176 4 153 176.1766297 176 4 154 176.5748452 177 3 155 176.9455568 177 3 156 177.289531 177 3 157 177.6075673 178 2 158 177.9004964 178 2 159 178.1691798 178 2 160 178.4145086 178 2 161 178.6374021 179 1 162 178.8388071 179 1 163 179.0196967 179 1 164 179.1810687 179 1 165 179.3239449 179 1 166 179.4493696 179 1 167 179.5584083 180 0 168 179.6521465 180 0 169 179.7316884 180 0 170 179.7981554 180 0 171 179.8526848 180 0 172 179.8964286 180 0 173 179.9305518 180 0 174 179.9562312 180 0 175 179.9746539 180 0 176 179.9870157 180 0 177 179.9945199 180 0 178 179.9983758 180 0 179 179.9997969 180 0

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))

"Regelungstechnik. Erweiterungen der Regelungsstruktur."

https://www.robotshop.com/letsmakerobots/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
* 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;

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);
}

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

void indicateMovement()
{
static boolean output = HIGH;

digitalWrite(movmentIndicatorPin, output);
output = !output;
}

void stopMove(){
}

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

void setupServo(){
}

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?