Nhận xét:
- Xe chỉ cân bằng tại chỗ, chưa thể di chuyển được. - Bộ điều khiển có biên độ dao động 174 đến 188. - Sai số 0,97 -1,04% so với giá trị đặt.
3.2. Kết quả chưa đạt được
Mơ hình cịn bị trơi nhẹ do xe chưa cân đối.
Kết quả các bộ điều khiển đều chưa ổn định khi di chuyển.
Do bộ điều khiển chưa thể tự điều chỉnh nên chưa di chuyển trên các môi trường dốc hoặc các môi trường khác được.
3.3. Hướng phát triển
Thiết kế bộ điều khiển PID tự chỉnh như LQR, LQG, Fuzzy,…. Sử dụng camera để quan sát hướng di chuyển và chở vật. Di chuyển được trên nhiều địa hình.
PHỤ LỤC PHỤ LỤC #include "I2Cdev.h" #include <PID_v1.h> #include "MPU6050_6Axis_MotionApps20.h" MPU6050 mpu; #include <SoftwareSerial.h> //kiểm sốt MPU tình trạng có
bool dmpReady = false; // đặt true nếu init DMP thành công uint8_t mpuIntStatus; // giữ byte trạng thái ngắt thực tế từ MPU
uint8_t devStatus; // trạng thái trả về sau mỗi thao tác thiết bị (0 = thành công,! 0 = lỗi)
uint16_t packetSize; // kích thước gói DMP dự kiến (mặc định là 42 byte) uint16_t fifoCount; // count của tất cả các byte hiện tại trong FIFO
uint8_t fifoBuffer[64]; // Bộ đệm lưu trữ FIFO // định hướng / chuyển động
Quaternion q; // [w, x, y, z] vùng chứa thứ ba VectorFloat gravity; // [x, y, z] vector trọng lực
float ypr[3]; // [yaw, pitch, roll] yaw / pitch / roll container và vector trọng lực
//Điều chỉnh 4 giá trị này cho robot
double setpoint = 180.0; //đặt giá trị khi robot vng góc với mặt đất bằng màn hình nối tiếp.
double Kp = 30; //Đặt mục này trước double Kd = 1.8; //Đặt chế độ này
double Ki = 140; //Cuối cùng thiết lập điều này //Kết thúc các giá trị thiết lập
double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
volatile bool mpuInterrupt = false; // cho biết liệu pin ngắt MPU có cao khơng void dmpDataReady()
{
mpuInterrupt = true; }
void setup() {
Serial1.begin(9600);
Serial.begin(115200);// khởi tạo thiết bị Serial.println(F("Khởi tạo thiết bị I2C ...")); mpu.initialize();// xác minh kết nối
Serial.println(F("Kiểm tra kết nối thiết bị ..."));
Serial.println (mpu.testConnection ()? F ("Kết nối thành công MPU6050"): F ("Kết nối MPU6050 không thành công"));
// tải và cấu hình DMP
devStatus = mpu.dmpInitialize();
// cung cấp bù lệch con quay hồi chuyển chỉnh tỷ lệ cho độ nhạy tối thiểu mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76); mpu.setZGyroOffset(-85); mpu.setZAccelOffset(1788);
// đảm bảo nó hoạt động (trả về 0 nếu có) if (devStatus == 0) { // bật DMP, bây giờ nó đã sẵn sàng Serial.println(F ("Kích hoạt DMP ...")); mpu.setDMPEnabled(true); // bật phát hiện ngắt Arduino
//Serial.println(F ("Bật phát hiện ngắt (ngắt ngoài Arduino 0) ...")); attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
PHỤ LỤC
dmpReady = true;
// nhận kích thước gói DMP dự kiến để so sánh sau packetSize = mpu.dmpGetFIFOPacketSize(); // cài đặt PID pid.SetMode(AUTOMATIC); pid.SetSampleTime(5); pid.SetOutputLimits(-255, 255); } else { // LỖI!
// 1 = tải bộ nhớ ban đầu không thành công // 2 = Cập nhật cấu hình DMP khơng thành cơng // (nếu nó sẽ bị ngắt, thơng thường mã sẽ là 1)
// Serial.print (F ("Khởi tạo DMP không thành công mã")); // Serial.print(devStatus);
// Serial.println(F(")")); }
// Khởi tạo các chân output động cơ pinMode (8, OUTPUT);
pinMode (9, OUTPUT); pinMode (10, OUTPUT); pinMode (11, OUTPUT);
// Theo mặc định, tắt cả hai động cơ analogWrite(8,LOW); analogWrite(9,LOW); analogWrite(10,LOW); analogWrite(11,LOW); } char cha;
int spd = 255; void loop() {
// nếu lập trình khơng thành cơng, đừng cố làm gì cả while (!dmpReady) return;
{
// chờ đợi cho MPU ngắt hoặc gói thêm (s) có sẵn while (!mpuInterrupt && fifoCount < packetSize) {
// khơng có dữ liệu mpu - thực hiện các phép tính PID và đầu ra cho các động cơ
pid.Compute();
// In giá trị của đầu vào và đầu ra trên màn hình nối tiếp để kiểm tra xem nó hoạt động như thế nào.
Serial.println(input);// Serial.print(" =>"); Serial.println(output); if (input>150 && input<220)
{
// Nếu robot đang nghiêng
if (output>0) //Nghiêng về phía trước Forward(); //Xoay bánh xe về phía trước else if (output<0) //Nghiêng về phía sau Reverse(); //Xoay bánh xe về phía sau }
else //Nếu robot không nghiêng Stop();//Giữ bánh xe } if (Serial1.available() > 0) { cha=Serial1.read(); }
PHỤ LỤC { analogWrite(8,spd -55); analogWrite(9,0); analogWrite(10,spd -55); analogWrite(11,0); setpoint= 175.0; } if(cha=='D') { analogWrite(8,0); analogWrite(9,spd - 55); analogWrite(10,0); analogWrite(11,spd -55); setpoint= 183.0; } if(cha=='L') { setpoint= 180.0; analogWrite(8,spd ); analogWrite(9,0); analogWrite(10,0); analogWrite(11,spd); } if(cha=='R') { setpoint= 180.0; analogWrite(8,0); analogWrite(9,spd); analogWrite(10,spd); analogWrite(11,0);
} if(cha=='S') { analogWrite(8,0); analogWrite(9,0); analogWrite(10,0); analogWrite(11,0); setpoint= 180.0; }
// đặt lại cờ ngắt và nhận byte INT_STATUS mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus(); // nhận số FIFO hiện tại
fifoCount = mpu.getFIFOCount(); // kiểm tra tràn
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// đặt lại để chúng tơi có thể tiếp tục sạch mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// nếu không, hãy kiểm tra dữ liệu DMP sẵn sàng gián đoạn (điều này sẽ xảy ra thường xuyên)
}
else if (mpuIntStatus & 0x02) {
// chờ cho độ dài dữ liệu có sẵn chính xác, phải chờ đợi rất ngắn while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // đọc một gói từ FIFO
PHỤ LỤC
// (điều này cho phép chúng tôi ngay lập tức đọc thêm mà không phải chờ gián đoạn)
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer); // lấy giá trị cho q mpu.dmpGetGravity(&gravity, &q); // lấy giá trị cho lực hấp dẫn mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); // nhận giá trị cho ypr input = ypr[1] * 180/M_PI + 180;
} }}
void Forward() //Code xoay bánh xe về phía trước {
analogWrite(8,output); analogWrite(9,0); analogWrite(10,output); analogWrite(11,0);
Serial.print("F"); // Thông tin gỡ lỗi }
void Reverse() //Code xoay bánh xe lùi { analogWrite(8,0); analogWrite(9,output*-1); analogWrite(10,0); analogWrite(11,output*-1); Serial.print("R"); }
void Stop() //Code dừng cả hai bánh xe {
analogWrite(8,0); analogWrite(9,0); analogWrite(10,0);
analogWrite(11,0); Serial.print("S"); }
TÀI LIỆU THAM KHẢO
TÀI LIỆU THAM KHẢO
[1] Phạm Quang Huy và Lê Cảnh Trung. Lập trình điều khiển với arduino.
Nhà xuất bản khoa học và kĩ thuật, 2014.
[2] Nguyễn Thị Phương Hà và 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 TP.HCM, 2016.
[3] 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.
[4] Nguyễn Thị Phương Hà. Lý thuyết điều khiển nâng cao. Nhà xuất bản Đại
học Quốc gia TP. Hồ Chí Minh, 2011.
[5] Đường Khánh Sơn, Giáo trình Vi điều khiển.
[6] Trần Sĩ Lâm, Tiểu luận Nghiên cứu chế tạo robot di động đa hướng. [7] Cam Thái Tài, Nghiên cứu và thiết kế xe hai bánh tự cân bằng.
[8] Nguyễn Minh Tâm, Nguyễn Văn Đông Hải, Nguyễn Phong Lưu, Lê Văn
Tuấn. Mơ hình hóa và điều khiển tối ưu cho hệ xe hai bánh tự cân bằng. Trường đại
học Sư phạm Kỹ thuật TP.HCM. [9]http://create.arduino.cc/projecthub/twob/self-balancing-robot-using-blubug- 8894c6 (Ngày 30/3/2020). [10]https://www.instructables.com/id/2-Wheel-Self-Balancing-Robot-by- using-Arduino-and-/ (Ngày 29/12/2020). [11]https://circuitdigest.com/microcontroller-projects/arduino-based-self- balancingrobot?fbclid=IwAR3ENpX5ZYhLZ4jN48cYPjT3czze4xTWXTjshNheW 0EVoc7IzHteZ72gZwA (Ngày 23/2/2020).