NerdKits - electronics education for a digital generation

You are not logged in. [log in]

NEW: Learning electronics? Ask your questions on the new Electronics Questions & Answers site hosted by CircuitLab.

Microcontroller Programming » Smooth servo movement.

October 26, 2010
by met_fredrik
met_fredrik's Avatar


I have successfully connected a wii nunchuck controller to my nerdkit with the use of Rick_S's code. However like whenever I am controlling servos with an "unstable" input like a sensor or the controller the servos stutter a bit and it looks a bit unprofessional.

I made a little video of my project

How can I program to make the servos kinda slide in the right pwm? Like a smooth movement based on the input I get. Now my pwm_set looks like

    pwm_set(posx *18+400);

So is there possible to make like a loop which smooths out the movement?


October 26, 2010
by Ralphxyz
Ralphxyz's Avatar

I was wondering about something like what you are asking also.

I was thinking of maybe using a average of motions as the nunchuck is so sensitive to the slightest motion

maybe taking the average of ten or hundred movements would make for smoother movements.

I was going to ask Rick about the/a Kalman filter as that might be a better method than just a straight average.


October 26, 2010
by met_fredrik
met_fredrik's Avatar

Yea, if a Kalman filter could do it that would be great. Have been looking into using it with accelerometers, but I can't find any direct examples to learn from.

October 26, 2010
by hevans
(NerdKits Staff)

hevans's Avatar

Hi met_fredrik and Ralph,

First of all, awesome video. There are probably lots of ways you can turn this into a fun and dangerous setup.

The first thing to try to remove the stuttering is to try a simple average over many samples like Ralph suggested. Try printing out the values of posx you are sending to the servo, and just look at them over time to try to figure out the best strategy to kill the jitter. You need to ask yourself if it is actually noise causing the jitter, or is your sensor just good enough that it is picking up the jitter in your thumb on the controller.

A Kalman filter is not a magical filter that gets rid of noise. In fact to use a Kalman filter effectively you need to know a model of what you expect the real system to behave like. You then take your actual measurements and weight them with the expected measurements to get (in theory) a better estimate of the real thing. It is a great solution for estimating the state of a system given a noisy measurement of it, but I think in this case you are just trying to smooth out a jitter of your thumb, and I think a moving average (a low pass filter) will just fine.


October 26, 2010
by met_fredrik
met_fredrik's Avatar


