//#define PRINT_DEBUG_BUILD //This is to print the mpu data on serial monitor to
debug
//PID library
#include <PID_v1.h>
//These are needed for MPU
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// class default I2C address is 0x68
MPU6050 mpu;
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
// 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] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and
gravity vector
VectorInt16 gy; // [x, y, z] gyro sensor measurements
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has
gone high
void dmpDataReady() {
mpuInterrupt = true;
}
#define PID_MIN_LIMIT -255 //Min limit for PID
#define PID_MAX_LIMIT 255 //Max limit for PID
#define PID_SAMPLE_TIME_IN_MILLI 10 //This is PID sample time in milliseconds
//The pitch angle given by MPU6050 when robot is vertical and MPU6050 is horizontal
is 0 in ideal case.
//However in real case its slightly off and we need add some correction to keep
robot vertical.
//This is the angle correction to keep our robot stand vertically. Sometimes robot
moves in one direction so we need to adjust this.
#define SETPOINT_PITCH_ANGLE_OFFSET -2.2
#define MIN_ABSOLUTE_SPEED 0 //Min motor speed
double setpointPitchAngle = SETPOINT_PITCH_ANGLE_OFFSET;
double pitchGyroAngle = 0;
double pitchPIDOutput = 0;
double setpointYawRate = 0;
double yawGyroRate = 0;
double yawPIDOutput = 0;
#define PID_PITCH_KP 10
#define PID_PITCH_KI 80
#define PID_PITCH_KD .8
#define PID_YAW_KP 0.5
#define PID_YAW_KI 0.5
#define PID_YAW_KD 0
PID pitchPID(&pitchGyroAngle, &pitchPIDOutput, &setpointPitchAngle, PID_PITCH_KP,
PID_PITCH_KI, PID_PITCH_KD, DIRECT);
PID yawPID(&yawGyroRate, &yawPIDOutput, &setpointYawRate, PID_YAW_KP, PID_YAW_KI,
PID_YAW_KD, DIRECT);
int enableMotor1=9;
int motor1Pin1=5;
int motor1Pin2=6;
int motor2Pin1=7;
int motor2Pin2=8;
int enableMotor2=10;
void setupPID()
{
[Link](PID_MIN_LIMIT, PID_MAX_LIMIT);
[Link](AUTOMATIC);
[Link](PID_SAMPLE_TIME_IN_MILLI);
[Link](PID_MIN_LIMIT, PID_MAX_LIMIT);
[Link](AUTOMATIC);
[Link](PID_SAMPLE_TIME_IN_MILLI);
void setupMotors()
{
pinMode(enableMotor1,OUTPUT);
pinMode(motor1Pin1,OUTPUT);
pinMode(motor1Pin2,OUTPUT);
pinMode(enableMotor2,OUTPUT);
pinMode(motor2Pin1,OUTPUT);
pinMode(motor2Pin2,OUTPUT);
rotateMotor(0,0);
}
void setupMPU()
{
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
[Link]();
[Link](400000); // 400kHz I2C clock. Comment this line if having
compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
[Link]();
pinMode(INTERRUPT_PIN, INPUT);
devStatus = [Link]();
// supply your own gyro offsets here, scaled for min sensitivity
// X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro
//OFFSETS -1798, 263, 1124, -261, 26, 32
[Link](-1798);
[Link](263);
[Link](1124);
[Link](-261);
[Link](26);
[Link](32);
// make sure it worked (returns 0 if so)
if (devStatus == 0)
{
// Calibration Time: generate offsets and calibrate our MPU6050
//[Link](6);
//[Link](6);
// turn on the DMP, now that it's ready
[Link](true);
mpuIntStatus = [Link]();
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = [Link]();
}
else
{
// ERROR!
}
}
void setup()
{
//This is to set up motors
setupMotors();
//This is to set up MPU6050 sensor
setupMPU();
//This is to set up PID
setupPID();
}
void loop()
{
// if programming failed, don't try to do anything
if (!dmpReady) return;
// read a packet from FIFO. Get the Latest packet
if ([Link](fifoBuffer))
{
[Link](&q, fifoBuffer);
[Link](&gravity, &q);
[Link](ypr, &q, &gravity);
[Link](&gy, fifoBuffer);
yawGyroRate = gy.z; //rotation rate in degrees per second
pitchGyroAngle = ypr[1] * 180/M_PI; //angle in degree
[Link](true);
[Link](true);
rotateMotor(pitchPIDOutput+yawPIDOutput, pitchPIDOutput-yawPIDOutput);
#ifdef PRINT_DEBUG_BUILD
[Link]("The gyro before ");
[Link](pitchGyroAngle);
[Link]("The setpoints ");
[Link](setpointPitchAngle);
[Link]("The pid output ");
[Link](pitchPIDOutput);
delay(500);
#endif
}
}
void rotateMotor(int speed1, int speed2)
{
if (speed1 < 0)
{
digitalWrite(motor1Pin1,LOW);
digitalWrite(motor1Pin2,HIGH);
}
else if (speed1 >= 0)
{
digitalWrite(motor1Pin1,HIGH);
digitalWrite(motor1Pin2,LOW);
}
if (speed2 < 0)
{
digitalWrite(motor2Pin1,LOW);
digitalWrite(motor2Pin2,HIGH);
}
else if (speed2 >= 0)
{
digitalWrite(motor2Pin1,HIGH);
digitalWrite(motor2Pin2,LOW);
}
speed1 = abs(speed1) + MIN_ABSOLUTE_SPEED;
speed2 = abs(speed2) + MIN_ABSOLUTE_SPEED;
speed1 = constrain(speed1, MIN_ABSOLUTE_SPEED, 255);
speed2 = constrain(speed2, MIN_ABSOLUTE_SPEED, 255);
analogWrite(enableMotor1,speed1);
analogWrite(enableMotor2,speed2);
}