Kendini Dengeleyen Robot - Self Balancing Robot

Konu, 'Arduino' kısmında ekilali tarafından paylaşıldı.

  1. ekilali

    ekilali MB Üyesi

    Kayıt:
    11 Ekim 2015
    Mesajlar:
    15
    Beğeniler:
    9
    Meslek:
    Mekatronik Mühendisi
    En İyi Cevap:
    0
    Değerlendiriler:
    +15 / 0 / -0
    Üniversite:
    FIRAT ÜNİVERSİTESİ
    Merbahalar,

    Üniversitede kapalı çevrim kontrol sistemi ödevi için yaptığımız yabancı kaynaklardan da destek alarak yapmış olduğumuz dengele robotu ödev, tez, bitirme vb. projelerinizde kullanabilirsiniz. Umarım faydasını görürsünüz.

    > Malzemeler;
    • Arduino Uno
    • GY-80 BMP085 (ADXL345) IMU Sensör
    • L298N Motor Sürücü
    • 5V Redüktörlü Motor
    • Tekerlek
    • 7.4V Lipo Pil
    • Mekanik aksam
    > Mekanik Aksam
    20141219_142448.jpg 20141219_142454.jpg
    Resimde görüldüğü gibi robotu oluşturan komponentleri üç rafa bölerek ağırlık merkezini ortada dengeli bir şekilde yaymayı amaçladık. Rafları oluşturan parçalar lazer kesim sayesinde pleksiglas malzemesinden çıkartılmıştır. Referans çizimi Auto-Cad veya SolidWorkst vb. çizerek *.dwg uzantıda kayıt etmeniz yeterli. Lazer kesimi tabela reklam işi yapan yerlerde bulmanız mümkün, iki farklı şehirde kesim yaptırdım ortalama kesimin dakikası 3TL gibi bir fiyata mâl olmaktadır.
    Çizimi, kartların büyüklüğüne göre fazlada büyük olmayacak şekilde uygun bir ölçüde gerçekleştirebilirsiniz.
    *Resimde en üst rafta arduino uno *orta rafta motor sürücü *alt raftada lip pilo ve sensör bulunmaktadır.

    >Bağlantı Şeması


    20141219_1424499.png >>>>Yazılım ;
    Derlemede hatalar çıkabilir. Benim yaşadığım en sık problem derleyicinin kütüphaneleri görmemesiydi, bu yüzden kütüphaneleri tekrar import etmeniz gerekebilir.
    • Elde bu kadarlık bir görüntü kaldı, Ödevi bitirdikten sonra okula teslim ettiğimiz için video kayıt yapmaya vakit kalmadı.
    **Robotu dik konumda iken arduino unoya reset attığımızda daha robot daha stabil çalışmaktadır
    Kod:
    #include <Wire.h>
    #include <Kalman.h>
    #include <Adafruit_Sensor.h>
    #include <Adafruit_ADXL345_U.h>
    
    
    Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345);
    
    #define CTRL_REG1 0x20
    #define CTRL_REG2 0x21
    #define CTRL_REG3 0x22
    #define CTRL_REG4 0x23
    #define CTRL_REG5 0x24
    
    #define runEvery(t) for (static long _lasttime;\
                             (uint16_t)((uint16_t)millis() - _lasttime) >= (t);\
                             _lasttime += (t))
                      
    int L3G4200D_Address = 105;
    
    
    //Set up kalman instances
    Kalman kalmanPitch;
    Kalman kalmanRoll;
    
    
    //Set up accelerometer variables
    int16_t accValX, accValY, accValZ;
    float accBiasX, accBiasY, accBiasZ;
    float accAngleX, accAngleY;
    double accPitch, accRoll;
    
    //Set up gyroscope variables
    int16_t gyroX, gyroY, gyroZ;
    float gyroBiasX, gyroBiasY, gyroBiasZ;
    float gyroRateX, gyroRateY, gyroRateZ;
    float gyroBias_oldX, gyroBias_oldY, gyroBias_oldZ;
    float gyroPitch = 180;
    float gyroRoll = -180;
    float gyroYaw = 0;
    double gyro_sensitivity = 70; //From datasheet, depends on Scale, 2000DPS = 70, 500DPS = 17.5, 250DPS = 8.75.
    
    
    int x;
    int y;
    int z;
    //Set up a timer Variable
    uint32_t timer;
    
    // valores angulos
    double InputPitch, InputRoll;
    
    // Valores iniciales
    double PitchInicial,RollInicial;
    
    // Motores
    int enablea = 3;
    int enableb = 9;
    int a1 = 5;
    int a2 = 4;
    int b1 = 10;
    int b2 = 11;
    
    
    void setup() {
    
      Wire.begin();
    
      Serial.begin(9600);
      setupL3G4200D(200); // Configure L3G4200  - 250, 500 or 2000 deg/sec
      delay(1500);
    
      if(!accel.begin())
      {
        /* There was a problem detecting the ADXL345 ... check your connections */
        Serial.println("Ooops, no ADXL345 detected ... Check your wiring!");
        while(1);
      }
    
        // Motor
      pinMode(enablea, OUTPUT);
      pinMode(enableb, OUTPUT);
      pinMode(a1, OUTPUT);
      pinMode(a2, OUTPUT);
      pinMode(b1, OUTPUT);
      pinMode(b2, OUTPUT);
      digitalWrite(a1,HIGH);
      digitalWrite(a2,HIGH);
      digitalWrite(b1,HIGH);
      digitalWrite(b2,HIGH);
    
      accel.setRange(ADXL345_RANGE_2_G);
      sensors_event_t event;
      accel.getEvent(&event);
    
      // Calculate bias for the Gyro i.e. the values it gives when it's not moving
      for(int i=1; i < 100; i++){
    
        getGyroValues();
        gyroBiasX += (int)x;
        gyroBiasY += (int)y;
        gyroBiasZ += (int)z;
    
        accel.getEvent(&event);
        accBiasX += event.acceleration.x;
        accBiasY += event.acceleration.y;
        accBiasZ += event.acceleration.z;
    
        delay(1);
      }
    
    
      gyroBiasX = gyroBiasX / 100;
      gyroBiasY = gyroBiasY / 100;
      gyroBiasZ = gyroBiasZ / 100;
    
      accBiasX = accBiasX / 100;
      accBiasY = accBiasY / 100;
      accBiasZ = accBiasZ / 100;
    
    
      //Get Starting Pitch and Roll
      accel.getEvent(&event);
      accPitch = (atan2(-event.acceleration.x,-event.acceleration.z)+PI)*RAD_TO_DEG;
      accRoll = (atan2(event.acceleration.y,-event.acceleration.z)+PI)*RAD_TO_DEG;
    
      if (accPitch <= 360 & accPitch >= 180){
        accPitch = accPitch - 360;
      }
    
      if (accRoll <= 360 & accRoll >= 180){
        accRoll = accRoll - 360;
      }
    
    
      // Set starting angle for Kalman
      kalmanPitch.setAngle(accPitch);
      kalmanRoll.setAngle(accRoll);
    
      kalmanRoll.setQangle(0.01);      // 0.001
      kalmanRoll.setQbias(0.0003);    // 0.003
      kalmanRoll.setRmeasure(0.01);    // 0.03
    
      gyroPitch = accPitch;
      gyroRoll = accRoll;
    
      timer = micros();
      delay(1000);
      ValoresIniciales();
    
    }
    
    double tau=0.075;
    double a=0.0;
    double x_angleC = 0;
    
    double Complementary(double newAngle, double newRate,double looptime) {
    
      double dtC = float(looptime)/1000000.0;
      a=tau/(tau+dtC);
      x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
      return x_angleC;
    
    }
    
    double Setpoint;
    
    void MotorControl(double out){
    
      if (out > 0){
        digitalWrite(a1,HIGH);
        digitalWrite(a2,LOW);
        digitalWrite(b1,LOW);
        digitalWrite(b2,HIGH);
      }else{
        digitalWrite(a1,LOW);
        digitalWrite(a2,HIGH);
        digitalWrite(b1,HIGH);
        digitalWrite(b2,LOW);
      }
    
      byte vel = abs(out);
      if (vel<0)
        vel=0;
      if (vel > 255)
        vel=255;
    
      //Serial.println(out);
      analogWrite(enablea,vel);
      analogWrite(enableb,vel);
    }
    
    void ValoresIniciales(){
    
      //////////////////////
      //  Accelerometer   //
      //////////////////////
      sensors_event_t event;
    
      accel.getEvent(&event);
      accPitch = (atan2(-event.acceleration.x,-event.acceleration.z)+PI)*RAD_TO_DEG;
      accRoll = (atan2(event.acceleration.y,-event.acceleration.z)+PI)*RAD_TO_DEG;
    
      if (accPitch <= 360 & accPitch >= 180){
        accPitch = accPitch - 360;
      }
    
      if (accRoll <= 360 & accRoll >= 180){
        accRoll = accRoll - 360;
      }
    
    
      //////////////////////
      //      GYRO        //
      //////////////////////
    
    
      getGyroValues();
    
      // read raw angular velocity measurements from device
      gyroRateX = ((int)x - gyroBiasX)*.07; //*(.0105);
      gyroRateY = -((int)y - gyroBiasY)*.07; //*(.0105);
      gyroRateZ = ((int)z - gyroBiasZ)*.07; //*(.0105);
    
      gyroPitch += gyroRateY * ((double)(micros() - timer)/1000000);
      gyroRoll += gyroRateX * ((double)(micros() - timer)/1000000);
      gyroYaw += gyroRateZ * ((double)(micros() - timer)/1000000);
    
      PitchInicial = kalmanPitch.getAngle(accPitch, gyroPitch, (double)(micros()-timer)/1000000);
      //RollInicial = kalmanRoll.getAngle(accRoll, gyroRoll, (double)(micros()-timer)/1000000);
      timer = micros();
      RollInicial = gyroRoll;
    
      Serial.print("Pitch Inicial: ");
      Serial.println(PitchInicial);
      Serial.print("Roll Inicial: ");
      Serial.println(RollInicial);
      Setpoint = 0;
    
    }
    
    int i=0;
    double aaa =0;
    void loop() {
    
      runEvery(10){
        //////////////////////
        //  Accelerometer   //
        //////////////////////
        sensors_event_t event;
    
        accel.getEvent(&event);
        accPitch = (atan2(-event.acceleration.x,-event.acceleration.z)+PI)*RAD_TO_DEG;
        accRoll = (atan2(event.acceleration.y,-event.acceleration.z)+PI)*RAD_TO_DEG;
    
        if (accPitch <= 360 & accPitch >= 180){
          accPitch = accPitch - 360;
        }
    
        if (accRoll <= 360 & accRoll >= 180){
          accRoll = accRoll - 360;
        }
    
    
        //////////////////////
        //      GYRO        //
        //////////////////////
    
    
        getGyroValues();
    
        // read raw angular velocity measurements from device
        gyroRateX = -((int)x - gyroBiasX)*.07; //*(.0105);
        gyroRateY = -((int)y - gyroBiasY)*.07; //*(.0105);
        gyroRateZ = ((int)z - gyroBiasZ)*.07; //*(.0105);
    
        gyroPitch += gyroRateY * ((double)(micros() - timer)/1000000);
        double leeeeel = gyroRateX * ((double)(micros() - timer)/1000000);
        gyroRoll += gyroRateX * ((double)(micros() - timer)/1000000);
        gyroYaw += gyroRateZ * ((double)(micros() - timer)/1000000);
    
        InputPitch = kalmanPitch.getAngle(accPitch, gyroPitch, (double)(micros()-timer)/1000000);
        InputRoll = kalmanRoll.getAngle(accRoll, gyroRoll, (double)(micros()-timer)/1000000);
        timer = micros();
    
    
        //angle = (0.98)*(angle + gyroRateX * dt) + (0.02)*(accRoll);
    
        //Serial.write(57);
        //Serial.write((byte)accRoll);
        byte a= map(abs(Compute(InputRoll-RollInicial)),0,255,0,124);
        Serial.println(Setpoint);
        //Serial.write((byte)(InputRoll-RollInicial));
        //Serial.write((byte)accRoll);
        //Serial.write((byte)gyroRoll);
        aaa = 0.98* (aaa + leeeeel) + 0.02 * (accRoll);
        //Serial.write((byte)aaa);
        //Serial.write((byte)InputPitch);
        //Serial.write((byte)gyroRateY);
        //Serial.write((byte)InputRoll);
        //Serial.write((byte)heading);
        //MostrarDatos();
        MotorControl(Compute(aaa-RollInicial));
    
       // i++;
        //if (i=100){
          //i=0;
          //gyroRoll = (InputRoll-RollInicial);
        //}
    
      }
    
      if (Serial.available()){
        char a = Serial.read();
        switch (a){
          case 'q':
            Setpoint += 0.01;
          break;
          case 'w':
            Setpoint -= 0.01;
          break;
          case 'e':
            Setpoint += 0.1;
          break;
          case 'r':
            Setpoint -= 0.1;
          break;
        }
      }
    
    }
    
    void MostrarDatos(){
      Serial.print("DATOS: ");
      Serial.print("RollInicial: ");
      Serial.print(RollInicial);
      Serial.print("Roll: ");
      Serial.print(InputRoll);
      Serial.print("RollBueno: ");
      Serial.println(InputRoll-RollInicial);
    
    }
    
    
    int outMax = 255;
    int outMin = -255;
    float lastInput = 0;
    double ITerm =0;
    double kp = 100;
    double ki = 2;
    double kd = 0;
    
    double Compute(double input)
    {
    
          double error = Setpoint - input;
          ITerm+= (ki * error);
          if(ITerm > outMax) ITerm= outMax;
          else if(ITerm < outMin) ITerm= outMin;
          double dInput = (input - lastInput);
          /*Compute PID Output*/
          double output = kp * error + ITerm + kd * dInput;
    
          if(output > outMax) output = outMax;
          else if(output < outMin) output = outMin;
    
          /*Remember some variables for next time*/
          lastInput = input;
          return output;
    }
    
    
    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); // start transmission to device
        Wire.write(address);       // send register address
        Wire.write(val);         // send value to write
        Wire.endTransmission();     // end transmission
    }
    
    int readRegister(int deviceAddress, byte address){
    
        int v;
        Wire.beginTransmission(deviceAddress);
        Wire.write(address); // register to read
        Wire.endTransmission();
    
        Wire.requestFrom(deviceAddress, 1); // read a byte
    
        while(!Wire.available()) {
            // waiting
        }
    
        v = Wire.read();
        return v;
    }
    
    Saygılar
    
    
    Yazılımı aşağıdaki ekten indirebilirsiniz.​
     

    Ekli Dosyalar:

    • Beğen Beğen x 6
    • Bilgilendirici Bilgilendirici x 1
    • Yardımcı Yardımcı x 1
  2. Oğuzhan Mallı

    Oğuzhan Mallı Yetkili Kişi Genel Yönetici

    Kayıt:
    6 Şubat 2015
    Mesajlar:
    511
    Beğeniler:
    408
    Meslek:
    Nükleer ve Atom Mühendisi
    En İyi Cevap:
    11
    Değerlendiriler:
    +512 / 5 / -1
    Üniversite:
    MEPhI, KTÜ
    Güzel bir proje. Elinize sağlık.
     
    • Beğen Beğen x 1
  3. ekilali

    ekilali MB Üyesi

    Kayıt:
    11 Ekim 2015
    Mesajlar:
    15
    Beğeniler:
    9
    Meslek:
    Mekatronik Mühendisi
    En İyi Cevap:
    0
    Değerlendiriler:
    +15 / 0 / -0
    Üniversite:
    FIRAT ÜNİVERSİTESİ
    Teşekkür ederim
     
  4. Gookhan

    Gookhan MB Üyesi

    Kayıt:
    8 Kasım 2015
    Mesajlar:
    15
    Beğeniler:
    1
    En İyi Cevap:
    1
    Değerlendiriler:
    +3 / 0 / -0
    Üniversite:
    Atatürk Üniversitesi
    Tebrikler, başarılar.
     
    • Katılıyorum Katılıyorum x 1
  5. Cemal Okka

    Cemal Okka MB Üyesi

    Kayıt:
    12 Kasım 2015
    Mesajlar:
    4
    Beğeniler:
    0
    En İyi Cevap:
    0
    Değerlendiriler:
    +0 / 0 / -0
    Üniversite:
    Mevlana Üniversitesi
    Bence daha geliştirilebilir bir proje, başlangıç için gayet hoş olmuş.