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.

[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!

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

[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.

[EricVanWyk]

Originally Posted by Kevin Sevcik

Bomber,

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.

I don't want to belabor the point too heavily, but that future is currently in boxes on its way to the beta test teams. I'm going to follow with a lot of boring technical blah blah blah, but the conclusion is pretty simple to follow:

DON'T OVER OPTIMIZE

A fixed point multiply takes two or three clock cycles to execute.
A single precision floating point multiply takes three clock cycles to execute.
A double precision floating point multiply takes four clock cycles to execute.

In addition to the "execute" phase, each instruction must also go through fetch, decode/dispatch, and complete, which adds three more clock cycles.

Simply put, the difference between single fixed or floating instructions just don't matter any more. What will matter is how many instructions. By converting to fixed point math (and adding the shift instructions), you actually slowed down the filter.

To make things even more interesting, the processor is capable of dispatching up to 2 instructions per clock, but only if they are different types of instructions: You can dispatch a fixed point and a floating point instruction simultaneously, but you can't do the same with two fixed point instructions.

This means that you are *much* better off allowing a compiler to order your instructions. Write what you mean, and trust in the compiler.


- Eric (with a C)

PS:
Eliminating divides are almost worth your time.
Fixed point divides take 20 cycles to execute.
Floating point divides take 18 (single precision) or 33 (double precision) cycles to execute. For reference sake, a double precision divide on the cRIO takes about the same amount of time as an 8 bit fixed point instruction on the PIC. However, it will also be executing fixed point math while the FPU is working on the divide.

[Kevin Sevcik]

Originally Posted by EricVanWyk

I don't want to belabor the point too heavily, but that future is currently in boxes on its way to the beta test teams. I'm going to follow with a lot of boring technical blah blah blah, but the conclusion is pretty simple to follow.

Eric (sorry about the k),

To be brief, I wasn't maintaining that fixed point math is faster or would make more sense for most operations on the cRIO. NI's processor selection obviously makes that untrue. Two points remain, however:

1. At some point, we'll hopefully be given access to the FPGA behind the RIO in cRIO. Floating point math is more or less untenable on this FPGA. If a team wants to do fast filtering, averaging, oversampling, etc. with no extra processor load, fixed-point on the FPGA is the way to go. Many interesting applications on the FPGA are going to require some sort of fixed point math, really.

2. Fixed point math is loads faster on processors designed with it in mind and such processors consume less power to do the same amount of work as a floating point processor. I was attempting to point this out so everyone reading the thread didn't simply consign fixed-point to the dustbin of history. There's a reason the big DSP makers are still designing and making new fixed-point DSPs, after all.

[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.

[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.