Đồ án robot cân bằng 2 bánh

Nó giống như việc giữ thăng bằng một cây gậy trên ngón tay. Để giữ thăng bằng, chúng ta phải di chuyển ngón tay của mình theo hướng nghiêng và tốc độ nghiêng của cây gậy. Để điều khiển động cơ bánh xe cho robot tự cân bằng qua mạch cầu L298N, chúng ta cần một số thông tin về trạng thái của robot như: điểm thăng bằng cần cài đặt cho robot, hướng mà robot đang nghiêng, góc nghiêng và tốc độ nghiêng. Tất cả các thông tin này được thu thập từ MPU6050 và đưa vào bộ điều khiển PID để tạo ra một tín hiệu điều khiển động cơ, giữ cho robot ở điểm thăng bằng.

//welcome to balancerobot

//group2

#include <PID_v1.h>

#include <LMotorController.h>

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

#include "Wire.h"

#endif

#define MIN_ABS_SPEED 20

MPU6050 mpu;

// MPU control/status vars

bool dmpReady = false; // set true if DMP init was successful

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU

uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount; // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q; // [w, x, y, z] quaternion container

VectorFloat gravity; // [x, y, z] vecto trong luc

float ypr[3]; // [yaw, pitch, roll] cd,quay va vecto tròng luc

//PID

double originalSetpoint = 173;

double setpoint = originalSetpoint;

double movingAngleOffset = 0.1;

double input, output;

//adjust these values to fit your own design

double Kp =45;//45xd tac dong cua sai so hien tai (phu thuoc sai so hien tai)

//td lm robot chuyen dong qua lai va di xuong

double Kd = 1.5;//1.5xd tac dong cua toc do bien doi sai so(du doan sai so tuong lai dua vao toc do thay doi hien tai)

//td lam giam giao dong

double Ki = 60;//60xd tac dong cua tong cac sai so qua khu (phu thuoc tich luy sai so qua khu) //td rut ngan tg de robot on dinh

PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 0.6;//he so toc do banh trai

double motorSpeedFactorRight = 0.55;

//MOTOR CONTROLLER

int ENA = 5;

int IN1 = 6;

int IN2 = 7;

int IN3 = 8;

int IN4 = 9;

int ENB = 10;

LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high void dmpDataReady()

{

mpuInterrupt = true;

}

void setup()

{

// join I2C bus (I2Cdev library doesn't do this automatically)

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

Wire.begin();

TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)

#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

Fastwire::setup(400, true);

#endif

mpu.initialize();

devStatus = mpu.dmpInitialize();//khoi tao mpu

// cac gia tri ban dau

mpu.setXGyroOffset(144);

mpu.setYGyroOffset(-9);

mpu.setZGyroOffset(61);

mpu.setZAccelOffset(1166); //

// dam bao he thong da lam viec ( neu co tra ve 0)

if (devStatus == 0)

{

// bat DMP(thu thap, phan tich, gui gia tri) 6050

mpu.setDMPEnabled(true);

// cho phep arduino ngat

attachInterrupt(0, dmpDataReady, RISING);

// ngat 0 - chan digital 2

// dmpDataReady ham duoc goi khi co ngat

// kich hoat khi trang thai chan digital 2 chuyen tu muc dien ap thap sang muc dien ap cao mpuIntStatus = mpu.getIntStatus();

// dat co cho dmp san sang khi ct chinh hoat dong

dmpReady = true;

// nhan duoc kich thuoc goi dmp du kien de so sanh

packetSize = mpu.dmpGetFIFOPacketSize();

//khoi tao PID

pid.SetMode(AUTOMATIC);//tu dong

pid.SetSampleTime(10);//thoi gian lay mau 10ms

pid.SetOutputLimits(-255, 255); //gioi han toc do

}

else

{

// ERROR!

// 1 = tai bo nho ban dau khong thanh cong // 2 = cap nhat cau hinh dmp that bai

// (neu dang ngat se tra ve 1)

Serial.print(F("DMP Initialization failed (code ")); Serial.print(devStatus);

Serial.println(F(")"));

}

}

void loop()

{

// neu truong trinh that bai khong chay tiep

if (!dmpReady) return;

// cho ngat hoac them vao goi co san

while (!mpuInterrupt && fifoCount < packetSize) {

// tinh toan pid va toc do dong co

pid.Compute();

motorController.move(output, MIN_ABS_SPEED);

}

// Lam moi co va lay bien trang thai

mpuInterrupt = false;

mpuIntStatus = mpu.getIntStatus();

// lay so FIFO hien tai

fifoCount = mpu.getFIFOCount();

// kiem tra co tran

if ((mpuIntStatus & 0x10) || fifoCount == 1024)

{

// reset

mpu.resetFIFO();

Serial.println(F("FIFO overflow!"));

// kiem tra ma dmp co bi gian doan

}

else if (mpuIntStatus & 0x02)

{

// cho doi mot do dai du lieu chinh xac

while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// doc gia tri mot goi tu FIFO

mpu.getFIFOBytes(fifoBuffer, packetSize);

// theo doi FIFO dem

// cho phep lap tuc doc ma k can cho gian doan

fifoCount -= packetSize;

//tinh toan input

mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); input = ypr[1] * 180/M_PI + 180;

}

}