Diff: main.cpp

Revision:

1:40ade596b1e3

Parent:

0:894ba50f267c

Child:

2:3161f535d71a

--- a/main.cpp	Wed Feb 18 23:44:09 2015 +0000
+++ b/main.cpp	Thu Feb 19 00:16:33 2015 +0000
@@ -1,5 +1,22 @@
 #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
+
+#define FL   0    // Front left    
+#define FR   1    // Front right
+#define BL   2    // back left
+#define BR   3    // back right
+
+#define OFFSET_SAMPLES 50
+//define how the accelerometer is placed on surface
+#define X_AXIS 1
+#define Y_AXIS 2
+#define Z_AXIS 0
 
 #define MAX_CALIBRATE 1.0
 #define MIN_CALIBRATE 0.35
@@ -8,55 +25,120 @@
 #define FR   1    // Front right
 #define BL   2    // back left
 #define BR   3    // back right
+
+#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 ROLL_IN_MIN -90.0
+#define ROLL_IN_MAX 90.0
+#define ROLL_OUT_MIN -0.1
+#define ROLL_OUT_MAX 0.1
+
+#define Kc 0.1
+#define Ti 0.1
+#define Td 0.0
+#define RATE 0.1
 //--------------------------------ALL THE FUNCTION HEADERS-----------------------
 float map(float x, float in_min, float in_max, float out_min, float out_max);
 //---------------------------------------END-------------------------------------
 
 Quadcopter quad (p21, p22, p23, p24);
 Serial pc(USBTX, USBRX); // tx, rx
+MPU6050 mpu(p9, p10);   //Also disables sleep mode
+Timer timer;
+
+//Kc, Ti, Td, interval
+PID pitchPID (Kc, Ti, Td, RATE);
+PID rollPID (Kc, Ti, Td, RATE);
 
 int main() {
+    pc.baud (115200);
+    
     float pitchDiff;
     float rollDiff;
+    
     float speed[4];
     float actSpeed[4];
     
-    quad.calibrate(MIN_CALIBRATE, MAX_CALIBRATE);
+    float accOffset[3]; //offset values
+    float gyroOffset[3];
+    float angle[3];     //total calculated angle
+
+    float currTime;
+    float prevTime;
+    
+    if (mpu.testConnection())   //just testing if things are working...
+        pc.printf("MPU connection succeeded\n\r");
+    else
+        pc.printf("MPU connection failed\n\r");
+    
+    mpu.setAlpha(0.97);     //set Alpha coefficient for low/high pass filters
+    
+    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);
+
+    rollPID.setInputLimits (ROLL_IN_MIN, ROLL_IN_MAX);
+    rollPID.setOutputLimits (ROLL_OUT_MIN, ROLL_OUT_MAX);
+    
+    pitchPID.setMode(AUTO_MODE);    //start stabilising by puting AUTO mode
+    rollPID.setMode(AUTO_MODE);
+    
+    pitchPID.setSetPoint (0.0); //seting the middle point.. Or smth like that, need to look into it more
+    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
+    
+    timer.start();  //will need timer to detect when was the last time the values were updated
+    prevTime = timer.read();
+    
     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++)
-        {
-            rollPID.setProcessValue (imu.roll);
-            rollPID.setProcessValue (imu.pitch);
+//        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();
+            mpu.computeAngle (angle, accOffset, gyroOffset, &currTime, &prevTime); // get angle 
+            prevTime = timer.read();
         
-            pitchDif = pitchPID.compute();
-            rollDif = rollPID.compute();
+            rollPID.setProcessValue (angle[X_AXIS]);
+            pitchPID.setProcessValue (angle[Y_AXIS]);
+        
+            pitchDiff = pitchPID.compute();
+            rollDiff = rollPID.compute();
         
             quad.stabilise(speed, actSpeed, rollDiff, pitchDiff);
+            
+            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);
             
             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);
+//        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);
     }
 }
 
 //-----------------------------Mapping function-----------------------------
-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){
   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
\ No newline at end of file