#include <Wire.h> // Cung cấp thư viện I2C
#define ToDeg 180/PI // định nghĩa hằng chuyển đổi giá trị Radian sang Độ
Kalman kalman ; // Khởi tạo một hằng Kalman dùng để gọi các lệnh trong thư viện Kalman
uint32_t timerloop, timerold; // Tạo ra thời gian lặp để lấy mẫu // Chân điều khiển cho Motor //
#define leftpwm 9 // Điều khiển PWM cho động cơ 1 #define leftdir_t 8 // Điều khiển chiều
#define leftdir_n 11
#define rightpwm 10 // Điều khiển PWM cho động cơ 2 #define rightdir_t 12 // Điều khiển chiều
#define rightdir_n 7
volatile long leftencoder; // Đọc và lưu giá trị của encoder trái ( volatile là kiểu khai báo dành cho giá trị của biến thay đổi một cách không xác định.)
volatile long rightencoder; // Đọc và lưu giá trị của encoder phải #define leftencoder_a 2 // đọc trạng thái kênh A, LOW or HIGH #define leftencoder_b 6 // đọc trạng thái kênh B, LOW or HIGH #define rightencoder_a 3
#define rightencoder_b 5
// dữ liệu MPU6050 //
float mpudata; // Biến xác định góc hiện thời
float AcY, AcZ; // giá trị Gyroscope theo trục Y và Z float Gyro; // giá trị vận tốc góc quay tính theo trục X
dữ liệu từ MPU, thời gian cho bộ lọc Kalmal tính góc psi uint8_t i2cData[14]; // Lưu giá trị đọc từ cảm biến MPU // Dữ liệu LQR //
long PWML, PWMR; // Giá trị xung PWM cho cầu H float k1, k2, k3, k4, k5, k6; // các thông số ma trận K
bool falldown; // Chạy = true; Dừng = false, định nghĩa để điều khiển ĐC float theta, psi, phi; // Góc
float thetadot, psidot, phidot; // Giá trị vận tốc góc tương ứng float thetaold, psiold, phiold; // Giá trị trước đó của các góc
float leftvolt; // Điện áp ra của Motor trái trong LQR float rightvolt; // Điện áp ra của Motor phải trong LQR
float addtheta; // lưu lại giá trị đặt để động cơ di chuyển và bám theo nó float addphi; // lưu lại giá trị đặt để động cơ di chuyển và bám theo nó int ForwardBack; // 1 -> Tới; -1 -> Lùi; 0 -> Ngừng và cân bằng ////////////////////////////////////////////////////
////////////////// SERIAL BEGIN ////////////////////
////////////////////////////////////////////////////
void setup()
{ // Set up các ngỏ vào ra cho hệ xe //
pinMode(leftpwm, OUTPUT);
pinMode(leftdir_t, OUTPUT);
pinMode(rightdir_t, OUTPUT);
pinMode(rightdir_n, OUTPUT);
// Ngỏ vào của encoder //
pinMode(leftencoder_a, INPUT_PULLUP); // mặc định nhận giá trị HIGH pinMode(rightencoder_a, INPUT_PULLUP);
pinMode(leftencoder_b, INPUT_PULLUP);
pinMode(rightencoder_b, INPUT_PULLUP);
Serial.begin(115200); // Khởi tạo chuẩn giao tiếp bandte giữa Arduino và máy tính // Dữ liệu MPU6050 //
Wire.begin(); // Khởi tạo thư viện cho MPU hoạt động
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00;// Disale FSYNC and set 260Hz Accelerator filtering, 256 Hz Gyro filtering, 8Khz sampling
i2cData[2] = 0x00;// Set Gyro full scale range to +/-250deg/s i2cData[3] = 0x00;// Set Gyro full scale range to +/-2g
while (i2cWrite(0x19, i2cData, 4, false));// Write to all four registers at the same time
while (i2cWrite(0x6B, 0x01, true));// PLL with X Axis Gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) {
Serial.print(F("Error reading sensor"));
}
delay(100); // Chờ cảm biến ổn định while (i2cRead(0x3B, i2cData, 6));
AcY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
AcZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
double pitch = (-AcY, AcZ)*RAD_TO_DEG;
kalman.setAngle(pitch);
timer = micros();
// Các giá trị K cần lựa chọn //
k1 = 0.08 ; // k1*theta 0.1 k2 = -0.38; // k2*thetadot 0.38
k3 = -705; // k3*psi 129 705 178 214 k4 = 45; // k4*psidot 10
k5 = 0.038; // k5*phi 0.038 k6 = 0.32; // k6*phidot 0.32
// Dữ liệu khởi tạo động cơ//
ForwardBack = 0;
LeftRight = 0;
// Cài điểm bắt đầu //
addphi = 0;
addtheta = 0;
//Ngắt encoder //
Pin 2 ~ motor trái, ngắt xuống.
attachInterrupt(digitalPinToInterrupt(rightencoder_a), right_isr, FALLING);
// Pin 3 ~ motor phải.
}
///////////////////////////////////////////////////
////////////////// MAIN PROGRAM ///////////////////
///////////////////////////////////////////////////
void loop() {
if((micros() - timerloop) > 6000) // thời gian lấy mẫu 6s {
readMPU6050(); // đọc giá trị từ cảm biến
theta = gettheta(leftencoder,rightencoder)*ToRad;// tính góc theta
psi = (mpudata+0.54 )*ToRad; // tính góc nghiêng phi, 0.54 là sai số góc nghiêng tùy thuộc vào mỗi con cảm biến
phi = getphi(leftencoder,rightencoder)*ToRad; // tính góc xoay phi // Cập nhật và so sánh thời gian lấy mẫu//
float dt = (float)(micros() - timerloop)*1000000.0; // tiếp tục cập nhật thời gian lấy mẫu
timerloop = micros(); // Gán lại thời gian đầu cho vòng lặp // Cập nhật lại giá trị vận tốc góc vào//
thetadot = (theta - thetaold)/dt;
psidot = (psi- psiold)/dt;
phidot = (phi - phiold)/dt;
thetaold = theta;
psiold = psi;
phiold = phi;
getlqr(theta + addtheta, thetadot, psi, psidot, phi + addphi, phidot); // Tính toán đưa ra xung PWM cấp vào hai động cơ
motorcontrol(PWML, PWMR, (mpudata+0.54), falldown);// điều khiển motor // Gửi dữ liệu góc nghiêng lên//
Serial.print(psi*ToDeg); Serial.println("\t\t");
} }
//hàm con đọc encoder motor trái //
void left_isr() {
if(digitalRead(leftencoder_b) == 1) leftencoder++;
else
leftencoder--;
}
//hàm con đọc encoder motor phải //
void right_isr() {
if(digitalRead(rightencoder_b) == 1) rightencoder--;
}
// Đọc giá trị góc Psi //
void readMPU6050() {
while (i2cRead(0x3B, i2cData, 14));
AcY = (int16_t)((i2cData[2] << 8) | i2cData[3]); // gia tốc theo trục Y AcZ = (int16_t)((i2cData[4] << 8) | i2cData[5]); // gia tốc theo trục Z
Gyro = (int16_t)((i2cData[8] << 8) | i2cData[9]); // Vận tốc góc quay theo trục X double dt = (double)(micros() - timer)/1000000.0; // thời gian
timer = micros(); // trả về thời gian đầu lấy mẫu
double pitch = atan2(-AcY, AcZ)*RAD_TO_DEG; // tính giá trị góc theo trục Y và Z đổi ra độ
double Gyrorate = Gyro/131.0; // chia cho độ nhạy LBS của cảm biến
mpudata = kalman.getAngle(pitch, Gyrorate, dt); // Sữ dụng bộ lọc Kalman kết hợp giữa gia tốc và vận tốc góc tính ra góc hiện thời của cảm biến
if(abs(mpudata) > 30 ) // giới hạn góc hoạt động cho hệ xe falldown = true; // Ngừng Motor
else
falldown = false; // Chạy }
// Đọc giá trị Theta//
float gettheta(long lencoder, long rencoder) {
(xung/200)*360 xung ở đây là lencoder và rencoder) return angle;
}
// đọc giá trị góc phi//
float getphi(long lencoder, long rencoder) {
float angle = 0.25*(rencoder - lencoder);// 0.25 được tính toán theo công thức return angle;
}
// Hàm LQR//
void getlqr(float theta_, float thetadot_, float psi_, float psidot_, float phi_, float phidot_)
{
leftvolt = k1*theta_ + k2*thetadot_ + k3*psi_ + k4*psidot_ - k5*phi_ - k6*phidot_;
rightvolt = k1*theta_ + k2*thetadot_ + k3*psi_ + k4*psidot_ + k5*phi_ + k6*phidot_;
PWML = map(leftvolt, -(k3*PI)/14, (k3*PI)/14, -240, 240); // Giới hạn góc PWMR = map(rightvolt, -(k3*PI)/14, (k3*PI)/14, -240, 240);
PWML = constrain(PWML, -240, 240); // giới hạn xung cho cầu H PWMR = constrain(PWMR, -240, 240 );
}
// hàm điều khiển động cơ//
void motorcontrol(long PWML, long PWMR, float angle, bool stopstate)
if(stopstate == 1) stopandreset();
else {
if( abs(angle) < 0.2) stopandreset(); // góc psi trong khoảng -0.2 đến 0.2 thì stop động cơ
else {
if(leftvolt > 0) {
leftmotor(abs(PWML), 1); // Forward }
else if(leftvolt <0) {
leftmotor(abs(PWML), 0); // Back }
else {
stopandreset();
}
if(rightvolt >0) {
rightmotor(abs(PWMR), 1); // Forward }
{
rightmotor(abs(PWMR), 0); // Back }
else {
stopandreset();
} } } }
// Dừng và reset dữ liệu//
// dữ liệu góc và encoder sẽ reset về không//
void stopandreset() {
analogWrite(leftpwm, 0);
analogWrite(rightpwm, 0);
// Reset default place //
leftencoder = 0;
rightencoder = 0;
// Reset zero set point //
addtheta = 0;
addphi = 0;
5.1. MÔ HÌNH THỰC TẾ
Robot có thể giữ cân bằng khi có lực tác động, hình dạng của Robot được mô tả dưới hình 5.1:
Hình 5.4: Mô hình thực tế.
5.2. MÔ PHỎNG MATLAB
Hình 5.2 mô tả sữ dụng Simulink mô phỏng trạng thái hoạt động của Robot
Kết quả thu được
Với Đường Đỏ hiển thị cho góc Theta, đường xanh dương hiển thị cho góc psi, đường xanh lá cây hiển thị cho góc phi.