Konum Analizi
  • Merhabalar,

    Elimde AltIMU-10 eksenli çoklu sensör bulanmaktadır.Projemde okuduğum konum verilerini diferansiyel denklemle hıza indirgeyeceğim.Sensörün belli bir kütüphanesi var ve konum bilgilerini serial monitörde görebiliyorum fakat bu değerleri hangi program aralığında alacağım ve işyeceğimi bulamadım.Kodlar aşağıda bulunmaktadır.

    Sensör:http://www.robotistan.com/altimu-10-gyro-ivme-olcer-pusula-ve-yukseklik-sensor-unitesi-gyro-acce-1

    Kodlar

    // Uncomment the below line to use this axis definition:
    // X axis pointing forward
    // Y axis pointing to the right
    // and Z axis pointing down.
    // Positive pitch : nose up
    // Positive roll : right wing down
    // Positive yaw : clockwise
    int SENSOR_SIGN[9] = {1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
    // Uncomment the below line to use this axis definition:
    // X axis pointing forward
    // Y axis pointing to the left
    // and Z axis pointing up.
    // Positive pitch : nose down
    // Positive roll : right wing down
    // Positive yaw : counterclockwise
    //int SENSOR_SIGN[9] = {1,-1,-1,-1,1,1,1,-1,-1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer

    // tested with Arduino Uno with ATmega328 and Arduino Duemilanove with ATMega168

    #include

    // LSM303 accelerometer: 8 g sensitivity
    // 3.9 mg/digit; 1 g = 256
    #define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer

    #define ToRad(x) ((x)*0.01745329252) // *pi/180
    #define ToDeg(x) ((x)*57.2957795131) // *180/pi

    // L3G4200D gyro: 2000 dps full scale
    // 70 mdps/digit; 1 dps = 0.07
    #define Gyro_Gain_X 0.07 //X axis Gyro gain
    #define Gyro_Gain_Y 0.07 //Y axis Gyro gain
    #define Gyro_Gain_Z 0.07 //Z axis Gyro gain
    #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
    #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
    #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second

    // LSM303 magnetometer calibration constants; use the Calibrate example from
    // the Pololu LSM303 library to find the right values for your board
    #define M_X_MIN -421
    #define M_Y_MIN -639
    #define M_Z_MIN -238
    #define M_X_MAX 424
    #define M_Y_MAX 295
    #define M_Z_MAX 472

    #define Kp_ROLLPITCH 0.02
    #define Ki_ROLLPITCH 0.00002
    #define Kp_YAW 1.2
    #define Ki_YAW 0.00002

    /*For debugging purposes*/
    //OUTPUTMODE=1 will print the corrected data,
    //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
    #define OUTPUTMODE 1

    //#define PRINT_DCM 0 //Will print the whole direction cosine matrix
    #define PRINT_ANALOGS 0 //Will print the analog raw data
    #define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw

    #define STATUS_LED 13

    float G_Dt=0.02; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible

    long timer=0; //general purpuse timer
    long timer_old;
    long timer24=0; //Second timer used to print values
    int AN[6]; //array that stores the gyro and accelerometer data
    int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors

    int gyro_x;
    int gyro_y;
    int gyro_z;
    int accel_x;
    int accel_y;
    int accel_z;
    int magnetom_x;
    int magnetom_y;
    int magnetom_z;
    float c_magnetom_x;
    float c_magnetom_y;
    float c_magnetom_z;
    float MAG_Heading;

    float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
    float Gyro_Vector[3]= {0,0,0};//Store the gyros turn rate in a vector
    float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
    float Omega_P[3]= {0,0,0};//Omega Proportional correction
    float Omega_I[3]= {0,0,0};//Omega Integrator
    float Omega[3]= {0,0,0};

    // Euler angles
    float roll;
    float pitch;
    float yaw;

    float errorRollPitch[3]= {0,0,0};
    float errorYaw[3]= {0,0,0};

    unsigned int counter=0;
    byte gyro_sat=0;

    float DCM_Matrix[3][3]= {
    {
    1,0,0 }
    ,{
    0,1,0 }
    ,{
    0,0,1 }
    };
    float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here


    float Temporary_Matrix[3][3]={
    {
    0,0,0 }
    ,{
    0,0,0 }
    ,{
    0,0,0 }
    };

    void setup()
    {
    Serial.begin(115200);
    pinMode (STATUS_LED,OUTPUT); // Status LED

    I2C_Init();

    Serial.println("Pololu MinIMU-9 + Arduino AHRS");

    digitalWrite(STATUS_LED,LOW);
    delay(1500);

    Accel_Init();
    Compass_Init();
    Gyro_Init();

    delay(20);

    for(int i=0;i<32;i++) // We take some readings...<br /> {
    Read_Gyro();
    Read_Accel();
    for(int y=0; y<6; y++) // Cumulate values<br /> AN_OFFSET[y] += AN[y];
    delay(20);
    }

    for(int y=0; y<6; y++)<br /> AN_OFFSET[y] = AN_OFFSET[y]/32;

    AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];

    //Serial.println("Offset:");
    for(int y=0; y<6; y++)<br /> Serial.println(AN_OFFSET[y]);

    delay(2000);
    digitalWrite(STATUS_LED,HIGH);

    timer=millis();
    delay(20);
    counter=0;
    }

    void loop() //Main Loop
    {
    if((millis()-timer)>=20) // Main loop runs at 50Hz
    {
    counter++;
    timer_old = timer;
    timer=millis();
    if (timer>timer_old)
    G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
    else
    G_Dt = 0;

    // *** DCM algorithm
    // Data adquisition
    Read_Gyro(); // This read gyro data
    Read_Accel(); // Read I2C accelerometer

    if (counter > 5) // Read compass data at 10Hz... (5 loop runs)
    {
    counter=0;
    Read_Compass(); // Read I2C magnetometer
    Compass_Heading(); // Calculate magnetic heading
    }

    // Calculations...
    Matrix_update();
    Normalize();
    Drift_correction();
    Euler_angles();
    // ***

    printdata();
    }

    }
  • Tam olarak ne istediğini anlamadım. Seri monitörde gördüğün değerleri nasıl işleyeceğini mi soruyorsun? Serial print komutlarındaki değişkenleri kullanabilirsin.misal
    Serial.println(Serial.println(AN_OFFSET[y] kısmında birdeğişken dizisi seri monitöre aktarılmış. AN_OFFSET[1] dersen ekranda gördüğün çıktının ilk satır bilgisini alırsın.
  • Aynen hocam serial monitördeki değerleri hangi komuttan aldığını öğrenmek istedim,o değerleri işleyerek hıza ulaşmam lazım ve bu değerleri lcd ya da 7 segment display de göstereceğim,arduino da biraz yabancıyım araştırıdım fakat herhangi bir diferansiyel işlem yapan komut görmedim,bu konuda yardımcı olabilir misiniz?
  • Peki denklemin matematiksel fonksiyon formülünü biliyorsan program üzerinde uygulamanan gerek. Programın serimonitörde çıktısını atarmısın buraya. Derin bi konu verdiği sonuçlar nasılmış bakalım.
  • Değerler resimdeki gibi,burdan ben verileri alıp nasıl işleyebilirim bi bilginiz var mı acaba
  • void printdata(void)
    {
    Serial.print("!");

    #if PRINT_EULER == 1
    Serial.print("ANG:");
    Serial.print(ToDeg(roll));
    Serial.print(",");
    Serial.print(ToDeg(pitch));
    Serial.print(",");
    Serial.print(ToDeg(yaw));
    #endif
    #if PRINT_ANALOGS==1
    Serial.print(",AN:");
    Serial.print(AN[0]); //(int)read_adc(0)
    Serial.print(",");
    Serial.print(AN[1]);
    Serial.print(",");
    Serial.print(AN[2]);
    Serial.print(",");
    Serial.print(AN[3]);
    Serial.print (",");
    Serial.print(AN[4]);
    Serial.print (",");
    Serial.print(AN[5]);
    Serial.print(",");
    Serial.print(c_magnetom_x);
    Serial.print (",");
    Serial.print(c_magnetom_y);
    Serial.print (",");
    Serial.print(c_magnetom_z);
    #endif
    /*#if PRINT_DCM == 1
    Serial.print (",DCM:");
    Serial.print(convert_to_dec(DCM_Matrix[0][0]));
    Serial.print (",");
    Serial.print(convert_to_dec(DCM_Matrix[0][1]));
    Serial.print (",");
    Serial.print(convert_to_dec(DCM_Matrix[0][2]));
    Serial.print (",");
    Serial.print(convert_to_dec(DCM_Matrix[1][0]));
    Serial.print (",");
    Serial.print(convert_to_dec(DCM_Matrix[1][1]));
    Serial.print (",");
    Serial.print(convert_to_dec(DCM_Matrix[1][2]));
    Serial.print (",");
    Serial.print(convert_to_dec(DCM_Matrix[2][0]));
    Serial.print (",");
    Serial.print(convert_to_dec(DCM_Matrix[2][1]));
    Serial.print (",");
    Serial.print(convert_to_dec(DCM_Matrix[2][2]));
    #endif*/
    Serial.println();

    }

    long convert_to_dec(float x)
    {
    return x*10000000;
    }

    Bu kodlar ise output kütüphanesine ait .
  • Yukarda paylaştığınız resimdeki çıktılar 3 eksenin açı bilgileri. Roll pitch ve yaw. Modeli Planör gibi düşünürsen kanatların yukarı aşşağı hareketi roll burun ve kuyruğun yukarı aşşağı hareketi pitch sağa sola hareketide yaw. Bu değişkenleride
    x=(ToDeg(roll));

    Y=(ToDeg(pitch));
    Z=(ToDeg(yaw));

    şeklinde atayabilirsin.
    Accelometre bilgilerinide benzer şekilde diferansiyel denklem içine alıp fonksiyonunu oluşturursun.
  • Teşekkür ederim.Birde #if PRINT_ANALOGS ve PRINT_DCM hangi durumlarda 1 olmaktadır ve aşağıdaki kodlarda I2C kütüphanesinde m ve g gibi değerler var ve onlar biyerde tanımlanmamış onlar sabit değer mi acaba.

    #include
    #include

    L3G gyro;
    LSM303 compass;

    void I2C_Init()
    {
    Wire.begin();
    }

    void Gyro_Init()
    {
    gyro.init();
    gyro.enableDefault();
    gyro.writeReg(L3G::CTRL_REG4, 0x20); // 2000 dps full scale
    gyro.writeReg(L3G::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
    }

    void Read_Gyro()
    {
    gyro.read();

    AN[0] = gyro.g.x;
    AN[1] = gyro.g.y;
    AN[2] = gyro.g.z;
    gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
    gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
    gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
    }

    void Accel_Init()
    {
    compass.init();
    compass.enableDefault();
    switch (compass.getDeviceType())
    {
    case LSM303::device_D:
    compass.writeReg(LSM303::CTRL2, 0x18); // 8 g full scale: AFS = 011
    break;
    case LSM303::device_DLHC:
    compass.writeReg(LSM303::CTRL_REG4_A, 0x28); // 8 g full scale: FS = 10; high resolution output mode
    break;
    default: // DLM, DLH
    compass.writeReg(LSM303::CTRL_REG4_A, 0x30); // 8 g full scale: FS = 11
    }
    }

    // Reads x,y and z accelerometer registers
    void Read_Accel()
    {
    compass.readAcc();

    AN[3] = compass.a.x >> 4; // shift left 4 bits to use 12-bit representation (1 g = 256)
    AN[4] = compass.a.y >> 4;
    AN[5] = compass.a.z >> 4;
    accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
    accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
    accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
    }

    void Compass_Init()
    {
    // doesn't need to do anything because Accel_Init() should have already called compass.enableDefault()
    }

    void Read_Compass()
    {
    compass.readMag();

    magnetom_x = SENSOR_SIGN[6] * compass.m.x;
    magnetom_y = SENSOR_SIGN[7] * compass.m.y;
    magnetom_z = SENSOR_SIGN[8] * compass.m.z;
    }

  • //#define PRINT_DCM 0 //Will print the whole direction cosine matrix
    #define PRINT_ANALOGS 0 //Will print the analog raw data
    #define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw

    Burda 0 ları 1 yaparsanız serialden onlarında çıktısını alırsınız ama ortalık biraz karışır yoğun bir veri akışı olur takip edemezsiniz :) çıktı kodlarındaki if şartları sadece seri monitör çıktısı içindir istediğiniz analog ve dcm ise

    Analogdegiskeni1=AN[1]) şeklinde çağırırsınız. Çıktı kodlarından serialprint kısmını ayıklayın yeter.
  • M ve g geçen kısımlar açıklamadakilerse kuvvet birimleri. Açıkçası üzerinde pek çalışmadığım için kodları pek çözemedim. Diferansiyel denklemde accelometre ve gyro dan gelen açı bilgileri yeterli değil mi?
  • Hızı bulmak için diferansiyel denklem yerine iki değer arası farkı alıp t(zaman) ile çarpacağım fakat bu zamanı kendim nasıl ayarlarım ,serial monitördeki iki değer arasındaki geçen süreyi ayarlamam için delay komutu kullanmam kafi midir? Birde genel olarak böyle bir çözüm üretmek yanlış değil,değil mi?
  • Olmaz tabiki. Açı değişimi bir eksen etrafındaki hareketi verir nesne konum değişimi için her Açının vektörünü alacaksın. Trigonometrik bazı fonksiyonlarla konum hesabı yapıp hız hesabını öyle yapacaksınız. Bununla ilgili güzel bir blog vardı. Bulabilirsem paylaşayim.
  • Elimdeki kütüphanede vektörü almakta ama yanlış bişey yapıyor da olabilirim,rica etsem aşağıdaki linkten programa bakabilir misiniz

    https://github.com/pololu/minimu-9-ahrs-arduino/tree/master/MinIMU9AHRS
  • Tamam vectör hesabı yapan fonksiyon çok güzel.http://www.barissamanci.net/Makale/26/accelerometer-gyroscope-imu-nedir/

    Şu blogu güzelce oku konum hesabı için trigonometri formülü var.
  • Teşekkür ederim site gerçekten çok yararlı.Birde programın içinde Accel_Vector[0]= {0,0,0} ve yro_Vector[3]= {0,0,0} ile tanımlanan yerler direk bana vektörel olarak hesaplanmış halini verir mi programa bakma fırsatınız olursa söyler misiniz
  • merhaba arkadaş konumu nasıl buldurabilirim ?

Howdy, Stranger!

It looks like you're new here. If you want to get involved, click one of these buttons!

Login with Facebook

Tagged

Açık kaynak kültürü gereği, çözdüğünüz problemlerin çözümlerini paylaşmayı lütfen unutmayın.