moving average within an interrupt

hello, i’m having a little trouble with the moving average code on the playground here: http://arduino.cc/playground/Main/RunningAverage
so i have a robot which needs to take inclinometer readings at a set interval so i used a 6ms timer interrupt. basically what is happening is that when the inclinometer signal spikes, the signal coming out of the averaging filter is zeroing out. I have tried lengthening the interrupt (used to be 4ms) storing less values, hardcoding the filter in my code rather than in a library (i know this is dumb but i’m desperate here) and checking the hardware. I have narrowed the problem to the arithmetic within the filter. The signal coming in is always ok but upon summation, the sum variable gets zeroed out for some unknown reason. Attached is a graph of what i am observing where the incoming signal is black and the averaged signal is blue.

here’s the full code (since i honestly dont know what is causing the problem) and sum is zeroed at the first call within the averageAdd function when value spikes:

#define SELPIN 10 //Selection Pin
#define DATAOUT 11//MOSI
#define DATAIN  12//MISO
#define SPICLOCK  13//Clock

#define TRUE 1
#define FALSE 0

#define PWM2 6
//#define DIR2_1 7
//#define DIR2_2 8

#define PWM1 5
//#define DIR1_1 4
//#define DIR1_2 3

#define SERVO_PWM   3
#define SERVO_DIR1  2
#define SERVO_DIR2  4

#define SERVO_POT A2
#define I_SENSE A1

#define K1 -103.6202
#define K2 -12.9844
#define K3 2.8284
#define K4 -0.3985

#define OUT_MAX 255
#define OUT_MIN -255

#define J 606.05700 //in g*(cm^2)
#define RPM 12309

#define angRate 1371 //in rad/s

#define ts .004 //in s

double X1;   //states
double X2;
double X3;
double X4;

double count, volts;
double u = 0;

double gamma=0;
double tau, h;  //in rad, rad/s
double LastX1;
double LastX3;

unsigned long now;
unsigned long last = 0;

#define AVERAGE_SIZE 20
double averageArray[AVERAGE_SIZE];
double sum=0;
int cnt=0;
int index=0;

void setup(){
  Serial.begin(115200);
  
  for(int i=0;i<AVERAGE_SIZE;i++)averageArray[i]=0;  //initialize averagingArray[]
  
//  pinMode(DIR1_1,OUTPUT); digitalWrite (DIR1_1,LOW);
//  pinMode(DIR1_2,OUTPUT); digitalWrite (DIR1_2,LOW);
  pinMode(PWM1,OUTPUT); digitalWrite (PWM1,LOW);
  
//  pinMode(DIR2_1,OUTPUT); digitalWrite (DIR2_1,LOW);
//  pinMode(DIR2_2,OUTPUT); digitalWrite (DIR2_2,LOW);
  pinMode(PWM2,OUTPUT); digitalWrite (PWM2,LOW);
  
  pinMode (SERVO_PWM,OUTPUT); digitalWrite (SERVO_PWM,LOW);
  pinMode (SERVO_DIR1,OUTPUT); digitalWrite (SERVO_DIR1,LOW);
  pinMode (SERVO_DIR2,OUTPUT); digitalWrite (SERVO_DIR2,LOW);
  analogWrite (SERVO_PWM, 0);
  
  pinMode(SELPIN, OUTPUT);
  pinMode(DATAOUT, OUTPUT);
  pinMode(DATAIN, INPUT);
  pinMode(SPICLOCK, OUTPUT);
  //disable device to start with
  digitalWrite(SELPIN,HIGH);
  digitalWrite(DATAOUT,LOW);
  digitalWrite(SPICLOCK,LOW);
  
//  M1_F();
//  M2_F();
//  delay(1000);

  // using Timer2 breaks D3 & D11 PWM
  cli();          // disable global interrupts
  TCCR1A = 0;     // set entire TCCR1A register to 0
  TCCR1B = 0;     // same for TCCR1B
    
  //ASSR |=(0<<AS2);  //set up using internal clock rather than crystal oscillator
    
 // set compare match register to desired timer count:
  OCR1A = 374;
  TCCR1B |= (1 << WGM12);   // turn on CTC mode:
  TCCR1B |= (1 << CS12);    // Set CS12 bits for 64 prescaler:
  TIMSK1 |= (1 << OCIE1A);  // enable timer compare interrupt:

  sei();          // enable global interrupts
  

  //initialize necessary previous state variables
  count = read_adc(1);
  count = (count/4096)*5;
  count = asin((count-2.5)/2);
  LastX1 = count;
  
  gamma = analogRead(SERVO_POT);
  gamma = (0.0037*gamma)-1.8944;
  LastX3 = gamma;

}

