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ử
In bài này
Previous
Next Post »

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!! ConversionConversion EmoticonEmoticon

       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

THƯ MỤC KHO TÀI LIỆU MIỄN PHÍ ECHIPKOOL

Code 8051 - ASM Code 8051 - C Code AVR - C Code led sao băng Code PIC - C Điện tử cơ bản điện tử viễn thông Đo Nhiệt Độ DS18B20 + LCD Đo Nhiệt Độ LM35 + LCD Đo Nhiệt Độ LM35 + Led 7 thanh Đo tốc độ động cơ Động cơ robo Ebook Đại Học ebook điện tử Ebook đồ án Học Orcad Học Protues Hồng ngoại Lập Trình 8051 Lập Trình AVR lập trình c++ Lập Trình Led Quảng Cáo lập trình PIC Lập trình Robot Lập trình VHDL Lcd16x2 Led Clock Led Quay Led RGB Mạch 7seg Mạch Amply.Mạch Loa Mạch Cảm Biến Mạch cube Mạch Đếm Sản Phẩm Mạch điện cơ bản Mạch điện hay Mạch Điện Ứng Dụng Mạch đọc file nhạc MP3 dùng Atmega 8 Mạch Động Cơ Mạch đồng hồ Mạch đồng hồ LCD Mạch đồng hồ Matrix Mạch giao thông Mạch in Mạch khóa số điện tử Mạch Led đơn Mạch Led Quảng Cáo Mạch Led Vumeter Mạch Ma trận Phím Mạch Matrix Mạch nạp Mạch nguồn Mạch Nút Bấm Mạch RS232 Mạch RS485 Mạch thu phát Mạch tổ hợp MSI Mạch trái tim Mạch truyền điện không dây Mạch Vi điều khiển Module Bluetooth Module Sim Module Sim548 Motor Nhiệt độ - Độ ẩm oscilloscope Phần mềm điện tử Phần Mềm Diệt Viruts Phần Mềm Hay Phần Mềm Led Quảng Cáo Phần mềm vi tính robocon Rule robocon Sạc Acquy Sản Phẩm Thương Mại Sáng tạo Smart Home Tài liệu Điện Tử Tranzitor Tụ điện TUT - 8051 - ASM TUT - 8051 - KeilC UART Ứng Dụng Led Quảng Cáo Ứng dụng USB USB TO COM Vi điều khiển - Ứng dụng Vi mạch số VOM Wifi ESP8266