Revision 4:eb418af66d81, committed 2015-02-19

Comitter:

moklumbys

Date:

Thu Feb 19 23:30:15 2015 +0000

Parent:

3:5f43c8374ff2

Child:

5:8b3f82abe3a4

Commit message:

Adding comments, modified some thing to make things work as expected in reality....;

Changed in this revision

--- a/Quadcopter.lib	Thu Feb 19 12:20:06 2015 +0000
+++ b/Quadcopter.lib	Thu Feb 19 23:30:15 2015 +0000
@@ -1,1 +1,1 @@
-Quadcopter#62e387f1d095
+Quadcopter#84d246dccb8d

--- a/main.cpp	Thu Feb 19 12:20:06 2015 +0000
+++ b/main.cpp	Thu Feb 19 23:30:15 2015 +0000
@@ -1,71 +1,74 @@
-
-//firstly enable calibrating
 #include "mbed.h"
 #include "Quadcopter.h"
 #include "PID.h"
 #include "MPU6050.h"
 #include "Timer.h"
 
-#define MAX_CALIBRATE 1.0
-#define MIN_CALIBRATE 0.35
+//defines the number of samples to be taken when calculating the offset for gyro and accelerometer
+#define OFFSET_SAMPLES 50
 
-#define OFFSET_SAMPLES 50
 //define how the accelerometer is placed on surface
 #define X_AXIS 1
 #define Y_AXIS 2
 #define Z_AXIS 0
 
+//ESC calibration values
 #define MAX_CALIBRATE 1.0
 #define MIN_CALIBRATE 0.35
 
+//Just to remember which motor corresponds to which number...
 #define FL   0    // Front left    
 #define FR   1    // Front right
 #define BL   2    // back left
 #define BR   3    // back right
 
+//input and output values for pitch
 #define PITCH_IN_MIN -90.0
 #define PITCH_IN_MAX 90.0
 #define PITCH_OUT_MIN -0.3
 #define PITCH_OUT_MAX 0.3
 
+//input and output values for roll
 #define ROLL_IN_MIN -90.0
 #define ROLL_IN_MAX 90.0
 #define ROLL_OUT_MIN -0.3
 #define ROLL_OUT_MAX 0.3
 
+//PID intervals/constants
 #define Kc 0.5
 #define Ti 0.01
 #define Td 0.00
 #define RATE 0.01
+
 //--------------------------------ALL THE FUNCTION HEADERS-----------------------
-float map(float x, float in_min, float in_max, float out_min, float out_max);
+float map(float x, float in_min, float in_max, float out_min, float out_max);   //might be a useful function. One is used inside Quadcopter library though
 //---------------------------------------END-------------------------------------
 
-Quadcopter quad (p21, p22, p23, p24);
-Serial pc(USBTX, USBRX); // tx, rx
-MPU6050 mpu(p9, p10);   //Also disables sleep mode
-Timer timer;
+Quadcopter quad (p21, p22, p23, p24);   //intance of the Quadcopter class
+Serial pc(USBTX, USBRX);                // tx, rx
+MPU6050 mpu(p9, p10);                   //Also disables sleep mode
+Timer timer;                            //need a timer to tell how much time passed from the last calculation
 
-//Kc, Ti, Td, interval
+//put Kc, Ti, Td, interval for both pitch and roll PID models
 PID pitchPID (Kc, Ti, Td, RATE);
 PID rollPID (Kc, Ti, Td, RATE);
 
 //***************************************STARTING MAIN FUNCTION*********************
 int main() {
-    pc.baud (115200);
+    pc.baud (115200);   //fast transmition speed...
     
-    float pitchDiff;
-    float rollDiff;
+    float pitchDiff;    //difference in pitch. Explained in PID library...
+    float rollDiff;     //diference in roll
     
-    float speed[4];
-    float actSpeed[4];
+    float speed[4];     //speed for motors
+    float actSpeed[4];  //actual speed of for all motors
     
     float accOffset[3]; //offset values
     float gyroOffset[3];
     float angle[3];     //total calculated angle
 
-    float currTime;
-    float prevTime;
+    float currTime;     //current time variable will be given in the funtion
+    float prevTime;     //previous time values will be given in the function 
     
     if (mpu.testConnection())   //just testing if things are working...
         pc.printf("MPU connection succeeded\n\r");
@@ -74,7 +77,7 @@
     
     mpu.setAlpha(0.97);     //set Alpha coefficient for low/high pass filters
     
-//    quad.calibrate(MIN_CALIBRATE, MAX_CALIBRATE);   //calibrating motors
+    quad.calibrate(MIN_CALIBRATE, MAX_CALIBRATE);   //calibrating motors
     
     pitchPID.setInputLimits (PITCH_IN_MIN, PITCH_IN_MAX);   //seting input and output limits for both pitch and roll
     pitchPID.setOutputLimits (PITCH_OUT_MIN, PITCH_OUT_MAX);
@@ -86,64 +89,50 @@
     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);  //meaning that quadcopter is flying at a constant place.. no turning, blah blah blah
+    pitchPID.setSetPoint (0.0);     //seting the middle point meaning that quadcopter is balancing in one place
+    rollPID.setSetPoint (0.0); 
 
     mpu.getOffset(accOffset, gyroOffset, OFFSET_SAMPLES); //take some samples at the beginning to get an offset
-    wait(0.1);                //wait to settle down
+    wait(0.1);                                            //wait to settle down
     
-    timer.start();  //will need timer to detect when was the last time the values were updated
-    prevTime = timer.read();
+    timer.start();                  //will need timer to detect when was the last time the values were updated
+    prevTime = timer.read();        //set previous timer value
     
-    for (int i = 0; i < 4; i++){
+    for (int i = 0; i < 4; i++){    //initialise speed to be 0.0
         speed[i] = 0.0;
     }
+    
+//        for (int i = 0; i < 4; i++){  //start running motors ar 20% jus to make sure everything works fine
+//            speed[i] = 0.2;
+//        }
+//        quad.run (speed);
 //-------------------------------------------START INFINITE LOOP-------------------------------------------------
-pitchPID.setSetPoint (45.0);    //head forward! by 45 degrees :D
     while(1) {
-//        for (float height = 0.0; height < 0.5; height+=0.05){
-//            for (int i = 0; i < 4; i++){
-//                speed[i] = height;
-//            }
-//            quad.run (speed);
-//            wait(0.1);
-//        }
-//        for (uint16_t i = 0; i < 600; i++)
-//        {
-            currTime = timer.read();    //get present time
-            mpu.computeAngle (angle, accOffset, gyroOffset, &currTime, &prevTime); // get angle using all these values
+            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]);    //take some values to do processing
+            rollPID.setProcessValue (angle[X_AXIS]);    //take angle values, which correspond to pitch and roll and do processing
             pitchPID.setProcessValue (angle[Y_AXIS]);
         
             pitchDiff = pitchPID.compute();     //compute the difference
             rollDiff = rollPID.compute();
-        pc.printf ("pitchDiff=%0.4f, rollDiff=%0.4f\n", pitchDiff, rollDiff);
-            quad.stabilise(speed, actSpeed, rollDiff, pitchDiff);   //stabilise the speed by giving new actSpeed value
+            
+            pc.printf ("pitchDiff=%0.4f, rollDiff=%0.4f\n", pitchDiff, rollDiff);
+            quad.stabilise(speed, actSpeed, rollDiff, pitchDiff);   //stabilise the speed by giving out actual Speed value
             
             //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); //run the motors at the spesified speed actSpeed
-            
+            quad.run(actSpeed); //run the motors at the spesified speed actSpeed
             wait (0.01);
-//        }
-        
-//        for (float height = 0.5; height > 0.0; height-=0.05){
-//            for (int i = 0; i < 4; i++){
-//                speed[i] = height;
-//            }
-//            quad.run (speed);
-//            wait(0.1);
-//        }
-//        wait(1);
     }
 }
 //************************************************END MAIN FUNCTION********************************************************