File content as of revision 0:9cb9445a11f0:

#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));
        
    }
}