Kendini Dengeleyen Robot - Self Balancing Robot

ekilali · 11 Kasım 2015

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

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ı

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

Oğuzhan Mallı · 11 Kasım 2015

Güzel bir proje. Elinize sağlık.

ekilali · 11 Kasım 2015

Teşekkür ederim

Gookhan · 12 Kasım 2015

Tebrikler, başarılar.

Cemal Okka · 12 Kasım 2015

Bence daha geliştirilebilir bir proje, başlangıç için gayet hoş olmuş.

Kendini Dengeleyen Robot - Self Balancing Robot

ekilali

Dosyalar

Oğuzhan Mallı

ekilali

Gookhan

Cemal Okka