Go Down

Topic: Calcolare angolo assoluto da velocità angolare (Read 2636 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
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy