Diff: main.cpp

Revision:

8:3c174bef1dbd

Parent:

7:fe796ee2f59f

Child:

9:7b0f9fd4586b

--- a/main.cpp	Thu Jan 07 13:46:32 2021 +0000
+++ b/main.cpp	Sat Mar 27 00:06:06 2021 +0000
@@ -1,125 +1,38 @@
-// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
-// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
-// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
-//
-// Changelog:
-// 2013-05-08 - added seamless Fastwire support
-// - added note about gyro calibration
-// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
-// 2012-06-20 - improved FIFO overflow handling and simplified read process
-// 2012-06-19 - completely rearranged DMP initialization code and simplification
-// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
-// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
-// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
-// - add 3D math helper file to DMP6 example sketch
-// - add Euler output and Yaw/Pitch/Roll output formats
-// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
-// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
-// 2012-05-30 - basic DMP initialization working
+// Flex balancing robot using MPU6050 DMP (MotionApps v2.0)
+// 26/03/2021 by Ferréol Gagey <ferreol.gagey@ens-paris-saclay.fr>
 
-/* ============================================
-I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
+// Changelog:
+// 26/03/2021 - simple pid without encoder
 
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
+//===============================================
 
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-===============================================
-*/
 
 // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
 // for both classes must be in the include path of your project
 #include "I2Cdev.h"
 #include "mbed.h"
 #include <math.h>
-DigitalOut leds[] = {(LED1), (LED2),(LED3),(LED4)};
+#include "MC33926.h"
+#include "QEI.h"
+//DigitalOut leds[] = {(LED1), (LED2),(LED3),(LED4)};
 
 
 #include "MPU6050_6Axis_MotionApps20.h" // works
-//#include "MPU6050_9Axis_MotionApps41.h"
 
-//#include "MPU6050.h" // not necessary if using MotionApps include file
-
-
-// class default I2C address is 0x68
-// specific I2C addresses may be passed as a parameter here
-// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
-// AD0 high = 0x69
 MPU6050 mpu;
-//MPU6050 mpu(0x69); // <-- use for AD0 high
-
-/* =========================================================================
-NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
-depends on the MPU-6050's INT pin being connected to the Arduino's
-external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
-digital I/O pin 2.
-* ========================================================================= */
-
-/* =========================================================================
-NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
-when using Serial.write(buf, len). The Teapot output uses this method.
-The solution requires a modification to the Arduino USBAPI.h file, which
-is fortunately simple, but annoying. This will be fixed in the next IDE
-release. For more info, see these links:
-
-http://arduino.cc/forum/index.php/topic,109987.0.html
-http://code.google.com/p/arduino/issues/detail?id=958
-* ========================================================================= */
 
 
 #ifndef M_PI
 #define M_PI 3.1415
 #endif
 
-// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
-// quaternion components in a [w, x, y, z] format (not best for parsing
-// on a remote host such as Processing or something though)
-//#define OUTPUT_READABLE_QUATERNION
-
-// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
-// (in degrees) calculated from the quaternions coming from the FIFO.
-// Note that Euler angles suffer from gimbal lock (for more info, see
-// http://en.wikipedia.org/wiki/Gimbal_lock)
-#define OUTPUT_READABLE_EULER
 
 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
 // pitch/roll angles (in degrees) calculated from the quaternions coming
 // from the FIFO. Note this also requires gravity vector calculations.
 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
-//#define OUTPUT_READABLE_YAWPITCHROLL
-
-// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
-// components with gravity removed. This acceleration reference frame is
-// not compensated for orientation, so +X is always +X according to the
-// sensor, just without the effects of gravity. If you want acceleration
-// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
-//#define OUTPUT_READABLE_REALACCEL
-
-// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
-// components with gravity removed and adjusted for the world frame of
-// reference (yaw is relative to initial orientation, since no magnetometer
-// is present in this case). Could be quite handy in some cases.
-//#define OUTPUT_READABLE_WORLDACCEL
-
-// uncomment "OUTPUT_TEAPOT" if you want output that matches the
-// format used for the InvenSense teapot demo
-//#define OUTPUT_TEAPOT
-
+#define OUTPUT_READABLE_YAWPITCHROLL
 
 
 bool blinkState = false;
