main.cpp

00001 /*
00002                          F                  +X
00003                          |                   | 
00004                          |                   |
00005                  L ------|------R    -Y------|------+Y
00006                          |                   |
00007                          |                   |
00008                          B                  -X
00009 */
00010 #include "mbed.h"
00011 #include "rtos.h"
00012 #include "GlobalDefines.h"
00013 #include "MPU6050.h"
00014 #include "BMP085.h"
00015 //#include "DCM.h" Drifts a lot, needs tuning 
00016 #include "PID.h"
00017 #include "Motors.h"
00018 
00019 #define Debugging
00020 
00021 typedef enum{Armed=0,
00022 UnArmed
00023 }State;
00024 
00025 State MotorState = Armed;
00026 bool print = false;
00027 bool P_readingReady=false;
00028 bool T_readingReady=false;
00029 char c ;
00030 
00031 uint8 count=0;
00032 int16 Temperature=0;
00033 int32 Pressure=0;
00034 
00035 
00036 PIDClass PitchPID(4,5,2.5,0.1); // This is a nested PID loop
00037                                // Kp,Ki,Kp,Kd,iLim..... best till now 4,0,2.5,0.1
00038 MPU6050 Sensor;
00039 BMP085 BMP;
00040 //DCM IMU;
00041 
00042 void FlightControl(void const *args){
00043 
00044         //Get Sensor Data
00045         Sensor.GyroRate();
00046         Sensor.AccAngle();
00047         
00048         ///////////////////////BMP085 Contorol switch statement(Work in Progress!!!)//////////////////////////////////////
00049         switch(count){
00050         
00051         case 0: 
00052         BMP.readUT_Flag();
00053         count+=1; 
00054         break;
00055         
00056         
00057         case 1: 
00058         Temperature=BMP.read_Temperature();
00059         T_readingReady=true;
00060         BMP.readUP_Flag();
00061         count+=1;
00062         break;
00063         
00064         
00065         case 7: 
00066         Pressure = BMP.read_Pressure();
00067         P_readingReady=true;
00068         count=0;
00069         break;
00070         
00071         default: count+=1; break;
00072         }
00073     ///////////////////////////////////////////////////////////////////////////////////////////
00074     
00075         //Gotta Keep Time
00076         deltaTime = Global.read();
00077         Global.reset();
00078      
00079          pitch = (0.99)*(pitch+Sensor.Scaled_GyroX*deltaTime) + (0.01)*(Sensor.accangle[0]);
00080          roll  = (0.99)*(roll+ Sensor.Scaled_GyroY*deltaTime) + (0.01)*(Sensor.accangle[1]);
00081         
00082 
00083          //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);
00084 
00085         //Update PID 
00086         if(Throttle>=1200){
00087         PitchCorrection= PitchPID.update(PitchSetpoint,pitch,Sensor.Scaled_GyroX,deltaTime);
00088         }
00089         
00090         //Motor Commands
00091         if(MotorState==Armed){
00092         updateMotors(PitchCorrection,0);
00093         }
00094     
00095         print = true;
00096 }
00097 
00098 
00099 int main(){
00100     
00101     #ifdef Debugging
00102     pc.baud(115200);
00103     #endif
00104     
00105     //Start-Up functions
00106     __disable_irq();
00107      CalibrateESC(); // Throttle Sweep on startup
00108      two=1;        // check sensor connection
00109      while(!Sensor.CheckConnection()); //check sensor connection
00110      two=0;        // sensor okay 
00111      BMP.Calibrate(); // Calibrations sequence for BMP... TODO: store values in LocalFileSystem
00112      Sensor.MPU_Setup(); // Setup MPU6050
00113      Sensor.CalibrateGyro(); // Calibrate Gyros  (averaging 100 readings)
00114      Sensor.CalibrateAcc(); //  Calibrate ACC    (averaging 100 readings)
00115      one=1;   // System okay to start 
00116      __enable_irq();    
00117      
00118      RtosTimer Fly(FlightControl,osTimerPeriodic,NULL); // Initialize RTOS Timer  
00119      Global.start(); // Start timer
00120      Fly.start(5);  // RTOS Timer (200Hz Control Loop)
00121  
00122  while(true){
00123            
00124           if(pc.readable()){
00125           c = pc.getc();
00126           if(c == 'a'||c=='A') MotorState = Armed;
00127             else if(c == 's'||c=='S') MotorState = UnArmed;
00128                 else if(c == 'u'||c=='U') Throttle+=50 ;
00129                     else if(c == 'i'||c=='I') Throttle-=50;
00130                         else if(c=='m'||c=='M') PitchSetpoint+=10;
00131                             else if(c=='n'||c=='N') PitchSetpoint-=10;
00132 
00133           //print=true;
00134           }
00135           
00136           #ifdef Debugging
00137           if(print){
00138           //pc.printf("%d\n",Temperature);
00139           pc.printf("%6.2f\n",pitch);
00140           //T_readingReady=false;
00141           print=false;
00142           }
00143           #endif
00144           
00145           Thread::wait(20);         
00146 }
00147 }