Diff: main.cpp

Revision:

0:9cb9445a11f0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jul 17 15:58:25 2013 +0000
@@ -0,0 +1,166 @@
+#include "mbed.h"
+#include "PwmIn.h"
+#include "Utils.h"
+#include "GurvIMU.h"
+#include "DaclePID.h"
+
+//#define DEBUG
+
+//Vue d'artiste
+/*  AXE X
+     3
+     ^
+     |
+4 -------> 2 // AXE Y
+     |
+     1
+*/
+
+
+//------------- DEFINITION DES PINs -----------------
+DigitalOut ledG(LED_GREEN);
+
+
+//-- TELECOMMANDE --
+PwmIn telecommande_throttle(PTD5); // throttle, channel 3
+
+//------ ESCs ------
+PwmOut esc1(PTD4);  //rouge
+PwmOut esc2(PTA12); //verte
+PwmOut esc3(PTA4);  //rouge
+PwmOut esc4(PTA5);  //verte
+
+//---- CAPTEURS ----
+GurvIMU imu;
+
+//------ PIDs ------
+Timer pidTimer;
+float dt_pid = 0.01;
+DaclePID pidX(0.5,0.0,0.01,dt_pid);
+DaclePID pidY(0.5,0.0,0.01,dt_pid);
+
+
+//---------- DEFINITION DES VARIABLES ---------------
+
+float yaw;
+float pitch;  //rotation autour de l'axe X/rouge
+float roll;   //rotation autour de l'axe Y/vert
+float ypr[3];// = {yaw,pitch,roll};
+
+float commande_pidX;
+float commande_pidY;
+
+int _period_us=18502; // periode de la PWM en microseconde
+
+float consigne_throttle = 0.0635; //PWM minimale
+
+
+//----------------- SERIAL DEGBUG -------------------
+#ifdef DEBUG
+    Serial debugSerial(USBTX, USBRX);
+#endif
+
+
+//--------------- DEBUT PROGRAMME -------------------
+int main() {
+    #ifdef DEBUG
+        debugSerial.baud(921600);
+        debugSerial.printf("Initialisation du programme\r\n");
+    #endif
+    
+    //-- CAPTEURS --
+    #ifdef DEBUG
+        debugSerial.printf("Initialisation capteurs\r\n");
+    #endif
+    imu.init();  
+    while((abs(ypr[2])<0.001) || (abs(ypr[1])<0.001) ){
+        imu.getYawPitchRollRad(ypr);
+        ledG=!ledG;
+        wait_ms(50);
+    } 
+    #ifdef DEBUG
+        debugSerial.printf("Fin initialisation capteurs\r\n");
+    #endif
+    
+    //--- ESCs ---
+    #ifdef DEBUG
+        debugSerial.printf("Initialisation des ESCs\r\n");
+    #endif
+    esc1.period_us(_period_us);
+    esc2.period_us(_period_us);
+    esc3.period_us(_period_us);
+    esc4.period_us(_period_us);
+    
+    esc1.write(0.0635); 
+    esc2.write(0.0635);
+    esc3.write(0.0635);
+    esc4.write(0.0635);
+    
+    wait(1);
+    #ifdef DEBUG
+        debugSerial.printf("Fin initialisation des ESCs\r\n");
+    #endif
+    
+    //---- PIDs ----
+    #ifdef DEBUG
+        debugSerial.printf("Initialisation des PIDs\r\n");
+    #endif
+    pidTimer.start();
+    //pidX.setInputLimits(-1.57,1.57);
+    //pidY.setInputLimits(-1.57,1.57);
+    pidX.setInputLimits(-0.60,0.60);
+    pidY.setInputLimits(-0.60,0.60);
+    //pidX.setOutputLimits(-1.0,1.0);
+    //pidY.setOutputLimits(-1.0,1.0);
+    
+    pidX.setOutputLimits(-0.30*0.0385,0.30*0.0385);
+    pidY.setOutputLimits(-0.30*0.0385,0.30*0.0385);
+
+    #ifdef DEBUG
+        debugSerial.printf("Fin initialisation des PIDs\r\n");
+    #endif
+    
+    
+    #ifdef DEBUG
+        debugSerial.printf("Fin initialisation du programme\r\n");
+        debugSerial.printf("Entrée dans la boucle principale\r\n");
+    #endif
+    while(1) {
+    //------ RECUPERATION DONNEES TELECOMMANDE ----------
+        #ifdef DEBUG
+            debugSerial.printf("Recuperation des donnees telecommande\r\n");
+        #endif
+        consigne_throttle =min( telecommande_throttle.dutycycle(),0.090);
+        #ifdef DEBUG
+            debugSerial.printf("Throttle: %f\r\n",consigne_throttle);
+            wait_ms(100);
+        #endif
+        
+    //------ RECUPERATION DONNEES CAPTEURS ----------
+        #ifdef DEBUG
+            debugSerial.printf("Recuperation des donnees capteurs\r\n");
+        #endif
+        imu.getYawPitchRollRad(ypr);
+        #ifdef DEBUG
+            debugSerial.printf("Yaw: %f;, Pitch: %f, Roll: %f\r\n",ypr[0],ypr[1],ypr[2]);
+            wait_ms(5);
+        #endif
+    //--------------- CALCUL PIDs -------------------       
+        while(pidTimer.read() < dt_pid);
+        pidTimer.reset();
+        commande_pidX=clip(-0.50*0.0385,0.50*0.0385,pidX.dacalcul(ypr[2])); //roll       
+        commande_pidY=clip(-0.50*0.0385,0.50*0.0385,pidY.dacalcul(ypr[1])); //pitch
+        #ifdef DEBUG
+            debugSerial.printf("Commande PID: %f\r\n",commande_pidX);
+            wait_ms(5);
+        #endif
+    
+    //--------------- COMMANDE ESCs -----------------
+        
+        esc1.write(clip(0.0635,0.102,consigne_throttle - commande_pidY));
+        esc2.write(clip(0.0635,0.102,consigne_throttle - commande_pidX));
+        esc3.write(clip(0.0635,0.102,consigne_throttle + commande_pidY));
+        esc4.write(clip(0.0635,0.102,consigne_throttle + commande_pidX));
+        
+    }
+}