New implementation based on MPU6050 library

Franko/Franko.ino

@@ -1,103 +1,220 @@
-#include <Wire.h>
-#include <Kalman.h>
-#include <GY521.h>
 #include <PID_v1.h>
 #include <LMotorController.h>
+#include "I2Cdev.h"
 
+#include "MPU6050_6Axis_MotionApps20.h"
 
-//MOTOR CONTROLLER
-
+#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+    #include "Wire.h"
+#endif
 
-int ENA = 3;
-int IN1 = 4;
-int IN2 = 2;
-int IN3 = 5;
-int IN4 = 7;
-int ENB = 6;
 
-LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 1, 1);
+#define LOG_INPUT 0
 
 
 //MPU
 
 
-GY521 mpu;
+MPU6050 mpu;
+
+bool blinkState = false;
+
+// 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
+VectorInt16 aa;         // [x, y, z]            accel sensor measurements
+VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
+VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
+VectorFloat gravity;    // [x, y, z]            gravity vector
+float euler[3];         // [psi, theta, phi]    Euler angle container
+float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
 
 
 //PID
 
 
 double kp , ki, kd;
 double prevKp, prevKi, prevKd;
-double setpoint = 173.5, input, output;
+double setpoint = 174.14, input, output;
 
 PID pid(&input, &output, &setpoint, 0, 0, 0, DIRECT);
 
 
-//timers
+//MOTOR CONTROLLER
 
 
-long time1Hz = 0;
+int ENA = 3;
+int IN1 = 4;
+int IN2 = 8;
+int IN3 = 5;
+int IN4 = 7;
+int ENB = 6;
 
 
-//setup
+LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 1, 1);
 
 
-void setup()
-{    
-    Serial.begin(115200);
-    setupMPU();
-    setupPID();
-}
+//timer
 
 
-void setupMPU()
+long time1Hz = 0;
+
+
+volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
+void dmpDataReady()
 {
-    mpu.setDebugMode(false);
-    mpu.setup();
+    mpuInterrupt = true;
 }
 
 
-void setupPID()
+void setup()
 {
-    pid.SetMode(AUTOMATIC);
-    pid.SetSampleTime(1);
-    pid.SetOutputLimits(-255, 255);  
-}
+    // 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
+
+    // initialize serial communication
+    // (115200 chosen because it is required for Teapot Demo output, but it's
+    // really up to you depending on your project)
+    Serial.begin(115200);
+    while (!Serial); // wait for Leonardo enumeration, others continue immediately
 
+    // initialize device
+    Serial.println(F("Initializing I2C devices..."));
+    mpu.initialize();
 
-//loop
+    // verify connection
+    Serial.println(F("Testing device connections..."));
+    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
 
+    // load and configure the DMP
+    Serial.println(F("Initializing DMP..."));
+    devStatus = mpu.dmpInitialize();
 
-void loop()
-{
-    readIMUValues();
-    pid.Compute();
-    motorController.move(output);
+    // supply your own gyro offsets here, scaled for min sensitivity
+    mpu.setXGyroOffset(220);
+    mpu.setYGyroOffset(76);
+    mpu.setZGyroOffset(-85);
+    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
 
-    unsigned long currentMillis = millis();
-
-    if (currentMillis - time1Hz >= 1000)
+    // make sure it worked (returns 0 if so)
+    if (devStatus == 0)
     {
-        loopAt1Hz();
-        time1Hz = currentMillis;
+        // turn on the DMP, now that it's ready
+        Serial.println(F("Enabling DMP..."));
+        mpu.setDMPEnabled(true);
+
+        // enable Arduino interrupt detection
+        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
+        attachInterrupt(0, dmpDataReady, RISING);
+        mpuIntStatus = mpu.getIntStatus();
+
+        // set our DMP Ready flag so the main loop() function knows it's okay to use it
+        Serial.println(F("DMP ready! Waiting for first interrupt..."));
+        dmpReady = true;
+
+        // get expected DMP packet size for later comparison
+        packetSize = mpu.dmpGetFIFOPacketSize();
+
+        //setup PID
+
+        pid.SetMode(AUTOMATIC);
+        pid.SetSampleTime(10);
+        pid.SetOutputLimits(-255, 255);  
+    }
+    else
+    {
+        // ERROR!
+        // 1 = initial memory load failed
+        // 2 = DMP configuration updates failed
+        // (if it's going to break, usually the code will be 1)
+        Serial.print(F("DMP Initialization failed (code "));
+        Serial.print(devStatus);
+        Serial.println(F(")"));
     }
 }
 
 
-void loopAt1Hz()
+void loop()
 {
-    setPIDTuningValues();
-}
+    // if programming failed, don't try to do anything
+    if (!dmpReady) return;
 
+    // wait for MPU interrupt or extra packet(s) available
+    while (!mpuInterrupt && fifoCount < packetSize)
+    {
+        //no mpu data - performing PID calculations and output to motors
+
+        pid.Compute();
+        motorController.move(output);
+
+        unsigned long currentMillis = millis();
+
+        if (currentMillis - time1Hz >= 1000)
+        {
+            loopAt1Hz();
+            time1Hz = currentMillis;
+        }
+    }
+
+    // reset interrupt flag and get INT_STATUS byte
+    mpuInterrupt = false;
+    mpuIntStatus = mpu.getIntStatus();
+
+    // get current FIFO count
+    fifoCount = mpu.getFIFOCount();
+
+    // check for overflow (this should never happen unless our code is too inefficient)
+    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
+    {
+        // reset so we can continue cleanly
+        mpu.resetFIFO();
+        Serial.println(F("FIFO overflow!"));
 
-//Read IMU data
+    // otherwise, check for DMP data ready interrupt (this should happen frequently)
+    }
+    else if (mpuIntStatus & 0x02)
+    {
+        // wait for correct available data length, should be a VERY short wait
+        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
 
+        // read a packet from FIFO
+        mpu.getFIFOBytes(fifoBuffer, packetSize);
+
+        // track FIFO count here in case there is > 1 packet available
+        // (this lets us immediately read more without waiting for an interrupt)
+        fifoCount -= packetSize;
+
+        mpu.dmpGetQuaternion(&q, fifoBuffer);
+        mpu.dmpGetGravity(&gravity, &q);
+        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        #if LOG_INPUT
+            Serial.print("ypr\t");
+            Serial.print(ypr[0] * 180/M_PI);
+            Serial.print("\t");
+            Serial.print(ypr[1] * 180/M_PI);
+            Serial.print("\t");
+            Serial.println(ypr[2] * 180/M_PI);
+        #endif
+        input = ypr[1] * 180/M_PI + 180;
+   }
+}
 
-void readIMUValues()
+
+void loopAt1Hz()
 {
-    mpu.readIMUData();
-    input = mpu.kalAngleY+180;
+    setPIDTuningValues();
 }
 
 
@@ -125,6 +242,6 @@ void readPIDTuningValues()
     int potKd = analogRead(A2);
 
     kp = map(potKp, 0, 1023, 0, 25000) / 100.0; //0 - 250
-    ki = map(potKi, 0, 1023, 0, 25000) / 100.0; //0 - 250
+    ki = map(potKi, 0, 1023, 0, 100000) / 100.0; //0 - 1000
     kd = map(potKd, 0, 1023, 0, 500) / 100.0; //0 - 5
 }