Servo 'smoothing'

[Andy A.]

Ok guys, heres an off season question-

I'm a mechanical engineer who's gotten roped into programming. I'm using a 2008 IFI control system to control a small robot during it's prototyping stages. Later on we'll use a much simplified embedded controller, but right now I like the IFI equipment because it was cheap, familiar to me and offered all the capability I needed.

I am controlling a servo powered 'pan/tilt' platform, sometimes under scripted routines and sometimes under 'manual' control with some pots. I've run into a big 'jitter' problem. I'm assuming the pot's being used on the OI side of things float around a bit and that variation shows up in the servos as jitter (bounces around a position rapidly and seemingly randomly). I know that the servos them selves are fine because if I generate the pwm value in code they hold position with no jitter.

I'm stuck with the pots and servos I have, but I need to smooth out these jitters so the platform I'm moving is steady even when under 'manual' control. So I am I right assuming I need some sort of algorithm that takes the analog inputs from my crummy pots and smooths them out. Or maybe a function that checks my input value, evaluates it for a delta and only updates the PWM signal if that delta exceeds a value? If I can eliminate the jitter with out any loss of speed or range in the servo, fantastic.

Right now the pwm values are hooked directly to the analog inputs. I have some functions that limit their range and add a dead band, but none of my smoothing code attempts have done any good.

As a good friend of mine once said about my programming ability '(I) know just enough to be dangerous'. I'm really out of my clueless when it comes to PID loops, look up tables, interrupts and advanced stuff like that. My gut tells me there is a really simple way to do this, but anything beyond programming my coffee maker is over my head. So, small words and patience are always appreciated when I venture into this forum.

Thanks guys. You never appreciate your teams programmers till you don't have one around anymore. I know I'm missing them now.

-Andy A.

[Alan Anderson]

Originally Posted by Andy A.

I'm assuming the pot's being used on the OI side of things float around a bit and that variation shows up in the servos as jitter (bounces around a position rapidly and seemingly randomly). I know that the servos them selves are fine because if I generate the pwm value in code they hold position with no jitter.

I'll take your word for it that the problem is with the input, but if I were working on it myself I'd be printing out the OI values just to make extra certain that I was addressing the correct issue.

So I am I right assuming I need some sort of algorithm that takes the analog inputs from my crummy pots and smooths them out...My gut tells me there is a really simple way to do this,...

There is indeed a really simple way to do this: just read a bunch of input values and average them.

You can do that in several ways. The easiest is something like this:

// DISCLAIMER: THIS CODE IS UNTESTED

// declare some variables at the beginning of the code
char incount = 0; // this will count the input values being accumulated
int accumulator = 0; // this will accumulate input values
char smooth = 128; // this will hold the smoothed input value

// ....

// do this each time a new OI value is available
  accumulator += (int)potval; // replace 'potval' as appropriate, e.g. p1_aux
  if( incount++ == 8 ) // is this the eighth sample?
  {
    smooth = accumulator << 3; // a quick and sneaky way to divide by 8
    incount = 0; // set to count another eight samples
    accumulator = 0; // reset accumulator
  }

Now you can use smooth as your input value. It will only change once every eight samples, or about five times per second. If it's still too noisy, you can increase to 16 samples at the cost of even slower response. If you need it to be quicker, there are other ways to do it which take more code and more temporary storage, but which begin to respond more immediately. Filtering is always a tradeoff between stability and response time.

[BornaE]

You can average the past N analog samples and feed it to the pwm, this will add a little amount of lag but smooths the movement

your current configuration:
(i might be a bit off with the aliases names but you get the idea.)

pwmXX = Get_Analog_Value(ana_in_yy)/4;

now try this:
averaging 5 samples

static int temp[5]=0;

temp[4] = temp[3];
temp[3] = temp[2];
temp[2] = temp[1];
temp[1] = temp[0];
temp[0] = Get_Analog_Value(ana_in_yy);

pwmXX = (temp[0] + temp[1] + temp[2] + temp[3] + temp[4])/(5*4);

the more samples you average, the smoother the moviement but the longer the lag.

[rwood359]

Originally Posted by Alan Anderson

// DISCLAIMER: THIS CODE IS UNTESTED

// declare some variables at the beginning of the code
char incount = 0; // this will count the input values being accumulated
int accumulator = 0; // this will accumulate input values
char smooth = 128; // this will hold the smoothed input value

// ....