@@ -153,6 +66,56 @@
     mpuInterrupt = true;
 }
 
+/* Motors*/
+//MC33926 motorRight(D8,D9,A2);
+MC33926 motorLeft(D4,D6,A3);
+
+/* Encoders*/
+
+QEI rightEncoder(D3,D2,NC,360);
+QEI leftEncoder(A4,A5,NC,360);
+int pulseleftcoder;
+int pulserightcoder;
+
+/* bluetooth module*/
+Serial blue(D1, D0);  // Bluetooth TX, RX
+
+
+/* Variable declarations */
+float pitchAngle = 0;
+float rollAngle = 0;
+bool dir;           // direction of movement
+float Kp = 0.5;
+float Ki = 0.00001;
+float Kd = 0.01;//0.01;//0.05;
+float set_point = -0.8;         // Angle for staying upright
+float errorPID = 0;             // Proportional term (P) in PID
+float last_errorPID =0;         // Previous error value
+float integral = 0;             // Integral (I)
+float derivative = 0;           // Derivative (D)
+float outputPID = 0;            // Output of PID calculations
+int phone_char;                 //char returned by th bluetooth module
+
+
+
+Ticker toggler1;               // Ticker for led toggling
+Ticker gyro;                 // Ticker for periodic call to compFilter funcçs 
+Ticker balance;                 // Periodic routine for PID control of balance system
+Ticker speed;                  // Periodic routine for speed control
+Ticker bluetooth;              // Ticker for navigation
+
+void toggle_led1();
+void toggle_led2();
+void getAngle();
+void balancePID();
+void balanceControl();
+void Forward(float);
+void Reverse(float);
+void Stop(void);
+void Navigate();
+void TurnRight(float);
+void TurnLeft(float);
+
 // ================================================================
 // === INITIAL SETUP ===
 // ================================================================
@@ -161,18 +124,18 @@
 {
     
     //Pin Defines for I2C Bus
-#define D_SDA                  D4
-#define D_SCL                  D5
+#define D_SDA                  D14
+#define D_SCL                  D15
 //#define D_SDA                  p28
 //#define D_SCL                  p27
 I2C i2c(D_SDA, D_SCL);
 
 
 // mbed Interface Hardware definitions
-    DigitalOut myled1(LED1);
-    DigitalOut myled2(LED2);
-    DigitalOut myled3(LED3);
-    DigitalOut heartbeatLED(LED4);
+    //DigitalOut myled1(LED1);
+    //DigitalOut myled2(LED2);
+    //DigitalOut myled3(LED3);
+    //DigitalOut heartbeatLED(LED4);
 
 // initialize serial communication
 // (115200 chosen because it is required for Teapot Demo output, but it's
@@ -201,18 +164,18 @@
     // wait for ready
     pc.printf("\nSend any character to begin DMP programming and demo: ");
 
-    while(!pc.readable());
-            pc.getc();
-    pc.printf("\n");
+    //while(!pc.readable());
+            //pc.getc();
+    //pc.printf("\n");
      
     // load and configure the DMP
     pc.printf("Initializing DMP...\n");
     devStatus = mpu.dmpInitialize();
 
     // supply your own gyro offsets here, scaled for min sensitivity
-    mpu.setXGyroOffset(-61);
-    mpu.setYGyroOffset(-127);
-    mpu.setZGyroOffset(19);
+    mpu.setXGyroOffset(62);
+    mpu.setYGyroOffset(1);
+    mpu.setZGyroOffset(63);
     mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
 
     // make sure it worked (returns 0 if so)
@@ -241,6 +204,10 @@
         pc.printf("%u",devStatus);
         pc.printf(")\n");
     }
+    //gyro.attach(&getAngle, 0.005);         // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)  
+    balance.attach(&balancePID,  0.010);       // Same period with balancePID
+    speed.attach(&balanceControl, 0.01);
+    bluetooth.attach(&Navigate, 0.05);
 
 
 // ================================================================
