Arduino Balancing Robot

1#include <PID_v1.h>
2#include <LMotorController.h>
3#include "I2Cdev.h"
4#include  "MPU6050_6Axis_MotionApps20.h"
5
6#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
7  #include "Wire.h"
8#endif
9
10#define MIN_ABS_SPEED 20
11
12MPU6050 mpu;
13
14//  MPU control/status vars
15bool dmpReady = false; // set true if DMP init was successful
16uint8_t  mpuIntStatus; // holds actual interrupt status byte from MPU
17uint8_t devStatus;  // return status after each device operation (0 = success, !0 = error)
18uint16_t  packetSize; // expected DMP packet size (default is 42 bytes)
19uint16_t fifoCount;  // count of all bytes currently in FIFO
20uint8_t fifoBuffer[64]; // FIFO storage  buffer
21
22// orientation/motion vars
23Quaternion q; // [w, x, y, z] quaternion  container
24VectorFloat gravity; // [x, y, z] gravity vector
25float ypr[3]; //  [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
26
27//PID
28double  originalSetpoint =167;
29double setpoint = originalSetpoint;
30double movingAngleOffset  = 0.1;
31double input, output;
32
33//adjust these values to fit your own design
34double  Kp = 100;   
35double Kd = 8;
36double Ki = 60;
37PID pid(&input, &output, &setpoint,  Kp, Ki, Kd, DIRECT);
38
39double motorSpeedFactorLeft = 0.6;
40double motorSpeedFactorRight  = 0.6;
41//MOTOR CONTROLLER
42int ENA = 5;
43int IN1 = 6;
44int IN2 = 7;
45int  IN3 = 8;
46int IN4 = 9;
47int ENB = 10;
48LMotorController motorController(ENA,  IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);
49
50volatile  bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
51void  dmpDataReady()
52{
53 mpuInterrupt = true;
54}
55
56
57void setup()
58{
59  // join I2C bus (I2Cdev library doesn't do this automatically)
60 #if I2CDEV_IMPLEMENTATION  == I2CDEV_ARDUINO_WIRE
61 Wire.begin();
62 TWBR = 24; // 400kHz I2C clock (200kHz  if CPU is 8MHz)
63 #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
64 Fastwire::setup(400,  true);
65 #endif
66
67 mpu.initialize();
68
69 devStatus = mpu.dmpInitialize();
70
71  // supply your own gyro offsets here, scaled for min sensitivity
72 mpu.setXGyroOffset(220);
73  mpu.setYGyroOffset(76);
74 mpu.setZGyroOffset(-85);
75 mpu.setZAccelOffset(1788);  // 1688 factory default for my test chip
76
77 // make sure it worked (returns  0 if so)
78 if (devStatus == 0)
79 {
80 // turn on the DMP, now that it's ready
81  mpu.setDMPEnabled(true);
82
83 // enable Arduino interrupt detection
84 attachInterrupt(0,  dmpDataReady, RISING);
85 mpuIntStatus = mpu.getIntStatus();
86
87 // set our  DMP Ready flag so the main loop() function knows it's okay to use it
88 dmpReady  = true;
89
90 // get expected DMP packet size for later comparison
91 packetSize  = mpu.dmpGetFIFOPacketSize();
92 
93 //setup PID
94 pid.SetMode(AUTOMATIC);
95  pid.SetSampleTime(10);
96 pid.SetOutputLimits(-255, 255); 
97 }
98 else
99 {
100  // ERROR!
101 // 1 = initial memory load failed
102 // 2 = DMP configuration updates  failed
103 // (if it's going to break, usually the code will be 1)
104 Serial.print(F("DMP  Initialization failed (code "));
105 Serial.print(devStatus);
106 Serial.println(F(")"));
107  }
108}
109
110
111void loop()
112{
113 // if programming failed, don't try to do  anything
114 if (!dmpReady) return;
115
116 // wait for MPU interrupt or extra packet(s)  available
117 while (!mpuInterrupt && fifoCount < packetSize)
118 {
119 //no mpu  data - performing PID calculations and output to motors 
120 pid.Compute();
121  motorController.move(output, MIN_ABS_SPEED);
122 
123 }
124
125 // reset interrupt  flag and get INT_STATUS byte
126 mpuInterrupt = false;
127 mpuIntStatus = mpu.getIntStatus();
128
129  // get current FIFO count
130 fifoCount = mpu.getFIFOCount();
131
132 // check for  overflow (this should never happen unless our code is too inefficient)
133 if ((mpuIntStatus  & 0x10) || fifoCount == 1024)
134 {
135 // reset so we can continue cleanly
136  mpu.resetFIFO();
137 Serial.println(F("FIFO overflow!"));
138
139 // otherwise,  check for DMP data ready interrupt (this should happen frequently)
140 }
141 else  if (mpuIntStatus & 0x02)
142 {
143 // wait for correct available data length, should  be a VERY short wait
144 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
145
146  // read a packet from FIFO
147 mpu.getFIFOBytes(fifoBuffer, packetSize);
148 
149  // track FIFO count here in case there is > 1 packet available
150 // (this lets  us immediately read more without waiting for an interrupt)
151 fifoCount -= packetSize;
152
153  mpu.dmpGetQuaternion(&q, fifoBuffer);
154 mpu.dmpGetGravity(&gravity, &q);
155 mpu.dmpGetYawPitchRoll(ypr,  &q, &gravity);
156 input = ypr[1] * 180/M_PI + 180;
157 }
158}
159