I took a look at the input signals I am getting from the nunchuck. It is in fact these signals that in certain positions tends to spike even when i'm not moving my thumb. I tried to average the input:

                                posy_avg = 0.0;
                for(i=0; i<100; i++) {


This would just enlarge and soften the spikes in a weird way.. Am I going at this the wrong way?

October 26, 2010
by bretm
bretm's Avatar

I'm not sure what nc_data[1] is, but it doesn't look like it changes each time through the loop. So your resulting posy_avg is just nc_data[1] (after dividing by 100.0 one hundred times and multiplying by 100 again).

October 26, 2010
by Rick_S
Rick_S's Avatar

One of the ways to remove the stutter is to limit movement to a change larger than a value.

1st, add a varaible to remember the last position. Then check against that variable with the current position so see if the movement is more than your minimum. So if for instance, you were at the center of the accelerometer (around 500), that would be stored. If you moved slightly, to say 505, nothing would happen but if you move to say 475, it would move. You could make your window as wide as you needed it to be to take out the jitter.

Just my thoughts, but I think it would work...


October 27, 2010
by jbremnant
jbremnant's Avatar

Hi guys

Just a bit to add to this discussion. I think you first need to define the window of time you record your samples. This requires multiple polling to the nunchuck via synchronous I2C using Rick's code. I am not sure how long it takes to get a single polling done on the nunchuck, but assuming I2C bus runs on 400Khz and we use about ~200 clocks to get the data (master-slave init, master-receive 6bytes, ack/nack, delays?), each full read will take about 0.50 msec, if I did the math correctly.

(1/400000) * 200 = 0.00050 seconds -> 0.50 msec

If this is true, you can probably sample 20 times before user feels the delay. ~10msec. But then again, I am not sure about 200 clocks. That was a pure ball park number assuming it takes one rise and one fall of the clock line to transfer a single bit on the data line. Best way is to measure how long it takes to poll the nunchuck in the code.

There are couple of ways to add "smoothing" to your data.

  • moving averages on continuous stream of data, or averages of multiple samples
  • weighted averages: sum( wgt[i] * data[i] ) / sum( wgt[i] ) If you know a certain sample needs more weight than the other values, you'd use this technique. If the data sample was collected over certain number of milliseconds, I'd give higher weight to the sample earliest in the range. So your weight can simply become n - i, where n is the sample size and i is the order in which data was collected.
  • bounding: this technique "truncates" data beyond a certain range. This is exactly as what Rick described. You impose min and max to your data. For example, assuming you have vector v, and you want to limit the values within 200 and 400: v_truncated = max(200, min( 400, v ))
  • z-scoring: if you know historical standard deviation (or variance) in your jitters, you can allow data that's within certain standard deviation from the mean. This is assuming the distribution of your jitters are normally distributed. However, I think this techique might be an overkill.

My suggestion is to poll the nunchuck at least 5 times to get a good average. Keep in mind that you don't want to sample the data too many times, because you still want your controls to be responsive. Collecting multiple samples is at the heart of smoothing. You will need to get reasonable amount of sample points, but not overdo it because responsiveness feedback of user action is in consideration.

Now, to calc the averages, I'd use something along the lines of:

uint8_t get_avgval(uint8 *values, uint_8 n)
 uint_8 i;
 uint_16 xsum;
 for(i=0; i<n; i++)
   xsum += values[i]; 
 return (uint_8) xsum / n;  // casting truncates

or if you have the entire ncdata stored in outer array:

void get_avgvals(uint_8 **ncdatap, uint_8 n, uint8* ncsmooth)
 uint_8 i;
 uint_8 *ncdata;
 uint_16 xsum;
 for(i=0; i<n; i++)
   ncdata = ncdatap[i]; // assumes you built your outer array with necessary pointers
   // smoothing x for example
   xsum += ncdata[0];
   // .. do other variables here, except buttons 
 ncsmooth[0] = xsum / n;
 // again, calc the avg here for other vars

Keep in mind that you need to build the necessary sample data and stick it into an array of some sort with these methods. Or maybe you can do polling and average calc in one go:

// this entire loop needs to execute within users initiating action and feedback on servo
uint16 xsum,ysum,axsum,aysum,azsum,wgtsum;
uint8 xavg,yavg,axavg,ayavg,azavg;
uint8 ncdata[7];
for(i=0; i<samplesize; i++)
  get_ncdata( ncarray ); // polls multiple times.
  xsum  += ncdata[0];    // xsum += i * ncdata[0];  for weighted avg
  ysum  += ncdata[1];
  axsum += ncdata[2]; 
  // ... 
  wgtsum += i;
xavg = (uint_8) xsum / samplesize;   // xavg = xsum / wgtsum; for weighted avg
yavg = (uint_8) ysum / samplesize;

I can think of other ways to do this, but this would probably be sufficient for now.

October 27, 2010
by jbremnant
jbremnant's Avatar

Oops, hit "post" button too soon. Just noticed my datatype declarations were inconsistent. I didn't test the code (I am at work). The uints should be:

uint8_t and uint16_t

We need the edit functionality!

October 27, 2010
by esoderberg
esoderberg's Avatar

If you are looking for working code that averages the wii sensor outputs in order to smooth the data, what I've already posted at the very bottom of the the I2C/TWI thread does this, at least for the gyro outputs.

Post a Reply

Please log in to post a reply.

Did you know that signed numbers need to be sign-extended when chaging variable sizes? Learn more...