// do this each time a new OI value is available
  accumulator += (int)potval; // replace 'potval' as appropriate, e.g. p1_aux
  if( incount++ == 8 ) // is this the eighth sample?
  {
    smooth = accumulator << 3; // a quick and sneaky way to divide by 8
    incount = 0; // set to count another eight samples
    accumulator = 0; // reset accumulator
  }

Shouldn't it be:
smooth = accumulator >> 3; // a quick and sneaky way to divide by 8

[EricVanWyk]

0705290 has given an example of a FIR Filter. Basically, the output is the weighted sum of past readings. With 07's code, you can make all sorts of filters by changing the weights.

I also like using IIR filters. These hold state a little differently, and I find the simple low pass IIR the easiest to tune.

tau = .75;  //This is the tuning variable
pwm_out = tau*sensor_in + (1-tau)*old_pwm_out;
old_pwm_out = pwm_out;

This does a passable job of modeling a resistor - capacitor filter - tau is the equivalent of the RC time constant, ish.

To tune the filter, you can link tau to a wheel input. Play with it until it feels right, and record it.

Also, I did this in floating point math, converting it to fixed shouldn't be too hard.

Have fun!

[BornaE]

This would impreve your responsivness: (I just tought of it, not tested before)

static int temp[5]=0;

temp[4] = temp[3];
temp[3] = temp[2];
temp[2] = temp[1];
temp[1] = temp[0];
temp[0] = Get_Analog_Value(ana_in_yy);

pwmXX = (temp[0]/2 + temp[1]/4 + temp[2]/8 + temp[3]/16 + temp[4]/32)/4;

[Kevin Sevcik]

Erik,

Translating your IIR to fixed point doesn't seem entirely straightforward to me. Or, at least, we should remind the audience of the wonderful effects known as truncation and loss of precision. You'd need to process your new value and carry your old value as scaled integers, or you'd lose everything behind the decimal point and get weird jumps in your values. So it'd look something like:

#define SCL 4  //4 gives you 1/2^4th (1/16th) adjustments in tau
tau = 12;  //This is the tuning variable
old_sensor_in = tau*(sensor_in<<SCL) + ((1<<SCL)-tau)*old_sensor_in;
old_sensor_in >>= SCL;
pwm_out = old_sensor_in >> SCL;

With typecasting as appropriate and always remembering that MPLAB doesn't support signed shifting, so this only works for unsigned numbers.

To 0705920,

I've always shied from the brute force style of moving averages, as your processing time increases quadratically with the number of samples you want to take, and that doesn't really sit well with me. Plus attempts to increase your number of samples quickly get out of hand. My preference is something like:

#define AVG_SAMPLES 8

static int temp[AVG_SAMPLES] = 0;
static int accumulator = 0;
static char count = 0;
static int smooth = 0;

accumulator -= temp[count];
temp[count] = Get_Analog_Value(ana_in_yy);
accumulator += temp[count];
smooth = accumulator / AVG_SAMPLES;
count += 1;
count = (count >= AVG_SAMPLES) ? 0 : count;

With the caveat that you must always make certain that accumulator and the temp array start out with zero values and that they don't get out of sync. If you clear the accumulator without clearing the array, you subtract old values from zero and basically start measuring the difference between the average when you cleared the accumulator and your current average. (Not a bad way of canceling an offset, though.) Clearing the array and not the accumulator gives the opposite problem.

And of course if you're sticking with strict powers of 2, there's several optimizations you can make to speed things up. (Hint: Division is evil, and XOR is, in fact, useful for math occasionally) But I think your main benefit comes from not adding 32, 64, or 128 numbers in every cycle.

[Bomberofdoom]

Originally Posted by Alan Anderson

// DISCLAIMER: THIS CODE IS UNTESTED

// declare some variables at the beginning of the code
char incount = 0; // this will count the input values being accumulated
int accumulator = 0; // this will accumulate input values
char smooth = 128; // this will hold the smoothed input value

// ....

// do this each time a new OI value is available
  accumulator += (int)potval; // replace 'potval' as appropriate, e.g. p1_aux
  if( incount++ == 8 ) // is this the eighth sample?
  {
    smooth = accumulator << 3; // a quick and sneaky way to divide by 8
    incount = 0; // set to count another eight samples
    accumulator = 0; // reset accumulator
  }

Does the incount++ action actually happen? I mean, when you check if the "if" statment is true, does the Microprocessor increase the valuce of incount? If it doesn't work, this should be done in a for loop, but if this "if" statment counts as a loop, then OK (it's just a method of a loop i've never seen before ).

[EricVanWyk]

Originally Posted by Kevin Sevcik