void loop(){
}

void M1_F(){
//  digitalWrite(DIR1_1,LOW);
//  digitalWrite(DIR1_2,HIGH);
  analogWrite(PWM1,255);
  return;
}

void M2_F(){
//  digitalWrite(DIR2_1,LOW);
//  digitalWrite(DIR2_2,HIGH);
  analogWrite(PWM2,255);
  return;
}

ISR(TIMER1_COMPA_vect) {  

    count = read_adc(1);
    count = (count/4096)*5;
    count = asin((count-2.5)/2);
    AverageAdd(count);
    
    gamma = analogRead(SERVO_POT);
    gamma = (0.0037*gamma)-1.8944;
    
    X1 = Average();
    X2 = (X1-LastX1)/ts;
    X3 = gamma;
    X4 = (X3-LastX3)/ts;
   
      u=(K1*X1)+(K2*X2)+(K3*X3)+(K4*X4);
      
      if (u>=1) u=1;
      else if (u<=-1) u=-1;
      u = u*-255;
  
      if (u < 0 && X3 < 0.785) {
        digitalWrite(SERVO_DIR1,HIGH);
        digitalWrite(SERVO_DIR2,LOW);
        analogWrite(SERVO_PWM,abs(u));
      }
      else if(u > 0 && X3 > -0.785){
        digitalWrite(SERVO_DIR1,LOW);
        digitalWrite(SERVO_DIR2,HIGH);
        analogWrite(SERVO_PWM,abs(u));
      }
      else{
        digitalWrite(SERVO_DIR1,LOW);
        digitalWrite(SERVO_DIR2,LOW);
        analogWrite(SERVO_PWM,0);
      }
  
   now = micros();
   last=now;

   LastX1 = X1;
   LastX3 = X3;
    
} 

int read_adc(int channel){
  int adcvalue = 0;
  byte commandbits = B11000000; //command bits - start, mode, chn (3), dont care (3)

  //allow channel selection
  commandbits|=((channel-1)<<3);

  digitalWrite(SELPIN,LOW); //Select adc
  // setup bits to be written
  for (int i=7; i>=3; i--){
    digitalWrite(DATAOUT,commandbits&1<<i);
    //cycle clock
    digitalWrite(SPICLOCK,HIGH);
    digitalWrite(SPICLOCK,LOW);
  }
  //read bits from adc
  for (int i=11; i>=0; i--){
    adcvalue+=digitalRead(DATAIN)<<i;
    //cycle clock
    digitalWrite(SPICLOCK,HIGH);
    digitalWrite(SPICLOCK,LOW);
  }
  digitalWrite(SELPIN, HIGH); //turn off device
  return adcvalue;
}

void AverageAdd(double value)
{
  [color=red]sum -= averageArray[index];[/color]
  averageArray[index]=value;
  sum += value;
  index = (index+1)%AVERAGE_SIZE;
  if (cnt<AVERAGE_SIZE)cnt++;
}

double Average()
{
  if (count ==0) return 0;
  return sum/cnt;
}

untitled.jpg

bump

any information would be helpful at this point. If you need more info, please let me know.

Thanks

I haven't followed what your filter is trying to do, and I can only guess what variables those lines represent in the chart, but it looks to me as if you may be plotting the value of read_adc(1) and count. If so, you may be passing a value greater than 1 to asin(). That's unlikely to give you a useful answer.

I have some doubts:

void AverageAdd(double value)
{
  [color=red]sum -= averageArray[index];[/color]
  averageArray[index]=value;
  sum += value;
  index = (index+1)%AVERAGE_SIZE;
  if (cnt<AVERAGE_SIZE)cnt++;
}

Where do you increasing index variable? What happens when cnt ! isn’t < AVERAGE_SIZE ?
I’d suggest write with less ambiguity in a code, may be this correct, but who can tell for sure:

for (int i=7; i>=3; i--){
    digitalWrite(DATAOUT,commandbits&1<<i);

And one more:

count = read_adc(1);
    count = (count/4096)*5;
    count = asin((count-2.5)/2);
    AverageAdd(count);

Why 4096, there is 12-bit ADC?

I see you using variables in a ISR but aren't using the volatile keyword.

Ok sorry for all the ambiguity in the code, i should have done a better job explaining.
Basically i have an SCA61T-FA1H1G inclinometer running through an mcp3202 (2-channel 12 bit ADC) and i’m just using one channel. The incoming signal is recorded as count then converted into volts then to radians using the asin() function as per the inclinometer’s datasheet.

The signal itself doesn’t seem to be the problem but i will definitely check to see if the asin operand is ever >1. The graph is showing the relationship between count after it has been converted to radians (which is the signal being fed into the averaging filter) and the averaged signal coming out of the filter.

The filter works by simply subtracting the current index’s value from sum adding the incoming value to sum, adding the incoming value in the index’s place in the averageArray then incrementing the index in such a way that it wraps around. By using the modulo function, the index will always be constrained by the AVERAGE_SIZE.

The cnt variable is simply incremented until it is the size of AVERAGE_SIZE so i don’t get garbage until the array is full, it will just divide by how many variables are currently in the array rather than the full size of the array.
As far as the digitalWrite(DATAOUT,commandbits&1<<i); that is basically handling the SPI serial communication between the Arduino and the 12bit ADC taken from here: http://arduino.cc/playground/Code/MCP3208
It has been modified slightly to accommodate the mcp3202 rather than the mcp3208.

JamesC4S: Can you please elaborate on that? Should i be using the volatile keyword on any variables being declared within the ISR? I’ll look into the volatile keyword as well.

Thanks for all of your inputs!

You should only use volatile if the variable is used in an ISR or other places it could be reset to 0.

http://arduino.cc/en/Reference/Volatile

Thank you so much for this reference. I'll take a look and try it out tomorrow!

O'K, than. What is "if" for:

double Average()
{
  if (count ==0) return 0;
  return sum/cnt;
}

to avoid division by 0? Why not if (cnt == 0) ?

Trying to debug data-specific behaviour using uncontrolled data can be problematic. I suggest you write a minimal test sketch that feeds a hard-coded sequence of values to your filter and tells you what the output is. This will tell you whether the problem is in the filter, or elsewhere.

the if is so that the sketch never divides by zero so if count is equal to zero, then the sketch simply returns zero rather than sum/0. I agree that is probably isnt necessary since it will only affect if you call Average() before AverageAdd().
PeterH, thanks for the suggestion, I wasnt sure how to mimick this spike numerically other than doing it with a set of data then feeding that back into another test sketch however since the arduino only has a limited amount of memory i wasnt sure if i could store all those numbers in a large array. I suppose there is probably another way to do it, but i dont know of one =/

i did get it working this morning though to anyone who is interested. I put the volatile keyword on all the variables being changed within the interrupt but that still seemed to produce the same results. I decided that i should try to average a different set of values because the values that were being averaged were very small. I ended up averaging the signal right as it was coming out of the sensor (prior to any conversions) this way the signal was large (0 to 4095) rather than the small signal that had been converted (-1 to 1). I still dont know what truly caused the problem or why it would drop to zero that way just cause the signal was small. It seemed like somehow breaking about 1 would overload one of the variables (sum in this case) but i don’t know why that would be the case. So if anyone can figure this out i’d love to know just for future edification.

I’m sorry to have posted my full code and i know that it is hard to read through like that but i really want quite sure whether the problem was in that i had an ISR or in the averaging or in something else, so i apologize for making you all read through my entire code :roll_eyes:

thanks for all the suggestions everyone!

You didn't get it.

the if is so that the sketch never divides by zero so if count is equal to zero, then the sketch simply returns zero rather than sum/0. I agree that is probably isnt necessary since it will only affect if you call Average() before AverageAdd().

double Average()
{
  if (count ==0) return 0;
  return sum/cnt;
}

The problem here, that you returning from function with result 0 when input drops to 0: Which isn't correct. When 1 sample is "0" there are should be 19 others intact, and must be some results on the output of sum / cnt. Consequently, erroneous "0" returning from Average() resets X2 to "0" as well.

X1 = Average();
    X2 = (X1-LastX1)/ts;
    X3 = gamma;
    X4 = (X3-LastX3)/ts;

oh i didnt notice that it was count==0 rather than cnt==0 like it should be. Sorry about that. I'll check that out.