- Continued -
5 - angle and rate calculation
ACC_angle = getAccAngle();
the angle is obtained by using the arctangent of the two accelerometers readings.
The accelerometer values do not need to be scaled into actual units, but must be zeroed and have the same scale.
GYRO_rate = getGyroRate();
Determine gyro angular rate from raw analog values.
Gyro sensitivity is 2.0 mV/degrees/second
With the Arduino 10 bit ADC:
One ADC unit is 4.583333333 quid/sec (1.611328 degrees/sec)
// Main module K_bot angle angles in Quids, 10 bit ADC -----------------------------
// 5 - angle and rate calculation display ACC_Angle and GYRO_rate
#include <math.h>
#define GYR_Y 0 // Gyro Y (IMU pin #4)
#define ACC_Z 1 // Acc Z (IMU pin #7)
#define ACC_X 2 // Acc X (IMU pin #9)
int STD_LOOP_TIME = 9;
int sensorValue[3] = { 0, 0, 0};
int sensorZero[3] = { 0, 0, 0 };
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
int ACC_angle;
int GYRO_rate;
void setup() {
analogReference(EXTERNAL); // Aref 3.3V
Serial.begin(115200);
delay(100);
calibrateSensors();
}
void loop() {
// ********************* Sensor aquisition & filtering *******************
updateSensors();
ACC_angle = getAccAngle(); // in Quids
GYRO_rate = getGyroRate(); // in Quids per seconds
// ********************* print Debug info *************************************
serialOut_sensor();
// *********************** loop timing control **************************
lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME) delay(STD_LOOP_TIME-lastLoopUsefulTime);
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
}
void serialOut_sensor() {
static int skip=0;
if(skip++==20) {
skip = 0;
Serial.print(ACC_angle); Serial.print(",");
Serial.print(GYRO_rate); Serial.print("\n");
}
}
// Sensors Module -----------------------------------------------------------------------------------
void calibrateSensors() { // Set zero sensor values
long v;
for(int n=0; n<3; n++) {
v = 0;
for(int i=0; i<50; i++) v += readSensor(n);
sensorZero[n] = v/50;
} //(618 - 413)/2 = 102.5 330/3.3 = x/1024
sensorZero[ACC_Z] -= 100; //102; // Sensor: horizontal, upward
}
void updateSensors() { // data acquisition
long v;
for(int n=0; n<3; n++) {
v = 0;
for(int i=0; i<5; i++) v += readSensor(n);
sensorValue[n] = v/5 - sensorZero[n];
}
}
int readSensor(int channel){
return (analogRead(channel));
}
int getGyroRate() { // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
return int(sensorValue[GYR_Y] * 4.583333333); // in quid/sec:(1024/360)/1024 * 3.3/0.002)
}
int getAccAngle() {
return arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256; // in Quid: 1024/(2*PI))
}
int arctan2(int y, int x) { // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
int coeff_1 = 128; // angle in Quids (1024 Quids=360°)
int coeff_2 = 3*coeff_1;
float abs_y = abs(y)+1e-10;
float r, angle;
if (x >= 0) {
r = (x - abs_y) / (x + abs_y);
angle = coeff_1 - coeff_1 * r;
} else {
r = (x + abs_y) / (abs_y - x);
angle = coeff_2 - coeff_1 * r;
}
if (y < 0) return int(-angle);
else return int(angle);
}
You can check the (noisy) Acc angle and Gyro rate within the serial monitor
a better option is to display graphical data using LabView or Processing
I can share my code with Labview owners
Gybby623, would you please post your Processing code ??
Next part: Kalman filtering & PID Algorithm
Do I put to much detail ??
Should I go deeper ??
Let me know :