Diff: main.cpp

Revision:

1:e08a4f517989

Parent:

0:54b67cd15a5b

--- a/main.cpp	Sun Dec 23 23:50:21 2012 +0000
+++ b/main.cpp	Tue Aug 27 09:38:49 2013 +0000
@@ -1,71 +1,147 @@
+/*
+                         F                  +X
+                         |                   | 
+                         |                   |
+                 L ------|------R    -Y------|------+Y
+                         |                   |
+                         |                   |
+                         B                  -X
+*/
+#include "mbed.h"
 #include "rtos.h"
 #include "GlobalDefines.h"
-#include "MPU.h"
+#include "MPU6050.h"
+#include "BMP085.h"
+//#include "DCM.h" Drifts a lot, needs tuning 
+#include "PID.h"
 #include "Motors.h"
 
-#define FlightControlUpdateRate 5
-#define RxUpdateRate 20
-#define Enable_Serial
+#define Debugging
+
+typedef enum{Armed=0,
+UnArmed
+}State;
+
+State MotorState = Armed;
+bool print = false;
+bool P_readingReady=false;
+bool T_readingReady=false;
+char c ;
 
-#ifdef Enable_Serial
-Serial pc(USBTX, USBRX);
-#endif
+uint8 count=0;
+int16 Temperature=0;
+int32 Pressure=0;
+
 
- 
+PIDClass PitchPID(4,5,2.5,0.1); // This is a nested PID loop
+                               // Kp,Ki,Kp,Kd,iLim..... best till now 4,0,2.5,0.1
+MPU6050 Sensor;
+BMP085 BMP;
+//DCM IMU;
+
+void FlightControl(void const *args){
 
-void RX_thread(void const *args){
+        //Get Sensor Data
+        Sensor.GyroRate();
+        Sensor.AccAngle();
+        
+        ///////////////////////BMP085 Contorol switch statement(Work in Progress!!!)//////////////////////////////////////
+        switch(count){
+        
+        case 0: 
+        BMP.readUT_Flag();
+        count+=1; 
+        break;
+        
+        
+        case 1: 
+        Temperature=BMP.read_Temperature();
+        T_readingReady=true;
+        BMP.readUP_Flag();
+        count+=1;
+        break;
+        
+        
+        case 7: 
+        Pressure = BMP.read_Pressure();
+        P_readingReady=true;
+        count=0;
+        break;
+        
+        default: count+=1; break;
+        }
+    ///////////////////////////////////////////////////////////////////////////////////////////
+    
+        //Gotta Keep Time
+        deltaTime = Global.read();
+        Global.reset();
      
-     while(true){
-     RX_Data[3] = Throttle.read();
-     #ifdef Enable_Serial
-     //pc.printf("%d\n",RX_Data[3]);
-     #endif
-     Thread::wait(RxUpdateRate);
-     }
+         pitch = (0.99)*(pitch+Sensor.Scaled_GyroX*deltaTime) + (0.01)*(Sensor.accangle[0]);
+         roll  = (0.99)*(roll+ Sensor.Scaled_GyroY*deltaTime) + (0.01)*(Sensor.accangle[1]);
+        
+
+         //IMU.Update_DCM(deltaTime,ToRad(Sensor.Scaled_GyroX),ToRad(Sensor.Scaled_GyroY),ToRad(Sensor.Scaled_GyroZ),Sensor.Scaled_AccX,Sensor.Scaled_AccY,Sensor.Scaled_AccZ);
+
+        //Update PID 
+        if(Throttle>=1200){
+        PitchCorrection= PitchPID.update(PitchSetpoint,pitch,Sensor.Scaled_GyroX,deltaTime);
+        }
+        
+        //Motor Commands
+        if(MotorState==Armed){
+        updateMotors(PitchCorrection,0);
+        }
+    
+        print = true;
 }
 
+
 int main(){
-
-    //i2c.frequency(400000);
+    
+    #ifdef Debugging
+    pc.baud(115200);
+    #endif
     
-    #ifdef Enable_Serial
-        pc.baud(57600);
-    #endif
+    //Start-Up functions
+    __disable_irq();
+     CalibrateESC(); // Throttle Sweep on startup
+     two=1;        // check sensor connection
+     while(!Sensor.CheckConnection()); //check sensor connection
+     two=0;        // sensor okay 
+     BMP.Calibrate(); // Calibrations sequence for BMP... TODO: store values in LocalFileSystem
+     Sensor.MPU_Setup(); // Setup MPU6050
+     Sensor.CalibrateGyro(); // Calibrate Gyros  (averaging 100 readings)
+     Sensor.CalibrateAcc(); //  Calibrate ACC    (averaging 100 readings)
+     one=1;   // System okay to start 
+     __enable_irq();    
      
-     one=1;
-     MPU_Setup();
-     CalibrateGyro();
-     CalibrateAcc();
-     InitializeMotors();
-     one=0;
- 
- Thread thread(RX_thread);
- 
- Global.start();
+     RtosTimer Fly(FlightControl,osTimerPeriodic,NULL); // Initialize RTOS Timer  
+     Global.start(); // Start timer
+     Fly.start(5);  // RTOS Timer (200Hz Control Loop)
  
  while(true){
-    
-    // Get Sensor Data
-    filterGyro();
-    AccAngle();
-    
-    // Gotta Keep Time
-    deltaTime = Global.read();
-    Global.reset();
-    
-    //Pitch and Roll Calculation using complimentary filter 
-    //pitch= (0.98)*(pitch+Scaled_GyroX*deltaTime) + (0.02)*(accangle[0]);
-    pitch+= Scaled_GyroX*deltaTime;
-    
-    // update PID and Motor Commands
-    //updatePID();
-    updateMotors();
-    
-    #ifdef Enable_Serial
-    pc.printf("%f,%f\n",pitch,accangle[0]);
-    #endif
-    
-    Thread::wait(FlightControlUpdateRate);
+           
+          if(pc.readable()){
+          c = pc.getc();
+          if(c == 'a'||c=='A') MotorState = Armed;
+            else if(c == 's'||c=='S') MotorState = UnArmed;
+                else if(c == 'u'||c=='U') Throttle+=50 ;
+                    else if(c == 'i'||c=='I') Throttle-=50;
+                        else if(c=='m'||c=='M') PitchSetpoint+=10;
+                            else if(c=='n'||c=='N') PitchSetpoint-=10;
 
+          //print=true;
+          }
+          
+          #ifdef Debugging
+          if(print){
+          //pc.printf("%d\n",Temperature);
+          pc.printf("%6.2f\n",pitch);
+          //T_readingReady=false;
+          print=false;
+          }
+          #endif
+          
+          Thread::wait(20);         
 }
 }