main.cpp

00001 #include "mbed.h"
00002 #include "PwmIn.h"
00003 #include "Utils.h"
00004 #include "GurvIMU.h"
00005 #include "DaclePID.h"
00006 
00007 //#define DEBUG
00008 
00009 //Vue d'artiste
00010 /*  AXE X
00011      3
00012      ^
00013      |
00014 4 -------> 2 // AXE Y
00015      |
00016      1
00017 */
00018 
00019 
00020 //------------- DEFINITION DES PINs -----------------
00021 DigitalOut ledG(LED_GREEN);
00022 
00023 
00024 //-- TELECOMMANDE --
00025 PwmIn telecommande_throttle(PTD5); // throttle, channel 3
00026 
00027 //------ ESCs ------
00028 PwmOut esc1(PTD4);  //rouge
00029 PwmOut esc2(PTA12); //verte
00030 PwmOut esc3(PTA4);  //rouge
00031 PwmOut esc4(PTA5);  //verte
00032 
00033 //---- CAPTEURS ----
00034 GurvIMU imu;
00035 
00036 //------ PIDs ------
00037 Timer pidTimer;
00038 float dt_pid = 0.01;
00039 DaclePID pidX(0.5,0.0,0.01,dt_pid);
00040 DaclePID pidY(0.5,0.0,0.01,dt_pid);
00041 
00042 
00043 //---------- DEFINITION DES VARIABLES ---------------
00044 
00045 float yaw;
00046 float pitch;  //rotation autour de l'axe X/rouge
00047 float roll;   //rotation autour de l'axe Y/vert
00048 float ypr[3];// = {yaw,pitch,roll};
00049 
00050 float commande_pidX;
00051 float commande_pidY;
00052 
00053 int _period_us=18502; // periode de la PWM en microseconde
00054 
00055 float consigne_throttle = 0.0635; //PWM minimale
00056 
00057 
00058 //----------------- SERIAL DEGBUG -------------------
00059 #ifdef DEBUG
00060     Serial debugSerial(USBTX, USBRX);
00061 #endif
00062 
00063 
00064 //--------------- DEBUT PROGRAMME -------------------
00065 int main() {
00066     #ifdef DEBUG
00067         debugSerial.baud(921600);
00068         debugSerial.printf("Initialisation du programme\r\n");
00069     #endif
00070     
00071     //-- CAPTEURS --
00072     #ifdef DEBUG
00073         debugSerial.printf("Initialisation capteurs\r\n");
00074     #endif
00075     imu.init();  
00076     while((abs(ypr[2])<0.001) || (abs(ypr[1])<0.001) ){
00077         imu.getYawPitchRollRad(ypr);
00078         ledG=!ledG;
00079         wait_ms(50);
00080     } 
00081     #ifdef DEBUG
00082         debugSerial.printf("Fin initialisation capteurs\r\n");
00083     #endif
00084     
00085     //--- ESCs ---
00086     #ifdef DEBUG
00087         debugSerial.printf("Initialisation des ESCs\r\n");
00088     #endif
00089     esc1.period_us(_period_us);
00090     esc2.period_us(_period_us);
00091     esc3.period_us(_period_us);
00092     esc4.period_us(_period_us);
00093     
00094     esc1.write(0.0635); 
00095     esc2.write(0.0635);
00096     esc3.write(0.0635);
00097     esc4.write(0.0635);
00098     
00099     wait(1);
00100     #ifdef DEBUG
00101         debugSerial.printf("Fin initialisation des ESCs\r\n");
00102     #endif
00103     
00104     //---- PIDs ----
00105     #ifdef DEBUG
00106         debugSerial.printf("Initialisation des PIDs\r\n");
00107     #endif
00108     pidTimer.start();
00109     //pidX.setInputLimits(-1.57,1.57);
00110     //pidY.setInputLimits(-1.57,1.57);
00111     pidX.setInputLimits(-0.60,0.60);
00112     pidY.setInputLimits(-0.60,0.60);
00113     //pidX.setOutputLimits(-1.0,1.0);
00114     //pidY.setOutputLimits(-1.0,1.0);
00115     
00116     pidX.setOutputLimits(-0.30*0.0385,0.30*0.0385);
00117     pidY.setOutputLimits(-0.30*0.0385,0.30*0.0385);
00118 
00119     #ifdef DEBUG
00120         debugSerial.printf("Fin initialisation des PIDs\r\n");
00121     #endif
00122     
00123     
00124     #ifdef DEBUG
00125         debugSerial.printf("Fin initialisation du programme\r\n");
00126         debugSerial.printf("Entrée dans la boucle principale\r\n");
00127     #endif
00128     while(1) {
00129     //------ RECUPERATION DONNEES TELECOMMANDE ----------
00130         #ifdef DEBUG
00131             debugSerial.printf("Recuperation des donnees telecommande\r\n");
00132         #endif
00133         consigne_throttle =min( telecommande_throttle.dutycycle(),0.090);
00134         #ifdef DEBUG
00135             debugSerial.printf("Throttle: %f\r\n",consigne_throttle);
00136             wait_ms(100);
00137         #endif
00138         
00139     //------ RECUPERATION DONNEES CAPTEURS ----------
00140         #ifdef DEBUG
00141             debugSerial.printf("Recuperation des donnees capteurs\r\n");
00142         #endif
00143         imu.getYawPitchRollRad(ypr);
00144         #ifdef DEBUG
00145             debugSerial.printf("Yaw: %f;, Pitch: %f, Roll: %f\r\n",ypr[0],ypr[1],ypr[2]);
00146             wait_ms(5);
00147         #endif
00148     //--------------- CALCUL PIDs -------------------       
00149         while(pidTimer.read() < dt_pid);
00150         pidTimer.reset();
00151         commande_pidX=clip(-0.50*0.0385,0.50*0.0385,pidX.dacalcul(ypr[2])); //roll       
00152         commande_pidY=clip(-0.50*0.0385,0.50*0.0385,pidY.dacalcul(ypr[1])); //pitch
00153         #ifdef DEBUG
00154             debugSerial.printf("Commande PID: %f\r\n",commande_pidX);
00155             wait_ms(5);
00156         #endif
00157     
00158     //--------------- COMMANDE ESCs -----------------
00159         
00160         esc1.write(clip(0.0635,0.102,consigne_throttle - commande_pidY));
00161         esc2.write(clip(0.0635,0.102,consigne_throttle - commande_pidX));
00162         esc3.write(clip(0.0635,0.102,consigne_throttle + commande_pidY));
00163         esc4.write(clip(0.0635,0.102,consigne_throttle + commande_pidX));
00164         
00165     }
00166 }