Sensör için kod yazdım fikirlerinizi öğrenebilir miyim?

Katılım
26 Eki 2018
Mesajlar
60
Tepkime puanı
4
Puanları
8
Konum
Aksaray
Sensör için kod için yazdım. Fakat şuan arduino için biraz acemi sayılırım önerileriniz var mı, şöyle yaparsan daha iyi olur diye?

Kod:
C++:
 #include <Wire.h>

  #define CTRL_REG1 0x20
  #define CTRL_REG2 0x21
  #define CTRL_REG3 0x22
  #define CTRL_REG4 0x23
  #define CTRL_REG5 0x24


  int L3G4200D_Address = 0x69; // sensörün I2C addres yeri

  float DPS_carpim = 0.0000085; // Can fine-tune this if you need to

  // Delta angles (raw input from gyro)
  int x = 0;
  int y = 0;
  int z = 0;

  // gerçek ölçümler
  float angX = 0;
  float angY = 0;
  float angZ = 0;

  // hesaplanılan önceki açı değerleri
  float p_angX = 0;
  float p_angY = 0;
  float p_angZ = 0;

  // kalibrasyon değerleri
  int gyroLowX = 0;
  int gyroLowY = 0;
  int gyroLowZ = 0;
  int gyroHighX = 0;
  int gyroHighY = 0;
  int gyroHighZ = 0;

  float kalman = 0.25;
  unsigned long pastMicros = 0;

float kalman_old_x=0;
float kalman_old_y=0;
float kalman_old_z=0;
float cov_old_x=1;
float cov_old_y=1;
float cov_old_z=1;
float kalman_calculated_x;
float kalman_calculated_y;
float kalman_calculated_z;

void setup() {

  Wire.begin();
  Serial.begin(9600);
  setupL3G4200D(250);
  Serial.println("L3G4200D baslatiliyor! ");
  calibrate();//kalibre et
  attachInterrupt(0, updateAngle, RISING);//açılar update olunca kesime gir, artarak
  pastMicros = micros();// geçen süre
  delay(300); //sensörü bekliyoruz
}

void loop() {


  getGyroValues();
  kalman_calculated_X();
  kalman_calculated_Y();
  kalman_calculated_Z();


  // delta zamanını hesaplama
  float dt;
  if(micros() > pastMicros) //micros() overflows every ~70 minutes
    dt = (float) (micros()-pastMicros)/1000000.0;
  else
    dt = (float) ((4294967295-pastMicros)+micros())/1000000.0;

    pastMicros = micros();
   
  // Calculate angles
  if(x >= gyroHighX || x <= gyroLowX) {
    angX += ((p_angX + (x * DPS_carpim))/2) * dt;
    p_angX = x * DPS_carpim;
  } else {
    p_angX = 0;
  }

   
  if(y >= gyroHighY || y <= gyroLowY) {
    angY += ((p_angY + (y * DPS_carpim))/2) * dt;
    p_angY = y * DPS_carpim;
  } else {
    p_angY = 0;
  }


  if(z >= gyroHighZ || z <= gyroLowZ) {
    angZ += ((p_angZ + (z * DPS_carpim))/2) * dt;
    p_angZ = z * DPS_carpim;
  } else {
    p_angZ = 0;
  }



  Serial.println("angX:   ");
  Serial.print(angX);
  Serial.println("kalman_calculated_X:   ");
  Serial.print(kalman_calculated_x);
  Serial.println("***********************");
  Serial.println("angY:   ");
  Serial.print(angY);
  Serial.println("kalman_calculated_Y:   ");
  Serial.print(kalman_calculated_y);
  Serial.println("***********************");
  Serial.println("angZ:   ");
  Serial.print(angZ);
  Serial.println("kalman_calculated_Z:   ");
  Serial.print(kalman_calculated_z);
  Serial.println("***********************");


  delay(10);

}

void kalman_calculated_X(){
  float kalman_new_x = kalman_old_x; // eski değer alınır
  float cov_new_x = cov_old_x + 0.50; //yeni kovaryans değeri belirlenir. Q=0.50 alınmıştır

  float kalman_gain_x = cov_new_x / (cov_new_x + 0.9); //kalman kazancı hesaplanır. R=0.9 alınmıştır
  float kalman_calculated_x = kalman_new_x + (kalman_gain_x * (angX - kalman_new_x)); //kalman değeri hesaplanır

  cov_new_x = (1 - kalman_gain_x) * cov_old_x; //yeni kovaryans değeri hesaplanır
  cov_old_x = cov_new_x; //yeni değerler bir sonraki döngüde kullanılmak üzere kaydedilir

  kalman_old_x = kalman_calculated_x;
  return kalman_calculated_x; //hesaplanan kalman değeri çıktı olarak verilir
}

