Dựa trên kết quả robot chạy thành công,chúng em có ý tưởng phát triển thêm để chế tạo thành 1 sản phẩm có thể chở được cả con người.Ý tưởng được đưa ra dựa vào một số yêu cầu thực tế như tạo ra một phương tiện giúp con người có thể di chuyển thuận tiện và nhỏ gọn có thể di chuyển trong nhà,phục vụ việc giải trí hoặc có thể dùng trong một số công việc như vận chuyển hàng hóa,đồ đạc.Bước đầu chúng em đã lắp đặt được phần khung của xe,xe có thể di chuyển được nhưng do thời gian hạn chế nên sản phẩm vẫn chưa hoàn thiện.Hiện tại chúng em vẫn đang tiến hành thực hiện phát triển thêm.
TÀI LIỆU THAM KHẢO
1) http://www.Google.com 2) http://www.Arduino360.com
3) Alwafi Husein, Attitude and altitude control of two wheel trirotor hybrid robot,Master‟s thesis, 2013.
4) Xe hai bánh tự cân bằng di chuyển trên địa hình phẳng của Mai Tuấn Đạt. 5) Hệ thống điều khiển phi tuyến, Huỳnh Thái Hoàng
6) Lý thuyết điều khiển tự động, Huỳnh Thái Hoàng 7) Hệ thống điều khiển thông minh, Huỳnh Thái Hoàng 8) http://www.Arduino.vn
9) Arduino.cc
PHỤ LỤC Phụ lục 1: Chƣơng trình chính
#include "I2Cdev.h" //Thu vien I2C #include "MPU6050.h" // Thu vien MPU #include "Kalman.h" // Thu vien Kalman
#include "pwm01.h" // Thu vien xuat xung PWM #include <PID_v1.h> // Thu vien PID cua arduino
#include <DueTimer.h> // Thu vien ngat timer cua Arduino
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h"
#endif
MPU6050 accelgyro; Kalman kalmanY;
int16_t accX, accY, accZ; int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ; uint32_t timer; uint16_t PWM_L,PWM_R,PWM_T; bool dir = 0; bool play = 0; double CounterEncoder= 0; double Value;
double accXangle, accYangle; // Goc cua cam bien gia toc double temp;
double gyroXangle, gyroYangle; // Goc cam bien con quay hoi chuyen double Setpoint, Input, Output;
double Kp,Ki,Kd; // Thong so thanh phan PID
double kalAngleX, kalAngleY; // Goc tinh toan su dung bo loc Kalman long frequency = 15000; // Tan so PWM cho dong co
char PIN_PWM_L = 8; // Chan xuat xung PWM o dong co trai char PIN_DIR_L = 23; // Chan chieu cua dong co trai
char PIN_PWM_R = 9; // Chan xuat xung PWM o dong co phai char PIN_DIR_R = 25; // Chan chieu cua dong co phai
char PIN_BUTTON = 27; // nut nhan khoi dong pid #define LED_PIN 13
double Angle; //Goc tren robot
double SetAngle = 88; // Goc khi robot o vi tri can bang
PID myPID(&Input, &Output, &Setpoint,0,0,0, DIRECT); // Cat dat thong so PID
#define OUTPUT_READABLE_ACCELGYRO void timerIsr() // Ham ngat timer 1 ms
{
Angle = kalAngleY;
if(Angle <(SetAngle - 20)) Angle= SetAngle -20; if(Angle >(SetAngle + 20)) Angle= SetAngle + 20; SetAngle=86-Value;
Input = SetAngle-Angle;
myPID.Compute(); // Tinh toan PID
(Output >= 0)? dir=0:dir=1; // Xac dinh chieu de robot can bang digitalWrite(PIN_DIR_L,dir);
digitalWrite(PIN_DIR_R,dir); PWM_T=4095-abs(Output); PWM_L= PWM_T;
PWM_R= PWM_T*0.98;
pwm_write_duty( PIN_PWM_R, PWM_R ); // Xuat xung PWM dieu khien dong co pwm_write_duty( PIN_PWM_L, PWM_L ); } void setup() {
// Cai dat I2C
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true);
#endif
// Cat dat UARRT voi toc do baud la 9600 Serial.begin(9600);
// Cat dat thong so dau cho PMU accelgyro.initialize(); accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG; kalmanY.setAngle(accYangle); gyroYangle = accYangle; timer = micros();
// Cac thong so PID Kp=90;
Ki=8500; Kd=0.3;
Setpoint = 0; // Thong so cat dat ban dau cua PID
myPID.SetOutputLimits(-2000,2000); // Gioi han dau ra cua PID myPID.SetTunings(Kp,Ki,Kd);
myPID.SetMode(AUTOMATIC); // PID chay o che do Auto // Cai dat thong so xuat xung PWM
pwm_set_resolution(12); // Xung PWM voi 12 bit pwm_setup( 6, frequency, 1); pwm_setup( 7, frequency, 2); pwm_setup( 8, frequency, 2); pwm_setup( 9, frequency, 2); pwm_write_duty( PIN_PWM_L, 4095); pwm_write_duty( PIN_PWM_R, 4095); // Cai dat che do OUTPUT/INPUT
pinMode(LED_PIN, OUTPUT); pinMode(PIN_DIR_L, OUTPUT); pinMode(PIN_DIR_R, OUTPUT);
attachInterrupt(39, Encoder, RISING); // Ngat ngoai canh xuong cho encoder pinMode(39, INPUT); pinMode(41, INPUT); pinMode(PIN_BUTTON, INPUT); } void loop() {
accelgyro.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ); // Doc du lieu I2C tu cam bien IMU
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; // Tinh toan doi tu radian sang do
double gyroYrate = -((double)gyroY/131.0);
gyroYangle += gyroYrate*((double)(micros()-timer)/1000000); // Tinh toan thong
so goc quay khi chua qua bo loc Kalman
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()- timer)/1000000); // Tinh toan goc quay khi qua bo loc Kalman
timer = micros();
temp = ((double)tempRaw + 12412.0) / 340.0;
if(play==0) // Kiem tra nut de robot thuc hien can bang {
if(digitalRead(PIN_BUTTON)) {
play=1;
Timer3.attachInterrupt(timerIsr); // Cat dat ngat timer 3 Timer3.start(1000); // Cai dat thoi gian ngat la 1 ms }
}
Value=(CounterEncoder/3300); Value = constrain(Value,-5,5); // Dua du lieu qua cong UART
Serial.print(CounterEncoder); Serial.print("\t"); Serial.print(Value); Serial.print("\t");
Serial.print(SetAngle); Serial.print("\t"); Serial.println(kalAngleY);
}
void Encoder() // Ham ngat tinh toan encoder {
if(((digitalRead(39)==0)&&(digitalRead(41)==1)) || ((digitalRead(39)==1) && (digitalRead(41)==0)))
CounterEncoder--;
else if(((digitalRead(39)==0)&&(digitalRead(41)==0)) || ((digitalRead(39)==1) && (digitalRead(41)==1)))
CounterEncoder++; }
Phụ lục 2: Chƣơng trình thƣ viện bộ lọc Kalman
/ Dinh nghia bo loc Kalman #ifndef _Kalman_h
#define _Kalman_h class Kalman { public:
Kalman() {
// Khai bao cac bien co dinh cho bo loc Kalman cac he so nay co the duoc thay doi
Q_angle = 0.001; Q_bias = 0.003; R_measure = 0.03; bias = 0; // Reset bias
// Thiet lap ma tran phuong sai loi P[0][0] = 0;
P[0][1] = 0; P[1][0] = 0; P[1][1] = 0; };
// dua cac thong so newAngle, newrate va delat time trong moi giay vao gia tri goc tinh toan
double getAngle(double newAngle, double newRate, double dt) { // cap nhat thoi gian (uoc luong)
// uoc luong gia tri moi rate = newRate - bias; angle += dt * rate;
// Uoc luong ma tran covariance tiep theo
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1]; P[1][1] += Q_bias * dt;
// Cap nhat gia tri do (hieu chinh) // Cap nhat gia tri do Zk
S = P[0][0] + R_measure; K[0] = P[0][0] / S;
K[1] = P[1][0] / S; // Tinh do loi Kalam y = newAngle - angle; angle += K[0] * y; bias += K[1] * y;
// Cap nhat lai ma tran covariance P[0][0] -= K[0] * P[0][0]; P[0][1] -= K[0] * P[0][1]; P[1][0] -= K[1] * P[0][0]; P[1][1] -= K[1] * P[0][1]; return angle; };
void setAngle(double newAngle) { angle = newAngle; }; / / su dung de thiet lap goc, co nghia la goc bat dau
double getRate() { return rate; }; // tro lai muc rate
/ * Khai bao cac chuong trinh con de dieu chinh bo loc Kalman * / void setQangle(double newQ_angle) { Q_angle = newQ_angle; }; void setQbias(double newQ_bias) { Q_bias = newQ_bias; };
void setRmeasure(double newR_measure) { R_measure = newR_measure; }; private:
/ * lọc biến Kalman * /
double Q_angle; // qua trinh nhieu phuong sai cho gia toc
double Q_bias; // qua trinh nhieu phuong sai cho con quay hoi chuyen double R_measure; // Do dac nhieu phuong sai
double angle; // cac goc tinh boi bo loc Kalman – mot phan ma tran 2 x 1 double bias; // con quay hoi chuyen tinh boi bo loc Kalman - bang 2 x 1matrix double rate; // cap nhat ti le getAngle
double P[2][2]; // Ma tran hiep phuong sai loi – day la ma tran 2 x 2 double K[2]; // do loi Kalman – day la ma tran 2 x 1
double y; // goc khac nhau ma tran 1 x1 double S; // uoc luong sai so loi ma tran 1 x 1 };
#endif
Phụ lục 3: Chƣơng trình thƣ viện PID
#ifndef PID_v1_h #define PID_v1_h #define LIBRARY_VERSION 1.0.0 class PID { public:
// Khai bao cac chuc nang su dung #define AUTOMATIC 1
#define MANUAL 0 #define DIRECT 0
#define REVERSE 1 // Cac chuc nang su dung
PID(double*, double*, double*, // * Lien ket PID ngo vao double, double, double, int); // Thiet lap cac thong so dieu chinh
void SetMode(int Mode); // dieu khien he PID bang tay (0) tu dong (non-0) bool Compute(); // Thuc hien tinh toan PID, thiet lap tan so va thoi gian lay mau
void SetOutputLimits(double, double); // Gia tri dau ra 0-255 doi voi 8-bit va tu 0 - 65536 doi voi 16-bit
void SetTunings(double, double, double); // Cai dat 3 he so Kp, Ki,Kd void SetControllerDirection(int); // * Dieu khien truc tiep
void SetSampleTime(int); // * Cai dat tan so theo mili giay // Hien thi chuc nang
// Dat cac he so PID double GetKp(); double GetKi(); double GetKd(); int GetMode(); int GetDirection(); private: void Initialize(); double dispKp; double dispKi; double dispKd; double kp; double ki; double kd; int controllerDirection;
double *myInput; // Lien ket cac bien dau vao, dau ra và diem dat double *myOutput; // Lien ket bien ngo ra
double *mySetpoint; // He so dat ban dau unsigned long lastTime;
double ITerm, lastInput; unsigned long SampleTime; double outMin, outMax; bool inAuto;
}; #endif
lastTime = now; }
void SetTunings(double Kp, double Ki, double Kd) {
kp = Kp; ki = Ki; kd = Kd; }
LỜI CẢM ƠN
Trong thời gian hiện đề tài nghiên cứu khoa học đó là một trong những bước ngoặt cuối cùng đánh dấu sự trưởng thành của một sinh viên ở giảng đường đại học.Để trở thành một cử nhân hay một kỹ sư đóng góp những gì mình đã học được cho sự phát triển đất nước.
Lời đầu tiên em xin chân thành cảm ơn sự hướng dẫn tận tình của thầy ThS. Nguyễn Ngọc Anh Tuấn, khoa Công nghệ Điện, trường Đại học Công Nghiệp Tp Hồ Chí Minh. Trong suốt thời gian thực hiện đề tài, thầy đã dành nhiều thời gian để hướng dẫn em thực hiện đề tài. Thầy đã hướng dẫn em tìm hiểu những kiến thức cần thiết để thực hiện đề tài, luôn nhiệt tình hướng dẫn, sẵn sàng giúp đỡ khi chúng em gặp khó khăn trong việc lập trình, chế tạo robot. Em cũng xin chân thành cảm ơn các thầy cô trong khoa Công nghệ Điện, cũng như các thầy cô trong trường đã giảng dạy, giúp đỡ em trong suốt bốn năm học qua. Chính các thầy cô đã xây dựng cho em những kiến thức nền tảng và những kiến thức chuyên môn để em có thể hoàn thành đề tài này.
TP Hồ Chí Minh, ngày 03 tháng 06 năm 2018