Gurvan PRIEM
/
RaptorControl
An incomplete quadcopter control programme.
Diff: main.cpp
- Revision:
- 0:9cb9445a11f0
diff -r 000000000000 -r 9cb9445a11f0 main.cpp --- /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)); + + } +}