Go Down

Topic: Calcolare angolo assoluto da velocità angolare (Read 2472 times) previous topic - next topic

qsecofr



pero non capisco nel tuo programma che fine ha poi fatto l'arcoseno...



ahhh si aggiungo anche una cosa: forse già la sai ma le operazioni sui double a livello di tempi di calcolo sono veleno... spesso in tal senso si creano degli array di "arcoseni" nel tuo caso già calcolati a partire magari dai gradi che puoi ipotizzare interi e che quindi ti fanno da "indice" dell'array.
Comunque casomai vedrai a tempo debito.

Dario Gogliandolo

#16
Dec 20, 2012, 11:03 pm Last Edit: Dec 21, 2012, 12:30 am by Dario Gogliandolo Reason: 1


Io vedo che il valore minimo che mi può restituire è -8191 che equivale a -90° e il valore massimo è 8191 che equivale a 90°.


Io vedo che continui a non capire, ma come puoi pensare di convertire direttamente l'accelerazione in gradi ?
Ti ho detto chiaramente che l'accelerazione rappresenta il seno, o coseno, dell'angolo non che è l'angolo.



Scusa astrobeed ma non è che non ho capito, al massimo mi sono spiegato male. 8191 effettivamente corrisponde a 90° ma non ho detto che faccio la proporzione fra i 2 valori. io la proporzione la faccio fra -8191/8191 e -1/1 e successivamente faccio l' arcsen (o l'arccos) del valore. Infatti nel codice che ho postato
Code: [Select]

 if (axis[Y]>0)
 {
   angVal=mapFloat(axis[Y], 0,8191, 0, 1);
 }
 else
 {
   angVal=mapFloat(axis[Y], -8191,0, -1, 0);
 }


il valore massimo in uscita della funziona mapFloat è 1.
Non per dire ma un briciolo di trigonometria me la ricordo!  :smiley-sweat:

Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Dario Gogliandolo

#17
Dec 22, 2012, 02:02 pm Last Edit: Dec 22, 2012, 05:15 pm by Dario Gogliandolo Reason: 1
AGGIORNAMENTI:
Sono riuscito a trovare l'angolo assoluto sia di gyroscopio che di accelerometro ed ho provato ad usare sia il filtro di kalman che quello complementare: quello di kalman mi da come risultato lo stesso valore identico dell'angolo dell'accelerometro, mentre quello complementare (di 1° livello) mi da in uscita un valore accettabile. Il codice che ho usato é il seguente:

Quote

#include <Servo.h>

#include <Wire.h>
#include <ITG3200.h>
#include <bma180.h>
#include <math.h>
#define address 0x40

#define RAD_TO_DEG 57.295779513082320876798154814105
ITG3200 gyro = ITG3200();
BMA180 bma180 = BMA180();
Servo myservo;
int accAxis[3];
float gyroAxis[3];
float angleGyro[3];
const int X=0, Y=1, Z=2;
float accErr[3] = {0.00, 1.50, 0.00};
long previousMicros = 0;
unsigned long currentMicros;

//###### COMPLEMENTARY #######
float tau=0.01;
float a=0.0;
float x_angleC=0;
//##### END COMPLEMENTARY ####


//######## KALMAN ##########
float Q_angle  =  0.001;//0.01; //0.001
float Q_gyro   =  0.003;//0.0003;  //0.003
float R_angle  =  0.03;//0.01;  //0.03
float x_angle = 0;
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float  y, S;
float K_0, K_1;
//###### END KALMAN ########


long intervallo2, currentMillis, previousMillis;
void setup(void) {
 Serial.begin(115200);
 Wire.begin();
 Serial.println("Inizializzazione accelerometro...");
 bma180.setAddress(BMA180_ADDRESS_SDO_LOW);
 bma180.SoftReset();
 bma180.enableWrite();
 bma180.SetFilter(bma180.F10HZ);
 bma180.setGSensitivty(bma180.G1);
 bma180.SetSMPSkip();
 bma180.SetISRMode();
 bma180.disableWrite();
 Serial.println("Completato.");
 myservo.attach(9);
 gyro.init(ITG3200_ADDR_AD0_LOW);
 
 Serial.print("Calibrazione zero gyro...");
 gyro.zeroCalibrate(2500, 2);
 Serial.println("Completato.");
 delay(100);
  previousMicros = micros();
}

float gyroY=0;
boolean stato=false;

void loop(void) {
currentMillis=millis();

   while (gyro.isRawDataReady()) {  
     
     currentMicros = micros();
     float intervallo = currentMicros-previousMicros;
     currentMillis=millis();
     intervallo2=currentMillis-previousMillis;
     if (intervallo2>500 && stato==false)
     {
     myservo.write(120);
     stato=true;
      previousMillis=currentMillis;
     }
     else if (intervallo2>500 && stato==true)
     {
     myservo.write(60);
     stato=false;
      previousMillis=currentMillis;
     }
     
     

     
     bma180.readAccel(accAxis);
     gyro.readGyro(&gyroAxis[X], &gyroAxis[Y], &gyroAxis[Z]);
      //gyroY+=gyroAxis[Y]*(intervallo/1000000);  //FUNZIONA
     float angolo = Complementary(getAngleAcc(Y),gyroY, intervallo);
   
     /*Serial.print(getAngleAcc(Y));
     Serial.print("\t");
     
     Serial.print((float)((int)(-gyroY*10))/10);
     //Serial.print(getAngleGyro(Y, intervallo));
     Serial.print("\t");*/
     Serial.println((float)(int)((angolo)*10)/10);
     previousMicros = currentMicros;
   
   }
}


float mapFloat(int x, int in_min, int in_max, float out_min, float out_max)
{
 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}



float getAngleGyro(int asse, float tempo)
{
 angleGyro[Y]+=gyroAxis[Y]*(tempo/1000000);
 return angleGyro[Y];
 //angleGyro[asse]+=gyroAxis[asse]*(tempo/1000000);
 //return angleGyro[asse];
}


float getAngleAcc(int asse)
{
  float angVal=0;
  angVal = mapFloat(accAxis[asse], -8191,8191, -1, 1);
  return acos(angVal)*RAD_TO_DEG-90-accErr[Y];//(float)(int)((acos(angVal)*RAD_TO_DEG-90-accErr[Y])*10)/10;
}





float kalmanCalculate(float newAngle, float newRate,int looptime)
{
float dt = float(looptime)/1000;
x_angle += dt * (newRate - x_bias);
P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
P_01 +=  - dt * P_11;
P_10 +=  - dt * P_11;
P_11 +=  + Q_gyro * dt;

y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

x_angle +=  K_0 * y;
x_bias  +=  K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return x_angle;
}



float Complementary(float newAngle, float newRate,int looptime) {
float dtC = float(looptime)/1000.0;
a=tau/(tau+dtC);
x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
return x_angleC;
}





A questo punto ho montato tutta l'imu su un servo facendola girare di 60° (da 60 a 120) ed ho ottenuto il grafico in allegato. Secondo voi come è il risultato del filtro?
Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Dario Gogliandolo

Ma se usassi 2 assi dell'accelerometro e 1 del giroscopio la misura migliora? Se si come posso fare ad usare 2 assi dell'accelerometro?
Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Go Up