Часть вторая:
приём данных, приведение их к нулю, усреднение. Вычисление углов.
Эпиграф: Акселерометры - шумные ребята.
1. Показания неподвижного акселерометра зависят от его углового положения относительно вектора земной гравитации (направления на центр Земли).
2. Приведение к нулю показаний акселерометра.
Перед началом обработки данных акселерометра в программном цикле, его показания должны быть приведены к нулю. Это означает, что когда робот неподвижен и строго вертикален, показания акселерометра по осям "X" и "Y" должны быть равны нулю. Показания оси "Z" должны быть максимальны, т.е. в "акселерометрических попугаях" численно равны одному "же" - ускорению свободного падения на планете Земля. Приведение к нулю производится в установочном блоке кода:
_________________
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() является one off action (? - прим. перев.), у нас есть достаточно времени, чтобы усреднить 3 х 50 измерений. В итоге сила земной гравитации оказывается полностью приложенной к оси "Z". Для используемого мной аналогового акселерометра, сигнал, соответствующий ускорению свободного падения, равен 330 мВ ±10%. Для десятибитного АЦП с опорным напряжением 3,3 В эти милливольты оказываются численно выражены следующим образом:
330/3300 х 1024 = 102
Это значение будет уточняться позже. Для второй версии (?), перед усреднением необходимо удалить по пять максимальных значений сверху и снизу получаемого диапазона чисел (излишний шум), или же найти характерный блок данных, "узор" значений которых наиболее часто повторяется - и именно этот кусок числовых данных использовать для усреднения.
3. Приём данных
_______________
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];
}
}
________________
Каждый сенсор опрашивается по пять раз и усредняется по результатам этого пятикратного опроса, а затем вычисляются нулевые значения по каждой из осей. Во второй версии (?) перед усреднением полученных данных необходимо удалить верхнее и нижнее значение их диапазона.
4. Формат данных
________________
// 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));
}
________________
Числовые значения, получаемые с сенсоров, оказываются сформатированы следующим образом:
Горизонт (0 град = 0 Quid) ACC_X:0 ACC_Z:XX GYR_X:0
Влево (-90 град = -256 Quid) ACC_X:XX ACC_Z:0 GYR_X:0
Вправо (+90 град = +256 Quid) ACC_X:-XX ACC_Z:0 GYR_X:0
Вверх ногами (180 град = +512 Quid) ACC_X:0 ACC_Z:-XX GYR_X:0
Для ADXL330/335 "ХХ" равно примерно 100.
Перед тем, как двигаться далее, убедитесь в том, что вы получаете СИММЕТРИЧНЫЕ данные! Это один из первых камней преткновения большинства балансерных проектов. Т.е. сейчас нам самое время установить
sensorZero[ACC_Z]
назначив
sensorZero[ACC_Z]-=102
Это нужно сделать для того, чтобы получить зеркально симметричные числа (+103 и -103) в процессе прохождения диапазона углов от 0 до 512 Квидов (Quids) - или от 0 до 180 градусов.
5. Вычисление углов и угловых скоростей
ACC_angle = getAccAngle();
Определяет угол наклона робота через арктангенс числовых данных двух акселерометрических осей. Эти данные не нужно переводить в привычные единицы измерения, однако они должны быть приведены к нулю (симметрированы относительно нуля) и иметь одинаковый масштаб.
GYRO_rate = getGyroRate();
Определяет гироскопическую угловую скорость из "аналогового сырья", которое выдаёт нам наш гироскоп на своём выходе. Чувствительность гироскопа составляет 2,0 мВ/град/с. С ардуинским десятибитным АЦП имеем один АЦП-шный "попугай", равный 4,58(3) Quid/sec или же 1,611328 град/с.
_______________
// 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);
}
_________________
Теперь вы можете наблюдать шумные сигналы акселерометра и гироскопа в мониторе компорта. Однако куда удобнее и нагляднее делать это в графической оболочке LabView или Processing. Могу поделиться своим кодом с владельцами LabView! Gybby623, не хочешь ли и ты обнародовать свой код для Processing???
Следующая часть: Фильтр Калмана и ПИД.
Не слишком ли я углубляюсь в детали? Или наоборот излишне краток? Дайте мне знать!
***
Gybby623: Конечно, вот мой код для Processing:
_____________________
/*
Serial Graphing Sketch
by Tom Igoe
Language: Processing
This sketch takes ASCII values from the serial port
at 9600 bps and graphs them.
The values should be comma-delimited, with a newline
at the end of every set of values.
The expected range of the values is between 0 and 1023.
Created 20 April 2005
Updated 27 June 2008
/
import processing.serial.;
int maxNumberOfSensors = 6; // Arduino has 6 analog inputs, so I chose 6
boolean fontInitialized = false; // whether the font's been initialized
Serial myPort; // The serial port
float[] previousValue = new float[maxNumberOfSensors]; // array of previous values
int xpos = 0; // x position of the graph
PFont myFont; // font for writing text to the window
void setup () {
// set up the window to whatever size you want:
size(800, 600);
// List all the available serial ports:
println(Serial.list());
// I know that the first port in the serial list on my mac
// is always my Arduino or Wiring module, so I open Serial.list()[0].
// Open whatever port is the one you're using.
String portName = Serial.list()[0];
myPort = new Serial(this, portName, 9600);
myPort.clear();
// don't generate a serialEvent() until you get a newline (\n) byte:
myPort.bufferUntil('\n');
// create a font with the fourth font available to the system:
myFont = createFont(PFont.list()[3], 14);
textFont(myFont);
fontInitialized = true;
// set inital background:
background(0);
// turn on antialiasing:
smooth();
}
void draw () {
// nothing happens in the draw loop,
// but it's needed to keep the program running
}
void serialEvent (Serial myPort) {
// get the ASCII string:
String inString = myPort.readStringUntil('\n');
// if it's not empty:
if (inString != null) {
// trim off any whitespace:
inString = trim(inString);
// convert to an array of ints:
int incomingValues[] = int(split(inString, ","));
// print out the values
// print("length: " + incomingValues.length + " values.\t");
if (incomingValues.length <= maxNumberOfSensors && incomingValues.length > 0) {
for (int i = 0; i < incomingValues.length; i++) {
// map the incoming values (0 to 1023) to an appropriate
// graphing range (0 to window height/number of values):
float ypos = map(incomingValues[i], 0, 1023, 0, height/incomingValues.length);
// figure out the y position for this particular graph:
float graphBottom = i * height/incomingValues.length;
ypos = ypos + graphBottom;
// make a black block to erase the previous text:
noStroke();
fill(0);
rect(10, graphBottom+1, 110, 20);
// print the sensor numbers to the screen:
fill(255);
int textPos = int(graphBottom) + 14;
// sometimes serialEvent() can happen before setup() is done.
// so you need to make sure the font is initialized before
// you text():
if (fontInitialized) {
text("Sensor " + i + ":" + incomingValues[i], 10, textPos);
}
// draw a line at the bottom of each graph:
stroke(127);
line(0, graphBottom, width, graphBottom);
// change colors to draw the graph line:
stroke(64*i, 32*i, 255);
line(xpos, previousValue[i], xpos+1, ypos);
// save the current value to be the next time's previous value:
previousValue[i] = ypos;
}
}
// if you've drawn to the edge of the window, start at the beginning again:
if (xpos >= width) {
xpos = 0;
background(0);
}
else {
xpos++;
}
}
}
____________________