void kalman_calculated_Y(){
  float kalman_new_y = kalman_old_y; // eski değer alınır
  float cov_new_y = cov_old_y + 0.50; //yeni kovaryans değeri belirlenir. Q=0.50 alınmıştır

  float kalman_gain_y = cov_new_y / (cov_new_y + 0.9); //kalman kazancı hesaplanır. R=0.9 alınmıştır
  float kalman_calculated_y = kalman_new_y + (kalman_gain_y * (angX - kalman_new_y)); //kalman değeri hesaplanır

  cov_new_y = (1 - kalman_gain_y) * cov_old_y; //yeni kovaryans değeri hesaplanır
  cov_old_y = cov_new_y; //yeni değerler bir sonraki döngüde kullanılmak üzere kaydedilir

  kalman_old_y = kalman_calculated_y;
  return kalman_calculated_y; //hesaplanan kalman değeri çıktı olarak verilir
}

void kalman_calculated_Z(){
  float kalman_new_z = kalman_old_z; // eski değer alınır
  float cov_new_z = cov_old_z + 0.50; //yeni kovaryans değeri belirlenir. Q=0.50 alınmıştır

  float kalman_gain_z = cov_new_z / (cov_new_z + 0.9); //kalman kazancı hesaplanır. R=0.9 alınmıştır
  float kalman_calculated_z = kalman_new_z + (kalman_gain_z * (angX - kalman_new_z)); //kalman değeri hesaplanır

  cov_new_z = (1 - kalman_gain_z) * cov_old_z; //yeni kovaryans değeri hesaplanır
  cov_old_z = cov_new_z; //yeni değerler bir sonraki döngüde kullanılmak üzere kaydedilir

  kalman_old_z = kalman_calculated_z;
  return kalman_calculated_z; //hesaplanan kalman değeri çıktı olarak verilir
}




void updateAngle()
{
  getGyroValues();
}

void calibrate()
{
  Serial.println("Calibrating gyro, don't move!");
  for(int i = 0; i < 200; i++) {
    getGyroValues();

    if(x > gyroHighX)
      gyroHighX = x;
    else if(x < gyroLowX)
      gyroLowX = x;

    if(y > gyroHighY)
      gyroHighY = y;
    else if(y < gyroLowY)
      gyroLowY = y;

    if(z > gyroHighZ)
      gyroHighZ = z;
    else if(z < gyroLowZ)
      gyroLowZ = z;
   
    delay(10);
  }
  Serial.println("Calibration complete.");
}

void getGyroValues() {
  byte xMSB = readRegister(L3G4200D_Address, 0x29);
  byte xLSB = readRegister(L3G4200D_Address, 0x28);
  x = ((xMSB << 8) | xLSB);

  byte yMSB = readRegister(L3G4200D_Address, 0x2B);
  byte yLSB = readRegister(L3G4200D_Address, 0x2A);
  y = ((yMSB << 8) | yLSB);

  byte zMSB = readRegister(L3G4200D_Address, 0x2D);
  byte zLSB = readRegister(L3G4200D_Address, 0x2C);
  z = ((zMSB << 8) | zLSB);
}

int setupL3G4200D(int scale) {
  //From  Jim Lindblom of Sparkfun's code

  // Enable x, y, z and turn off power down:
  writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);

  // If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
  writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);

  // Configure CTRL_REG3 to generate data ready interrupt on INT2
  // No interrupts used on INT1, if you'd like to configure INT1
  // or INT2 otherwise, consult the datasheet:
  writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);

  // CTRL_REG4 controls the full-scale range, among other things:

  if(scale == 250) {
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
  } else if(scale == 500) {
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
  } else {
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
  }

  // CTRL_REG5 controls high-pass filtering of outputs, use it
  // if you'd like:
  writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
}

void writeRegister(int deviceAddress, byte address, byte val)
{
    Wire.beginTransmission(deviceAddress); // I2C aktarımı başlatılıyor
    Wire.write(address); //registerin yazılacak adresi belirleniyor      
    Wire.write(val); // istenilen değer yazılıyor    
    Wire.endTransmission(); // I2C aktarımı durduruluyor  
}

int readRegister(int deviceAddress, byte address)
{
    int v;
    Wire.beginTransmission(deviceAddress);// I2C aktarımı başlatılıyor
    Wire.write(address); //registerin yazılacak adresi belirleniyor  
    Wire.endTransmission(); // I2C aktarımı durduruluyor
    Wire.requestFrom(deviceAddress, 1); // adresten bir bayt okunuyor

    while(!Wire.available()) {
        // bekleniyor
    }
    v = Wire.read();
    return v;
}
 
Üst