Revision 6:5e815d4b4d8f, committed 2015-02-25

Comitter:

moklumbys

Date:

Wed Feb 25 00:56:18 2015 +0000

Parent:

5:8b3f82abe3a4

Child:

7:0c31ccba7b3c

Commit message:

added bias values for PID controllers which corrected the problem with not giving negative (or sometimes positive) difference values.

Changed in this revision

--- a/Quadcopter.lib	Fri Feb 20 00:52:54 2015 +0000
+++ b/Quadcopter.lib	Wed Feb 25 00:56:18 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/moklumbys/code/Quadcopter/#5ab77c583ae3
+http://developer.mbed.org/users/moklumbys/code/Quadcopter/#22c52af13ac2

--- a/main.cpp	Fri Feb 20 00:52:54 2015 +0000
+++ b/main.cpp	Wed Feb 25 00:56:18 2015 +0000
@@ -14,7 +14,7 @@
 
 //ESC calibration values
 #define MAX_CALIBRATE 1.0
-#define MIN_CALIBRATE 0.35
+#define MIN_CALIBRATE 0.0
 
 //Just to remember which motor corresponds to which number...
 #define FL   0    // Front left    
@@ -25,18 +25,18 @@
 //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
+#define PITCH_OUT_MIN -0.2
+#define PITCH_OUT_MAX 0.2
 
 //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
+#define ROLL_OUT_MIN -0.2
+#define ROLL_OUT_MAX 0.2
 
 //PID intervals/constants
-#define Kc 0.5
-#define Ti 0.01
+#define Kc 1.0
+#define Ti 0.00
 #define Td 0.00
 #define RATE 0.01
 
@@ -45,6 +45,7 @@
 //---------------------------------------END-------------------------------------
 
 Quadcopter quad (p21, p22, p23, p24);   //intance of the Quadcopter class
+Serial bt(p13, p14);    //initialise a bluetooth module
 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
@@ -55,8 +56,12 @@
 
 //***************************************STARTING MAIN FUNCTION*********************
 int main() {
+    bt.baud(9600); 
     pc.baud (115200);   //fast transmition speed...
     
+    float val = 0.0;
+    char buff;  //buffers when bad value received
+    
     float pitchDiff;    //difference in pitch. Explained in PID library...
     float rollDiff;     //diference in roll
     
@@ -76,7 +81,8 @@
     
     mpu.setAlpha(0.97);     //set Alpha coefficient for low/high pass filters
     
-    quad.calibrate(MIN_CALIBRATE, MAX_CALIBRATE);   //calibrating motors
+    quad.setLimits(MIN_CALIBRATE, MAX_CALIBRATE);   //set calibration limits for the system
+//    quad.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);
@@ -84,6 +90,9 @@
     rollPID.setInputLimits (ROLL_IN_MIN, ROLL_IN_MAX);
     rollPID.setOutputLimits (ROLL_OUT_MIN, ROLL_OUT_MAX);
     
+    pitchPID.setBias(0.0);      //need to define middle point!!!! very important!!!
+    rollPID.setBias(0.0);
+    
     pitchPID.setMode(AUTO_MODE);    //start stabilising by puting AUTO mode
     rollPID.setMode(AUTO_MODE);
     
@@ -96,17 +105,33 @@
     
     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++){    //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-------------------------------------------------
     while(1) {
+        if(bt.readable()){
+            if (!bt.scanf("%f", &val)){
+                bt.scanf("%c", &buff);
+            } else {
+                    pitchPID.setTunings(val, 0.0, 0.0);
+                    rollPID.setTunings(val, 0.0, 0.0);  
+                    pc.printf ("______________________SET______________________\n"); 
+                }             
+//                if (buff == 'k'){
+//                    bt.scanf("%f", &val);
+//                    pitchPID.setTunings(val, 0.0, 0.0);
+//                    rollPID.setTunings(val, 0.0, 0.0);
+//                }
+//                if (buff == 's'){
+//                    bt.scanf("%f", &val);
+//                    spd = val;
+//                }
+     //       }
+        }
+        
+        for (int i = 0; i < 4; i++){    //initialise speed to be 0.0
+            speed[i] = 0.3;
+        }   
+        quad.run (speed); 
+            
             mpu.computeAngle (angle, accOffset, gyroOffset, timer.read()-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
@@ -114,20 +139,19 @@
             
             prevTime = timer.read();    //get present time -> will be used later on as previous value
         
-            rollPID.setProcessValue (angle[X_AXIS]);    //take angle values, which correspond to pitch and roll and do processing
-            pitchPID.setProcessValue (angle[Y_AXIS]);
+            rollPID.setProcessValue (angle[Y_AXIS]);    //take angle values, which correspond to pitch and roll and do processing
+            pitchPID.setProcessValue (angle[X_AXIS]);
         
             pitchDiff = pitchPID.compute();     //compute the difference
             rollDiff = rollPID.compute();
             
-            pc.printf ("pitchDiff=%0.4f, rollDiff=%0.4f\n", pitchDiff, rollDiff);
+        //    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("x=%0.3f y=%0.3f z=%0.3f\n", angle[X_AXIS]+90, angle[Y_AXIS]+90, 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]);
-            
+            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
             wait (0.01);
     }