Trong tương lai mô hình robot 2 bánh tự cân bằng sẽ được phát triển như sau: Mô hình sẽ được điều khiển thông qua máy tính nhờ kết nối bluetooth và giao diện trên máy tính sẽ thân thiện trực quan hơn.
Tối ưu hóa khối lượng, thuật toán để mô hình có thể hoạt động trơn tru, linh hoạt, dễ điều khiển, tiết kiệm năng lượng.
Có thể dùng thêm bộ encoder để xe hoạt động ổn định.
Có thể gắn thêm camera và gps để robot có thể định vị, ghi hình, xử lý ảnh và tự hoạt động trong không gian lớn.
Mô hình robot 2 bánh tự cân bằng là tiền đề để phát triển thành xe 2 bánh tự cân bằng có thể chở được người và di chuyển linh hoạt hơn.
61
Tài liệu tham khảo
[1] Huỳnh Thái Hoàng, Lý thuyết điều khiển tự động, Nhà xuất bản Đại học Quốc Gia,2005 [2] Nguy ễn Phùng Quang; “ Matlab và Simulink”; NXB khoa học và kỹ thuật.
[3] Dương Hoài Nghĩa; “Điều khiển hệ thống đa biến”; Nhà xuất bản Đại học Quốc Gia TP.HCM, 2007.
[4] Phạm Quang Huy và Nguyễn Trọng Hiếu. Vi điều khiển và ứng dụng arduino. Nhà xuất bản bách khoa Hà Nội, 2013
[5] http://codientu.org/v [6] http://vi.wikipedia.org/wiki/Segway_PT [7] http://arduino.vn/bai-viet/893 [8]http://www.invensense.com/products/motion-tracking/6-axis/mpu-6050/ [9] http://components101.com/sensors/mpu6050-module [10] http://fr.slideshare.net/ThanhTngNg/cm-bin-gia-tc [11]http://create.arduino.cc/projecthub/twob/self-balancing-robot-using-blubug- 8894c6 [12] http://www.instructables.com/id/2-Wheel-Self-Balancing-Robot-by usingArduino-a
62
Code điều khiển
//khai bao thu vien #include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <SoftwareSerial.h> //giao tiếp giữa arduino và mạch serial khác #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h"
#endif
#define OUTPUT_READABLE_YAWPITCHROLL //đầu ra có thể đọc được góc YAWPITCHROLL
MPU6050 mpu;
SoftwareSerial bluetooth(3, 4); // RX, TX
#define INTERRUPT_PIN 2 // sử dụng pin 2-ngắt #define LED_PIN 13 // khi led 13 sáng,xe hoạt động bool blinkState = false;//trạng thái nhấp nháy
uint16_t count_imu_ready=0; // Điều khiển MPU
bool dmpReady = false; // đặt true nếu DMP init thành công uint8_t mpuIntStatus; // giữ byte trạng thái ngắt thực tế từ MPU
uint8_t devStatus; // trả về trạng thái sau mỗi lần vận hành thiết bị (0 = thành công,! 0 = lỗi)
uint16_t packetSize; // đếm tất cả các byte hiện có trong FIFO uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // Bộ đệm lưu trữ FIFO
// các biến định hướng,chuyển động
Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] đo cảm biến gia tốc
VectorInt16 aaReal; // [x, y, z] các phép đo cảm biến gia tốc không trọng lực VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity; // [x, y, z] vector trọng lực
63
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector // packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; //khai báo góc nghiêng các trục x,y,z
int32_t sumGyroX = 0 , sumGyroY = 0 , sumGyroZ = 0; int16_t gyroOffsetX = 0, gyroOffsetY = 0, gyroOffsetZ = 0; bool imu_ready = false;
volatile bool mpuInterrupt = false; //cho biết chân ngắt MPU có tăng cao hay không void dmpDataReady() {
mpuInterrupt = true; }
//khai báo các chân arduino kết nối với L298 #define leftMotor1Pin 7 #define leftMotor2Pin 8 #define rightMotor1Pin 12 #define rightMotor2Pin 11 #define enaPin 5 #define enbPin 6
// BỘ ĐIỀU KHIỂN PID
#define Kp 21 #define Kd 0.88 #define Ki 65 #define sampleTime 0.01 #define Limit_Speed 100 float offsetRight = 0; float offsetLeft = 0;
//xe cân bằng sẽ tiến hoặc lùi,góc nghiêng để xe cân bằng khi tiến hoặc lùi là 4 độ #define targetAngleUp 4
64 #define targetAngleBalance 0
float targetAngle = targetAngleBalance; #define angle_offset 0
volatile int motorPower, gyroRate;
volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, iSum=0;
volatile byte count=0; // Thiết lập ban đầu void cal_gyro_offset() {
sumGyroX = 0;sumGyroY = 0;sumGyroZ = 0; for(int i = 0 ; i < 1000 ; i++) { sumGyroX += mpu.getXGyroOffset(); sumGyroY += mpu.getYGyroOffset(); sumGyroZ += mpu.getZGyroOffset(); delay(1); //delay 1ms } gyroOffsetX = sumGyroX/1000; gyroOffsetY = sumGyroY/1000; gyroOffsetZ = sumGyroZ/1000; } void setup() {
// cấu hình đèn LED cho đầu ra pinMode(LED_PIN, OUTPUT); digitalWrite(LED_PIN, LOW);
// vào I2C bus (thư viện I2Cdev không tự động làm việc này)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin();
65
Wire.setClock(400000); //xung nhịp 400kHz I2C
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true);
#endif
// khởi tạo giao tiếp nối tiếp Serial.begin(115200); while (!Serial);
bluetooth.begin(9600);
// LƯU Ý: Bộ xử lý máy chủ 8MHz hoặc chậm hơn, như Teensy @ 3.3V hoặc Arduino
// Pro Mini chạy ở 3.3V, không thể xử lý tốc độ truyền này một cách đáng tin cậy do // thời gian baud bị lệch quá mức với các tick của bộ xử lý. Bạn phải dùng
// 38400 hoặc chậm hơn trong những trường hợp này hoặc sử dụng một số loại tách biệt bên ngoài
// giải pháp tinh thể cho bộ định thời UART. // initialize device
Serial.println(F("Initializing I2C devices...")); mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT); // xác minh kết nối
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
//tải và định cấu hình DMP Serial.println(F("Initializing DMP...")); devStatus = mpu.dmpInitialize(); delay(5); //delay 5s mpu.setXGyroOffset(0); mpu.setYGyroOffset(0); mpu.setZGyroOffset(0); cal_gyro_offset();
66
// cung cấp hiệu số con quay hồi chuyển của riêng bạn ở đây, được chia tỷ lệ để có độ nhạy tối thiểu
mpu.setXGyroOffset(gyroOffsetX); mpu.setYGyroOffset(gyroOffsetY); mpu.setZGyroOffset(gyroOffsetZ);
mpu.setZAccelOffset(1788); // 1688 mặc định ban đầu cho chip thử nghiệm // mpu.setYAccelOffset(716);
// đảm bảo nó hoạt động (trả về giá trị = 0 nếu vậy ) if (devStatus == 0) {
// bật DMP
Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true);
// cho phép phát hiện ngắt Arduino
Serial.print(F("Enabling interrupt detection (Arduino external interrupt ")); Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize(); } else {
Serial.print(F("DMP Initialization failed (code ")); Serial.print(devStatus);
Serial.println(F(")")); }
// đặt điều khiển động cơ và các chân PWM ở chế độ đầu ra pinMode(leftMotor1Pin, OUTPUT);
67 pinMode(rightMotor1Pin, OUTPUT); pinMode(rightMotor2Pin, OUTPUT); pinMode(enaPin, OUTPUT); pinMode(enbPin, OUTPUT); setMotors(0,0); init_PID(); } void loop() { //đọc bluetooth
//thiết lập góc mục tiêu,góc lệch trái,góc lệch phải if (bluetooth.available()) {
char ch = bluetooth.read(); Serial.println(ch);
if(ch == 'A') //tiến { Serial.println("UP"); targetAngle = targetAngleUp; offsetLeft = 0; offsetRight = 0; }
else if(ch == 'B') //lùi { Serial.println("Down"); targetAngle = targetAngleDown; offsetLeft = 0; offsetRight = 0; }
else if(ch == 'D') //trái {
68 targetAngle = targetAngleBalance;
offsetLeft = 40; offsetRight = -40; }
else if(ch == 'C') //phải { Serial.println("Right"); targetAngle = targetAngleBalance; offsetLeft = -40; offsetRight = 40; } else //stop { Serial.println("Stop"); targetAngle = targetAngleBalance; offsetLeft = 0; offsetRight = 0; } } if (!dmpReady) return;
// chờ ngắt MPU hoặc (các) gói bổ sung khả dụng while (!mpuInterrupt && fifoCount < packetSize) { if (mpuInterrupt && fifoCount < packetSize) { // try to get out of the infinite loop
fifoCount = mpu.getFIFOCount(); }
}
// đặt lại cờ ngắt và nhận INT_STATUS byte mpuInterrupt = false;
69 // nhận số FIFO hiện tại
fifoCount = mpu.getFIFOCount(); // kiểm tra overflow
if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
mpu.resetFIFO();
fifoCount = mpu.getFIFOCount(); Serial.println(F("FIFO overflow!"));
// hãy kiểm tra ngắt dữ liệu DMP sẵn sàng (điều này sẽ xảy ra thường xuyên) } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) { while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); //đợi độ dài dữ liệu có sẵn chính xác, sẽ là một thời gian chờ RẤT ngắn
mpu.getFIFOBytes(fifoBuffer, packetSize); // đọc một gói từ FIFO // theo dõi số lượng FIFO tại đây trong trường hợp có> 1 gói có sẵn fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_YAWPITCHROLL // hiển thị các góc Euler theo độ
mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); currentAngle = ypr[2] * 180/M_PI + angle_offset; //Serial.println(currentAngle); if(count_imu_ready > 500)//5s { digitalWrite(LED_PIN, HIGH); imu_ready = true; } count_imu_ready++; #endif }
70 }
// ISR sẽ được gọi sau mỗi 5 mili giây ISR(TIMER1_COMPA_vect)
{
if(imu_ready && currentAngle > -30 && currentAngle < 30){ // Đọc góc nghiêng
error = currentAngle - targetAngle; // iSum += Ki*(error)*sampleTime; iSum = constrain(iSum, -255, 255); //tính toán đầu ra từ các giá trị P, I và D
motorPower = Kp*(error) + iSum + Kd*(error-prevError)/sampleTime; motorPower = constrain(motorPower, -255, 255);
targetAngle = (motorPower > Limit_Speed) ? (targetAngle - 0.1) : targetAngle; targetAngle = (motorPower < -Limit_Speed) ? (targetAngle + 0.1) : targetAngle; setMotors(motorPower + offsetLeft, motorPower + offsetRight);
prevError = error; } else { setMotors(0, 0); } } void init_PID() { // khởi tạo timer 1 cli();
TCCR1A = 0; // đặt toàn bộ thanh ghi TCCR1A thành 0 TCCR1B = 0; // tương tự cho TCCR1B
// cài đặt thanh ghi so sánh để đặt thời gian lấy mẫu là 5ms OCR1A = 19999; // bật chế độ CTC
71
TCCR1B |= (1 << CS11); // kích hoạt ngắt so sánh hẹn giờ TIMSK1 |= (1 << OCIE1A);
sei(); // cho phép ngắt }
//điều khiển động cơ
void setMotors(int leftMotorSpeed, int rightMotorSpeed) { if(leftMotorSpeed >= 0) { digitalWrite(leftMotor2Pin, HIGH); digitalWrite(leftMotor1Pin, LOW); } else { digitalWrite(leftMotor2Pin, LOW); digitalWrite(leftMotor1Pin, HIGH); } if(rightMotorSpeed >= 0) { digitalWrite(rightMotor2Pin, LOW); digitalWrite(rightMotor1Pin, HIGH); } else { digitalWrite(rightMotor2Pin, HIGH); digitalWrite(rightMotor1Pin, LOW); } analogWrite(enaPin, abs(leftMotorSpeed)); analogWrite(enbPin, abs(rightMotorSpeed)); }
6