__ ** Balancing robot for dummies **__
Part two: sensors aquisition smoothing and zeroing, angle calculation
Forewords: Acc sensors are... noisy :o
1 - Accelerators response vs gravity
taken from the ADXL330 data sheet
2 - Zeroing sensor
Before aquiring data in the loop, sensors should be zeroed
This means that when the bot is stricly still and vertical, sensors should read "0"
except for the vertical axis (Acc_Z), which is sensing gravity (1g)
Zero mesurement is performed in setup:
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;
}
sensorZero[ACC_Z] -= 102;
}
calibrateSensors() is a one off action, so we have time to average 3 X 50 measurements
Finally, gravity value is deducted to Acc_Z
For ADXL330/335: 1g = 330 mV (+- 10%)
ACC_Z correction for 10 bit ADC and 3.3V AREF: 330/3300*1024 = 102 (to be fine tuned later on)
//TODO for V2: before averaging, remove the 5 upper and lower values (abnormal noise) or find modal value (The mode of a set of numbers is the value that occurs most frequently)
3 - Sensor aquisition
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];
}
}
Each sensor is pooled 5 time and averaged, zero value is then deducted
//TODO for V2: before averaging, remove the upper and lower values (noise)
4 - Checking sensor data format
// Main module K_bot angles in Quids, 10 bit ADC -------------
// 4 - Checking sensor data format display raw sensors data
#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;
void setup() {
analogReference(EXTERNAL); // Aref 3.3V
Serial.begin(115200);
delay(100);
calibrateSensors();
}
void loop() {
// ********************* Sensor aquisition & filtering *******************
updateSensors();
// ********************* print Debug info *************************************
serialOut_raw();
// *********************** loop timing control **************************
lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME) delay(STD_LOOP_TIME-lastLoopUsefulTime);
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
}
void serialOut_raw() {
static int skip=0;
if(skip++==40) {
skip = 0;
Serial.print("ACC_X:"); Serial.print(sensorValue[ACC_X]);
Serial.print(" ACC_Z:"); Serial.print(sensorValue[ACC_Z]);
Serial.print(" GYR_Y:"); Serial.println(sensorValue[GYR_Y]);
}
}
// 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;
}
sensorZero[ACC_Z] -= 103;
}
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));
}
The sensors values vs position should read as follow:
Horizontal ( 0° = 0 Quid ) ACC_X: 0 ACC_Z: XX GYR_X: 0
Left side (-90° = -256 Quid) ACC_X: XX ACC_Z: 0 GYR_X: 0
Right side (+90° = +256 Quid) ACC_X:-XX ACC_Z: 0 GYR_X: 0
Reversed (180° = +512 Quid) ACC_X: 0 ACC_Z:-XX GYR_X: 0
For ADXL330/335, XX value is around 100:
Before going further, make sure you get that type of symetrical data
(this is one pitfall in this project ;))
It is now time to adjust sensorZero[ACC_Z] by adjusting "sensorZero[ACC_Z] -= 102"in order to have opposite/symetrical values (ie +103 and - 103) when going from 0 to 512 Quids (O° to 180°).