Erik,

Translating your IIR to fixed point doesn't seem entirely straightforward to me. Or, at least, we should remind the audience of the wonderful effects known as truncation and loss of precision. You'd need to process your new value and carry your old value as scaled integers, or you'd lose everything behind the decimal point and get weird jumps in your values. So it'd look something like:

Kevin - You are 100% correct in calling me on missing the target audience. Sorry all!

You will have to forgive me, I was working under the assumption that the processor had a built-in 32bit FPU. I'm so glad that >> and << optimizations will soon be a thing of the past. I started on the old PBASIC systems, and quickly learned that some optimizations simply obfuscate code. Now I make sure that the first pass is as easy to read as possible - sweet beautiful pseudo code. The second pass can compile.


Your translation is correct, I think. For those following along at home, Kevin moved tau from the 0..1 domain to the 0..15 domain. This allows him to use fixed point math and the shift operator. I'd move it into the 0..255 domain so you can use a wheel input as tau. If you do this, use 16 bit numbers.

[Kevin Sevcik]

Originally Posted by Bomberofdoom

Does the incount++ action actually happen? I mean, when you check if the "if" statment is true, does the Microprocessor increase the valuce of incount? If it doesn't work, this should be done in a for loop, but if this "if" statment counts as a loop, then OK (it's just a method of a loop i've never seen before ).

Bomber,

As with most things in the IFI RC, a for loop wouldn't work here. Remember that for the vast majority of things you do in the IFI RC, you do something once in one pass of the "slow loop", then wait 26.2ms for the next "slow loop" pass, and then do the next step in the sequence. What we're trying to do is measure the input signal at several different points in time, then average those measurements. If we were to use a for loop, the measurement we'd be taking would be very very close together in time on an analog input, or exactly the same if we're looking at a value from the operator interface. Averaging 8 samples of the same number obviously isn't very helpful!

So, since we want some time to pass, we just record one sample of the value during a single pass of the "slow loop" then wait until the next pass to take another one. In Alan's code, once we've built up 8 of these samples, we average them all together. Then the next time through the slow loop, we start all over again.

So to answer your question, this isn't a traditional for loop like you usually think of them. We have a timed, (nominally) infinitely repeating loop to work inside of, and this if statement lets us do something like a for loop inside this timed loop.

Also, I think Alan meant:
if( ++incount == 8) // is this the eighth sample?
as he's using a zero-based count, so you'd want to stop when count + 1 == 8.

Also to Erik and viewers at home,

The domain of tau is obviously changeable with the #define SCL. To move to a 0..255 range, you'd state #define SCL 8. It's important to note that at that point, you can't have an equivalent of tau=1.0, which basically removes the effect of the filter. Also, fixed point math may be painful at times, but it is and always* will be stupendously faster than floating point. If our audience members are at all interested in programming embedded controllers and/or high speed DSPs in the future, it can't hurt to look into fixed point math and play around with it.

*For the foreseeable future, until FPUs are fast enough that you couldn't possibly want to do things any faster than they can be done with floating point.

[dgitz]

Put your signal line on an o-scope and you can look at the jitter there. I would try then putting a low-pass filter on it to reduce some of the noise. my 2 cents.

[DonRotolo]

Originally Posted by Kevin Sevcik

Averaging 8 samples of the same number obviously isn't very helpful!

But it does make for an easier calculation !!

[Bomberofdoom]

Kevin,

What I meant was does the processor really increment incount by putting that line inside an if statment.

But I presume it does, since eveyone is saying the code is correct.

[rwood359]

Originally Posted by Bomberofdoom

Kevin,

What I meant was does the processor really increment incount by putting that line inside an if statment.

But I presume it does, since eveyone is saying the code is correct.

The table at:
http://www.difranco.net/cop2220/op-prec.htm (see Note 2)
shows the precedence of operators.
In the IF statement, the compare will be evaluated, then incount will be incremented.

[nickmagus]

I've had some experience with jittery pots myself i find the first thing you should do is debug every single thing in that system until you find out exactly what is failing. If your assumption is correct then i suggest you map the jitters and see if your digital position vs actual position follows a pattern. if it does than there is the easiest and best way to fix the problem remap the digital value with a lookup table to match the actual value. if you find that the jitters are indeed white noise and are completely random then i suggest a filter such as the ones proposed above (a simple average of the last few values should work fine.) keep in mind that jitters can be caused by all sorts of things not just bad pots i had a magnetic encoder i thought was bad and it turned out that the bolts just above it were causing it to "jitter".