Pages: 1 [2]   Go Down
Author Topic: Calcolare angolo assoluto da velocità angolare  (Read 1556 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Sr. Member
****
Karma: 7
Posts: 293
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset


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.
Logged

Messina (Italy)
Offline Offline
Sr. Member
****
Karma: 4
Posts: 276
Ciao a tutti!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
 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

« Last Edit: December 20, 2012, 06:30:15 pm by Dario Gogliandolo » Logged

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

Messina (Italy)
Offline Offline
Sr. Member
****
Karma: 4
Posts: 276
Ciao a tutti!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?


* Schermata 2012-12-22 alle 17.11.01.png (16.49 KB, 1042x242 - viewed 12 times.)
« Last Edit: December 22, 2012, 11:15:20 am by Dario Gogliandolo » Logged

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

Messina (Italy)
Offline Offline
Sr. Member
****
Karma: 4
Posts: 276
Ciao a tutti!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

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

Pages: 1 [2]   Go Up
Jump to: