hello, i'm having a little trouble with the moving average code on the playground here: Arduino Playground - 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;
}
