lunedì 30 novembre 2015

Integrazioni dati da accelerometro ADXL335/GY-61

Ho provato ad usare, su suggerimento di un amico, l'accelerometro come misuratore di spostamento.
In estrema sintesi integrando i dati di accelerazione si puo' avere una stima, seppure un po' grossolana, di quanto e' stato spostato il sensore (ovviamente se il sensore viene spostato con velocita' costante questo sistema non funziona)

Il sistema che ho usato e' una schedina GY-61 su cui e' montato un ADXL335 gia' usato altre volte nella versione Sparkfun


Il problema maggiore e' cercare di gestire il rumore dello strumento e la deriva (su quello orginale Sparkfun il modulo non mostra deriva mentre su questo modello cinese la deriva e' presente)

Questo e' lo script per la gestione del calcolo dello spostamento su due assi (x,y); l'asse z ha una sensibilita' differente a quella su x ed y e diventa difficile fare misure accurate

Per prima cosa viene convertito il valore delle porte analogiche in accelerazione in m/s2 togliendo l'offset per avere una misura piu'possibile vicina a zero in condizioni statiche
Successivamente viene integrata la velocita' ed in seguito lo spostamento (vedi questo link per una spiegazione piu' dettagliata)
L'integrazione avviene mediando il valore della accelerazione tra il valore attuale e quello precedente e moltiplicando per il delta di tempo (qui impostato a 0.05 secondi)
Succesivamente si passa allo spostamento sui due assi e calcolando quindi la risultante dello spostamento sui due assi

Il calcolo viene effettuato solo se viene superato un valore di trigger in modo da evitare di registrare tutto il rumore
ATTENZIONE : questo calcolo funziona solo per moti su superfici orizzontali. In caso contrario viene registrata anche una componente della accelerazione di gravita' per cui i dati sono falsati
--------------------------------------------------------------------------
unsigned long time, old_time;
int x,y,z;
float xx,yy,zz,ax,ay;
float old_ax,old_ay,old_vx,old_vy; //
float vx,vy,av_ax,av_ay,av_vx,av_vy; //valori medi di acc e vel sui due assi
float dx,dy,old_dx,old_dy; //spostamento
float delta_time;
float risultante;

void setup() {
  Serial.begin(115200);
  old_ax=0;
  old_ay=0;
  old_vx=0;
  old_vy=0;
  old_dx=0;
  old_dy=0;
  av_ax=0;
  av_ay=0;
  av_vx=0;
  av_vy=0;
}

void loop() {
  //z = analogRead(A3);
  y = analogRead(A4);
  x = analogRead(A5);
  //zz = z * (5.0 / 1023.0);
  yy = y * (5.0 / 1023.0)-1.67;
  xx = x * (5.0 / 1023.0)-1.61;
 
  ax = 9.8*(xx/0.33);
  ay = 9.8*(yy/0.33);

  //toglie offset
  ax = ax + 0.06;
  ay = ay + 0.68 ;
 
if ((ax-old_ax) > 0.3)
  {
  //time = millis();
  //delta_time = (time - old_time)/1000;
  delta_time = 0.05;
 
  //calcolo dell'accelerazione media
  av_ax = (ax+old_ax)/2;
  av_ay = (ay+old_ay)/2;

  //calcolo della velocita'
  vx = old_vx + (av_ax * delta_time);
  vy = old_vy + (av_ay * delta_time);

  // calcolo della velocita' media
  av_vx = (vx+old_vx)/2;
  av_vy = (vy+old_vy)/2;

  // spostamento
  dx = old_dx + (av_vx * delta_time);
  dy = old_dy + (av_vy * delta_time);

  // risultante
  risultante = sqrt((dx*dx)+(dy*dy));

  Serial.print(ax);
  Serial.print(";");
  Serial.print(ay);
  Serial.print(";");
  Serial.print(vx);
  Serial.print(";");
  Serial.print(vy);
  Serial.print(";");
  Serial.print(dx);
  Serial.print(";");
  Serial.print(dy);
  Serial.print(";");
  Serial.println(risultante);
  }

  delay(50);
  //old_time = time;

  //scambio delle variabili
  old_dx = dx;
  old_dy = dy;
  old_vx = vx;
  old_vy = vy;
  old_ax = ax;
  old_ay = ay;
}

--------------------------------------------------------------------------