Montvydas Klumbys / Mbed 2 deprecated QuadcopterProgram

Program used to control a quadcopter. It uses a PID library which can be found in: http://developer.mbed.org/cookbook/PID I also uses my own written library for easily controlling quadcopter motors, which can be found in: https://developer.mbed.org/users/moklumbys/code/Quadcopter/ One more library that I used is MPU6050 that was previously written by Erik Olieman but I was able to update it with new functions: https://developer.mbed.org/users/moklumbys/code/MPU6050/

Dependencies:   MPU6050 PID Quadcopter Servo mbed

Diff: main.cpp

Revision:
2:3161f535d71a
Parent:
1:40ade596b1e3
Child:
3:5f43c8374ff2
--- a/main.cpp	Thu Feb 19 00:16:33 2015 +0000
+++ b/main.cpp	Thu Feb 19 10:38:43 2015 +0000
@@ -28,18 +28,18 @@
 
 #define PITCH_IN_MIN -90.0
 #define PITCH_IN_MAX 90.0
-#define PITCH_OUT_MIN -0.1
-#define PITCH_OUT_MAX 0.1
+#define PITCH_OUT_MIN -0.5
+#define PITCH_OUT_MAX 0.5
 
 #define ROLL_IN_MIN -90.0
 #define ROLL_IN_MAX 90.0
-#define ROLL_OUT_MIN -0.1
-#define ROLL_OUT_MAX 0.1
+#define ROLL_OUT_MIN -0.5
+#define ROLL_OUT_MAX 0.5
 
 #define Kc 0.1
 #define Ti 0.1
 #define Td 0.0
-#define RATE 0.1
+#define RATE 0.01
 //--------------------------------ALL THE FUNCTION HEADERS-----------------------
 float map(float x, float in_min, float in_max, float out_min, float out_max);
 //---------------------------------------END-------------------------------------
@@ -53,6 +53,7 @@
 PID pitchPID (Kc, Ti, Td, RATE);
 PID rollPID (Kc, Ti, Td, RATE);
 
+//***************************************STARTING MAIN FUNCTION*********************
 int main() {
     pc.baud (115200);
     
@@ -87,8 +88,9 @@
     pitchPID.setMode(AUTO_MODE);    //start stabilising by puting AUTO mode
     rollPID.setMode(AUTO_MODE);
     
+    //need to vary this one to move quadcopter
     pitchPID.setSetPoint (0.0); //seting the middle point.. Or smth like that, need to look into it more
-    rollPID.setSetPoint (0.0);
+    rollPID.setSetPoint (0.0);  //meaning that quadcopter is flying at a constant place.. no turning, blah blah blah
 
     mpu.getOffset(accOffset, gyroOffset, OFFSET_SAMPLES); //take some samples at the beginning to get an offset
     wait(0.1);                //wait to settle down
@@ -96,6 +98,10 @@
     timer.start();  //will need timer to detect when was the last time the values were updated
     prevTime = timer.read();
     
+    for (int i = 0; i < 4; i++){
+        speed[i] = 0.0;
+    }
+//-------------------------------------------START INFINITE LOOP-------------------------------------------------
     while(1) {
 //        for (float height = 0.0; height < 0.5; height+=0.05){
 //            for (int i = 0; i < 4; i++){
@@ -106,23 +112,28 @@
 //        }
 //        for (uint16_t i = 0; i < 600; i++)
 //        {
-            currTime = timer.read();
-            mpu.computeAngle (angle, accOffset, gyroOffset, &currTime, &prevTime); // get angle 
-            prevTime = timer.read();
+            currTime = timer.read();    //get present time
+            mpu.computeAngle (angle, accOffset, gyroOffset, &currTime, &prevTime); // get angle using all these values
+            
+            rollPID.setInterval(timer.read()-prevTime);         //need to change the interval because don't know how much time passed
+            pitchPID.setInterval(timer.read()-prevTime);
+            
+            prevTime = timer.read();    //get present time -> will be used later on as previous value
         
-            rollPID.setProcessValue (angle[X_AXIS]);
+            rollPID.setProcessValue (angle[X_AXIS]);    //take some values to do processing
             pitchPID.setProcessValue (angle[Y_AXIS]);
         
-            pitchDiff = pitchPID.compute();
+            pitchDiff = pitchPID.compute();     //compute the difference
             rollDiff = rollPID.compute();
         
-            quad.stabilise(speed, actSpeed, rollDiff, pitchDiff);
+            quad.stabilise(speed, actSpeed, rollDiff, pitchDiff);   //stabilise the speed by giving new actSpeed value
             
-            pc.printf("x=%0.3f y=%0.3f z=%0.3f\n", angle[X_AXIS], angle[Y_AXIS], angle[Z_AXIS]);
+            //print some values to check how thing work out
+     //       pc.printf("x=%0.3f y=%0.3f z=%0.3f\n", angle[X_AXIS], angle[Y_AXIS], angle[Z_AXIS]);
             pc.printf("Speed_FL=%0.4f, Speed_FR=%0.4f, Speed_BL= %0.4f, Speed_BR=%0.4f\n", speed[FL], speed[FR], speed[BL], speed[BR]);
             pc.printf("ActSpeed_FL=%0.4f, ActSpeed_FR=%0.4f, ActSpeed_BL=%0.4f, ActSpeed_BR=%0.4f\n", actSpeed[FL], actSpeed[FR], actSpeed[BL], actSpeed[BR]);
             
-            quad.run(actSpeed);
+     //       quad.run(actSpeed); //run the motors at the spesified speed actSpeed
             
             wait (0.01);
 //        }
@@ -137,6 +148,7 @@
 //        wait(1);
     }
 }
+//************************************************END MAIN FUNCTION********************************************************
 
 //-----------------------------Mapping function-----------------------------
 float map(float x, float in_min, float in_max, float out_min, float out_max){