Thank you for your reply, Lauszus.
we changed the values of the sensitivity as you recommended, but it doesn't seem to be working.
I'm not sure what does
float Q_angleY = 0.3;
float Q_gyroY = 0.002;
float R_angleY = 0.03;
refer to..
do they mean
gyro
accelerometer
force vector?
//made by Kristian Lauszus - see http://arduino.cc/forum/index.php/topic,58048.0.html for information
#define gX A0
#define gY A1
#define aX A3
#define aY A4
#define aZ A5
#define RAD_TO_DEG 57.295779513082320876798154814105
//gyros
int gyroZeroX;//x-axis
float gyroXadc;
float gyroXrate;
float gyroXangle;
int gyroZeroY;//y-axis
float gyroYadc;
float gyroYrate;
float gyroYangle;
//accelerometers
int accZeroX;//x-axis
float accXadc;
float accXval;
float accXangle;
int accZeroY;//y-axis
float accYadc;
float accYval;
float accYangle;
int accZeroZ;//z-axis
float accZadc;
float accZval;
float accZangle;
//Results
float xAngle;
float yAngle;
float compAngleX;
float compAngleY;
float R;//force vector
//Used for timing
unsigned long timer=0;
unsigned long dtime=0;
//This is pretty simple. It takes 100 readings and calculate the average.
//gyros
int inputGyroX[100];//x-axis
long resultGyroX;
int inputGyroY[100];//y-axis
long resultGyroY;
//accelerometers
int inputAccX[100];//x-axis
long resultAccX;
int inputAccY[100];//y-axis
long resultAccY;
int inputAccZ[100];//z-axis
long resultAccZ;
//gyros
int calibrateGyroX()
{
for(int i=0;i<100;i++)
{
inputGyroX[i] = analogRead(gX);
}
for(int i=0;i<100;i++)
{
resultGyroX += inputGyroX[i];
}
resultGyroX = resultGyroX/100;
return resultGyroX;
}
int calibrateGyroY()
{
for(int i=0;i<100;i++)
{
inputGyroY[i] = analogRead(gY);
}
for(int i=0;i<100;i++)
{
resultGyroY += inputGyroY[i];
}
resultGyroY = resultGyroY/100;
return resultGyroY;
}
//accelerometers
int calibrateAccX()
{
for(int i=0;i<100;i++)
{
inputAccX[i] = analogRead(aX);
}
for(int i=0;i<100;i++)
{
resultAccX += inputAccX[i];
}
resultAccX = resultAccX/100;
return resultAccX;
}
int calibrateAccY()
{
for(int i=0;i<100;i++)
{
inputAccY[i] = analogRead(aY);
}
for(int i=0;i<100;i++)
{
resultAccY += inputAccY[i];
}
resultAccY = resultAccY/100;
return resultAccY;
}
int calibrateAccZ()
{
for(int i=0;i<100;i++)
{
inputAccZ[i] = analogRead(aZ);
}
for(int i=0;i<100;i++)
{
resultAccZ += inputAccZ[i];
}
resultAccZ = resultAccZ/100;
return resultAccZ;
}
// KasBot V2 - Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus
float Q_angleX = 0.3;
float Q_gyroX = 0.002;
float R_angleX = 0.03;
float x_angle = 0;
float x_bias = 0;
float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;
float dtX, yX, SX;
float KX_0, KX_1;
float kalmanCalculateX(float newAngle, float newRate,int looptime) {
dtX = float(looptime)/1000; // XXXXXXX arevoir
x_angle += dtX * (newRate - x_bias);
PX_00 += - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
PX_01 += - dtX * PX_11;
PX_10 += - dtX * PX_11;
PX_11 += + Q_gyroX * dtX;
yX = newAngle - x_angle;
SX = PX_00 + R_angleX;
KX_0 = PX_00 / SX;
KX_1 = PX_10 / SX;
x_angle += KX_0 * yX;
x_bias += KX_1 * yX;
PX_00 -= KX_0 * PX_00;
PX_01 -= KX_0 * PX_01;
PX_10 -= KX_1 * PX_00;
PX_11 -= KX_1 * PX_01;
return x_angle;
}
// KasBot V2 - Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus
float Q_angleY = 0.3;
float Q_gyroY = 0.002;
float R_angleY = 0.03;
float y_angle = 0;
float y_bias = 0;
float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;
float dtY, yY, SY;
float KY_0, KY_1;
float kalmanCalculateY(float newAngle, float newRate,int looptime) {
dtY = float(looptime)/1000; // XXXXXXX arevoir
y_angle += dtY * (newRate - y_bias);
PY_00 += - dtY * (PY_10 + PY_01) + Q_angleY * dtY;
PY_01 += - dtY * PY_11;
PY_10 += - dtY * PY_11;
PY_11 += + Q_gyroY * dtY;
yY = newAngle - y_angle;
SY = PY_00 + R_angleY;
KY_0 = PY_00 / SY;
KY_1 = PY_10 / SY;
y_angle += KY_0 * yY;
y_bias += KY_1 * yY;
PY_00 -= KY_0 * PY_00;
PY_01 -= KY_0 * PY_01;
PY_10 -= KY_1 * PY_00;
PY_11 -= KY_1 * PY_01;
return y_angle;
}
//print data to processing
void processing()
{
Serial.print(gyroXangle);Serial.print("\t");
Serial.print(gyroYangle);Serial.print("\t");
Serial.print(accXangle);Serial.print("\t");
Serial.print(accYangle);Serial.print("\t");
Serial.print(compAngleX);Serial.print("\t");
Serial.print(compAngleY); Serial.print("\t");
Serial.print(xAngle);Serial.print("\t");
Serial.print(yAngle);Serial.print("\t");
Serial.print("\n");
delay(1000);
}
void setup()
{
analogReference(EXTERNAL); //3.3V
Serial.begin(9600);
delay(100);//wait for the sensor to get ready
//calibrate all sensors in horizontal position
gyroZeroX = calibrateGyroX();
gyroZeroY = calibrateGyroY();
accZeroX = calibrateAccX();
accZeroY = calibrateAccY();
accZeroZ = calibrateAccZ();
timer=millis();//start timing
}
void loop()
{
gyroXadc = analogRead(gX);
gyroXrate = (gyroXadc-gyroZeroX)/1.0323;//(gyroXadc-gryoZeroX)/Sensitivity - in quids Sensitivity = 0.00333/3.3*1023=1.0323
gyroXangle=gyroXangle+gyroXrate*dtime/1000;//Without any filter
gyroYadc = analogRead(gY);
gyroYrate = (gyroYadc-gyroZeroY)/1.0323;//(gyroYadc-gryoZeroX)/Sensitivity - in quids Sensitivity = 0.00333/3.3*1023=1.0323
gyroYangle=gyroYangle+gyroYrate*dtime/1000;//Without any filter
accXadc = analogRead(aX);
accXval = (accXadc-accZeroX)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids Sensitivity = 0.33/3.3*1023=102,3
accYadc = analogRead(aY);
accYval = (accYadc-accZeroY)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids Sensitivity = 0.33/3.3*1023=102,3
accZadc = analogRead(aZ);
accZval = (accZadc-accZeroZ)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids Sensitivity = 0.33/3.3*1023=102,3
accZval++;//1g in horizontal position
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
accXangle = acos(accXval/R)*RAD_TO_DEG-90;
accYangle = acos(accYval/R)*RAD_TO_DEG-90;
//accZangle = acos(accZval/R)*RAD_TO_DEG;
/* //used for debugging
Serial.print(gyroZrate,0);Serial.print("\t");
Serial.print(gyroZrate,0);Serial.print("\t");
Serial.print(gyroZrate,0);Serial.print("\t");
Serial.print(gyroXangle,0);Serial.print("\t");
Serial.print(gyroYangle,0);Serial.print("\t");
Serial.print(gyroZangle,0);Serial.print("\t");
Serial.print(accXval,2);Serial.print("\t");
Serial.print(accYval,2);Serial.print("\t");
Serial.print(accZval,2);Serial.print("\t");
Serial.print(accXangle,0);Serial.print("\t");
Serial.print(accYangle,0);Serial.print("\t");
//Serial.print(accZangle,0);Serial.print("\t");
*/
dtime = millis()-timer;
timer = millis();
compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));
xAngle = kalmanCalculateX(accXangle, gyroXrate, dtime);
yAngle = kalmanCalculateY(accYangle, gyroYrate, dtime);
/*Serial.print(compAngleX,0);Serial.print("\t");
Serial.print(compAngleY,0);Serial.print("\t");
Serial.print(xAngle,0);Serial.print("\t");
Serial.print(yAngle,0);Serial.print("\t");
Serial.println("");*/
processing();
}