Mạch Khóa Số Điện Tử Mạch Đếm Sản Phẩm Mạch Đèn giao thông Ngã Tư Mạch Trái Tim Final Mạch Trái Tim Final Mạch Trái Tim I Love U

April 4, 2016

Robot 2 bánh tự cân bằng


Robot 2 bánh tự cân bằng dựa trên mô hình con lắc ngược là một đối tượng phi tuyến với các tham số bất định khó điều khiển với 6 biến trạng thái. Đặc điểm nổi bật của Robot 2 bánh tự cân bằng là cơ chế tự cân bằng, giúp cho xe dù chỉ có một trục chuyển động với hai bánh nhưng luôn ở trạng thái cân bằng.
Có rất nhiều công trình nghiên cứu về xe hai bánh tự cân bằng, nghiên cứu điều khiển xe 2 bánh tự cân bằng dùng giải thuật cuốn chiếu (backstepping control), giải thuật điều khiển trượt (sliding mode control), LQR, phương pháp điều khiển thông minh fuzzy, noron [8] cho thấy khả năng thích nghi và hiệu quả của những giải pháp điều khiển.

Để điều khiển robot 2 bánh tự cân bằng theo các bước sau:
1/ Mô hình hóa robot 2 bánh
 
2/ Tìm phương trình mô tả trạng thái robot
 
3/ Mô phỏng matlab theo luật điều khiển muốn điều khiển - tìm Udk
 
K_u  =  1.7946   -0.0030    1.8764    0.2079

4/ Thiết kế phần cứng
Board điều khiển trung tâm: Arduino Mega2560

Board điều khiển motor: L298N
 
Cảm biến góc nghiêng
 
Khung robot
 
Kết nối phần cứng
Board Arduino
Mega 2560 Chức năng Kết nối Ghi chú
Chân 2 Input Encode motor  
Chân 3 Input Encode motor  
Chân 4 Output Chân input L298N – IN1 Output L298N nối motor
Chân 5 Output Chân input L298N – IN2
Chân 6 Output Chân input L298N – IN3
Chân 7 Output Chân input L298N – IN4
Chân 9 Output Chân EA - L298N  
Chân 10 Output Chân EB - L298N  
Chân 20 Input Chân SDA cảm biến Gyro Giao tiếp chuẩn I2C
Chân 21 Input Chân SCL cảm biến Gyro

5/ Viết code arduino theo luật điều khiển

 

    B1. Code điều khiển xe 2 bánh tự cân bằng dùng phương pháp PID
#include <Kalman.h>
#include<Servo.h>
#include<Wire.h>
#include<I2Cdev.h>
#include<MPU6050.h>
MPU6050 CBgoc;
Kalman kalmanX;
//IMU 6050====================================================
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
float accXangle;
float gyroXangel;
float kalAngelX;
unsigned long timer;
uint8_t i2cData[14];
float CurrentAngle;
// MOTOR====================================================
int AIN1 = 4;
int AIN2 = 5;
int BIN1 = 6;
int BIN2 = 7;
int CIN1 = 9;
int CIN2 = 10;
int speed;
// PID====================================================
const float Kp = 30;
const float Ki = 1;
const float Kd = 8;
float pTerm, iTerm, dTerm, integrated_error, last_error, error;
const float K = 1.9*1.12;
#define   GUARD_GAIN   10.0
#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))
void setup() 
{
pinMode(AIN1, OUTPUT); 
pinMode(AIN2, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
Serial.begin(9600);
Wire.begin();
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz 
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling 
i2cData[2] = 0x00;
i2cData[3] = 0x00;
while(i2cWrite(0x19,i2cData,4,false)); 
while(i2cWrite(0x6B,0x01,true));
while(i2cRead(0x75,i2cData,1));
if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while(1);
  }
delay(100); 
//Kalman====================================================
while(i2cRead(0x3B,i2cData,6));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
kalmanX.setAngle(accXangle); 
gyroXangel = accXangle; 
timer = micros();
 }
void loop()
{
Serial.println(accX);
 // Serial.println(accY);
 // Serial.println(accZ);
Serial.println(accXangle);
Serial.println(CurrentAngle);
runEvery(25)
  {
dof();
if(CurrentAngle <=179 && CurrentAngle >=178.5)
    {
stop();
    }
else
    {
if(CurrentAngle < 230 && CurrentAngle >130)
      {
Pid();
Motors();
      }
else
      {
stop();
      }
    }
  } 
}
void Motors()
{
if(speed > 0)
  {
analogWrite(CIN1, speed);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
analogWrite(CIN2, speed);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
   }
else
   {
speed = map(speed,0,-255,0,255);
analogWrite(CIN1, speed);
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
analogWrite(CIN2, speed);
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
   }
}
void stop()
{
speed = map(speed,0,-150,0,150);
analogWrite(CIN1, speed);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
analogWrite(CIN2, speed);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
}
void Pid()
{
error = 180 - CurrentAngle;  // 180 = level
pTerm = Kp * error;
  integrated_error += error;
iTerm = Ki*constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd*(error - last_error);
  last_error = error;
speed = constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}
void dof()
{
while(i2cRead(0x3B,i2cData,14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = ((i2cData[6] << 8) | i2cData[7]);  
gyroX = ((i2cData[8] << 8) | i2cData[9]);
gyroY = ((i2cData[10] << 8) | i2cData[11]);
gyroZ = ((i2cData[12] << 8) | i2cData[13]);
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gyroX/131.0;
   CurrentAngle = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);
timer = micros();
}

 Nguồn sưu tầm
Điện Tử eChipKool.Com - Chia sẻ kiến thức - Kết nối đam mê điên tử

0 comments:

Post a Comment

Bạn đọc hãy giúp chúng tôi xây dựng cộng đồng bằng cách để lại bình luận, chúng tôi luôn đón nhận mọi ý kiến của các bạn:
» Bình luận nghiêm túc và không chứa các liên kết quảng cáo.
» Vui lòng không Spam nhận xét với mọi hình thức.
» Rất mong bạn đề tên cho nhận xét của chính mình - Bằng cách chọn vào Tên/URL và điền tên bạn vào (Phần URL có thể bỏ trống ).

- Bạn có thể chèn Link nhạc (NCT), video(Youtube),Hình ảnh vào comment bằng cú pháp:
+ [youtube] Link video Youtube [/youtube].
+ [img] Link ảnh( định dạng PNG, JPG,GIF) [/img]
+Chèn link liên kết: <a href="LINK" rel="nofollow">Name</a>
-Bạn copy mã bên cạnh biểu tượng chèn vào nhận xét để bày tỏ cảm xúc!!

:) :( :)) :(( =)) =D> :D :P