@@ -249,9 +216,22 @@
 
 while(1)
 {
+    if(blue.readable()) 
+        {       
+        phone_char = blue.getc();
+        pc.putc(phone_char);
+        pc.printf("Bluetooth Start\r\n");
+        //myled = !myled;
+        }
+      pulseleftcoder=leftEncoder.getPulses();
+    pc.printf("compteur = %d    ",pulseleftcoder);    
+    pc.printf("Roll Angle: %.1f, Pitch Angle: %.1f    ",rollAngle,pitchAngle);
+    pc.printf("Error = %f\r\n",outputPID);
+  
+    
     // if programming failed, don't try to do anything
-    if (!dmpReady) continue;
-        myled2=0;
+    if (!dmpReady) ;//continue;
+        //myled2=0;
         
     // wait for MPU interrupt or extra packet(s) available
 //    while (!mpuInterrupt && fifoCount < packetSize) {
@@ -283,7 +263,7 @@
     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
         // reset so we can continue cleanly
         mpu.resetFIFO();
-        pc.printf("FIFO overflow!");
+        //pc.printf("FIFO overflow!");
 
     // otherwise, check for DMP data ready interrupt (this should happen frequently)
     } else if (mpuIntStatus & 0x02) {
@@ -297,95 +277,140 @@
         // (this lets us immediately read more without waiting for an interrupt)
         fifoCount -= packetSize;
 
-#ifdef OUTPUT_READABLE_QUATERNION
-            // display quaternion values in easy matrix form: w x y z
-            mpu.dmpGetQuaternion(&q, fifoBuffer);
-        //    pc.printf("quat\t");
-            pc.printf("%f",q.w);
-            pc.printf(",");
-            pc.printf("%f",q.x);
-            pc.printf(",");
-            pc.printf("%f",q.y);
-            pc.printf(",");
-            pc.printf("%f\n",q.z);
-#endif
-
-#ifdef OUTPUT_READABLE_EULER
-            // display Euler angles in degrees
-            mpu.dmpGetQuaternion(&q, fifoBuffer);
-            mpu.dmpGetEuler(euler, &q);
-            pc.printf("euler\t");
-            pc.printf("%f",euler[0] * 180/M_PI);
-            pc.printf("\t");
-            pc.printf("%f",euler[1] * 180/M_PI);
-            pc.printf("\t");
-            pc.printf("%f\n",euler[2] * 180/M_PI);
-#endif
-
 #ifdef OUTPUT_READABLE_YAWPITCHROLL
             // display Euler angles in degrees
             mpu.dmpGetQuaternion(&q, fifoBuffer);
             mpu.dmpGetGravity(&gravity, &q);
             mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-            pc.printf("ypr\t");
-            pc.printf("%f3.2",ypr[0] * 180/M_PI);
-            pc.printf("\t");
-            pc.printf("%f3.2",ypr[1] * 180/M_PI);
-            pc.printf("\t");
-            pc.printf("%f3.2\n",ypr[2] * 180/M_PI);
-#endif
-
-#ifdef OUTPUT_READABLE_REALACCEL
-            // display real acceleration, adjusted to remove gravity
-            mpu.dmpGetQuaternion(&q, fifoBuffer);
-            mpu.dmpGetAccel(&aa, fifoBuffer);
-            mpu.dmpGetGravity(&gravity, &q);
-            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
-            pc.printf("areal\t");
-            pc.printf("%d",aaReal.x);
-            pc.printf("\t");
-            pc.printf("%d",aaReal.y);
-            pc.printf("\t");
-            pc.printf("%d\n",aaReal.z);
+            rollAngle=ypr[1] * 180/M_PI;
+            pitchAngle=ypr[2] * 180/M_PI;
+            //pc.printf("ypr\t");
+//            pc.printf("%f3.2",ypr[0] * 180/M_PI);
+//            pc.printf("\t");
+//            pc.printf("%f3.2",ypr[1] * 180/M_PI);
+//            pc.printf("\t");
+//            pc.printf("%f3.2\n",ypr[2] * 180/M_PI);
 #endif
-
-#ifdef OUTPUT_READABLE_WORLDACCEL
-            // display initial world-frame acceleration, adjusted to remove gravity
-            // and rotated based on known orientation from quaternion
-            mpu.dmpGetQuaternion(&q, fifoBuffer);
-            mpu.dmpGetAccel(&aa, fifoBuffer);
-            mpu.dmpGetGravity(&gravity, &q);
-            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
-            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
-            pc.printf("aworld\t");
-            pc.printf("%d",aaWorld.x);
-            pc.printf("\t");
-            pc.printf("%d",aaWorld.y);
-            pc.printf("\t");
-            pc.printf("%d\n",aaWorld.z);
-#endif
-#ifdef OUTPUT_TEAPOT
-// display quaternion values in InvenSense Teapot demo format:
-            teapotPacket[2] = fifoBuffer[0];
-            teapotPacket[3] = fifoBuffer[1];
-            teapotPacket[4] = fifoBuffer[4];
-            teapotPacket[5] = fifoBuffer[5];
-            teapotPacket[6] = fifoBuffer[8];
-            teapotPacket[7] = fifoBuffer[9];
-            teapotPacket[8] = fifoBuffer[12];
-            teapotPacket[9] = fifoBuffer[13];
-            
-            for(int i=0;i<14;i++)
-            {
-            pc.putc(teapotPacket[i]);
-            }
-//            pc.printf("%d",teapotPacket, 14);
-            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
-#endif
-
         // blink LED to indicate activity
-        blinkState = !blinkState;
-        myled1 = blinkState;
+        //blinkState = !blinkState;
+        //myled1 = blinkState;
     }
   }
-}
\ No newline at end of file
+}
+
+void getAngle(){
+    
+}
+
+void balancePID()
+{
+    errorPID = set_point - pitchAngle;          //Proportional (P) 
+    integral += errorPID;                       //Integral (I)
+    derivative = errorPID - last_errorPID;      //Derivative (D)
+    
+    last_errorPID = errorPID;                   //Save current value for next iteration
+    
+    outputPID = (Kp * errorPID) + (Ki * integral) + (Kd * derivative);  //PID calculation
+    
+    /* errorPID is restricted between -1 and 1 */ 
+    if(outputPID > 0.8)
+        outputPID = 0.8;
+    else if(outputPID < -0.8)
+        outputPID = -0.8;   
+}
+
+void balanceControl() 
+{    
+    int direction=0;                                            // Variable to hold direction we want to drive
+    if (outputPID>0)direction=1;                                 // Positive speed indicates forward
+    if (outputPID<0)direction=2;                                 // Negative speed indicates backwards
+    if((abs(pitchAngle)>10.00))direction=0; 
+  
+    switch( direction) {                                        // Depending on what direction was passed
+        case 0:                                                 // Stop case
+            Stop();
+            break;
+        case 1:                                                 // Forward case
+            Forward(abs(outputPID));
+            break;
+        case 2:                                                 // Backwards
+            Reverse(abs(outputPID));
+            break;
+        default:                                                // Catch-all (Stop)
+            Stop();
+            break;
+    }    
+}
+
+void Stop(void)
+{
+    //motorRight.SetPWMPulsewidth(2,0);
+    motorLeft.SetPWMPulsewidth(2,0); 
+}
+ 
+void Forward(float s)
+ {
+    //motorRight.SetPWMPulsewidth(1,s);
+    motorLeft.SetPWMPulsewidth(1,s); 
+ }
+
+void TurnRight(float s)
+ {
+     //motorRight.SetPWMPulsewidth(0,s);
+     motorLeft.SetPWMPulsewidth(1,s);
+ }
+ 
+void TurnLeft(float s)
+ {
+     //motorRight.SetPWMPulsewidth(1,s);
+     motorLeft.SetPWMPulsewidth(0,s);
+ }
+
+void Reverse(float s)
+ {
+    //motorRight.SetPWMPulsewidth(0,s);
+    motorLeft.SetPWMPulsewidth(0,s); 
+ }
+ 
+ 
+ 
+ void Navigate()
+ {
+     switch (phone_char)
+            {
+                case 'f':
+                Forward(0.7);
+                break;
+                
+                case 'F':
+                Forward(0.7);
+                break;
+                
+                case 'b': 
+                Reverse(0.7);
+                break;
+                
+                case 'B': 
+                Reverse(0.7);
+                break;
+                
+                case 'r':
+                TurnRight(0.7);
+                break;
+                
+                case 'R':
+                TurnRight(0.7);
+                break;
+                
+                case 'l':
+                TurnLeft(0.7);
+                break;
+                
+                case 'L':
+                TurnLeft(0.7);
+                break;
+                
+                default:
+                Stop();
+            }
+ }
\ No newline at end of file