Gurvan PRIEM / Mbed 2 deprecated RaptorControl

An incomplete quadcopter control programme.

Dependencies:   mbed

main.cpp@0:9cb9445a11f0, 2013-07-17 (annotated)

Committer:
Gurvan
Date:
Wed Jul 17 15:58:25 2013 +0000
Revision:
0:9cb9445a11f0
Pour Zobson, fi(r)st commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gurvan 0:9cb9445a11f0 1 #include "mbed.h"
Gurvan 0:9cb9445a11f0 2 #include "PwmIn.h"
Gurvan 0:9cb9445a11f0 3 #include "Utils.h"
Gurvan 0:9cb9445a11f0 4 #include "GurvIMU.h"
Gurvan 0:9cb9445a11f0 5 #include "DaclePID.h"
Gurvan 0:9cb9445a11f0 6
Gurvan 0:9cb9445a11f0 7 //#define DEBUG
Gurvan 0:9cb9445a11f0 8
Gurvan 0:9cb9445a11f0 9 //Vue d'artiste
Gurvan 0:9cb9445a11f0 10 /* AXE X
Gurvan 0:9cb9445a11f0 11 3
Gurvan 0:9cb9445a11f0 12 ^
Gurvan 0:9cb9445a11f0 13 |
Gurvan 0:9cb9445a11f0 14 4 -------> 2 // AXE Y
Gurvan 0:9cb9445a11f0 15 |
Gurvan 0:9cb9445a11f0 16 1
Gurvan 0:9cb9445a11f0 17 */
Gurvan 0:9cb9445a11f0 18
Gurvan 0:9cb9445a11f0 19
Gurvan 0:9cb9445a11f0 20 //------------- DEFINITION DES PINs -----------------
Gurvan 0:9cb9445a11f0 21 DigitalOut ledG(LED_GREEN);
Gurvan 0:9cb9445a11f0 22
Gurvan 0:9cb9445a11f0 23
Gurvan 0:9cb9445a11f0 24 //-- TELECOMMANDE --
Gurvan 0:9cb9445a11f0 25 PwmIn telecommande_throttle(PTD5); // throttle, channel 3
Gurvan 0:9cb9445a11f0 26
Gurvan 0:9cb9445a11f0 27 //------ ESCs ------
Gurvan 0:9cb9445a11f0 28 PwmOut esc1(PTD4); //rouge
Gurvan 0:9cb9445a11f0 29 PwmOut esc2(PTA12); //verte
Gurvan 0:9cb9445a11f0 30 PwmOut esc3(PTA4); //rouge
Gurvan 0:9cb9445a11f0 31 PwmOut esc4(PTA5); //verte
Gurvan 0:9cb9445a11f0 32
Gurvan 0:9cb9445a11f0 33 //---- CAPTEURS ----
Gurvan 0:9cb9445a11f0 34 GurvIMU imu;
Gurvan 0:9cb9445a11f0 35
Gurvan 0:9cb9445a11f0 36 //------ PIDs ------
Gurvan 0:9cb9445a11f0 37 Timer pidTimer;
Gurvan 0:9cb9445a11f0 38 float dt_pid = 0.01;
Gurvan 0:9cb9445a11f0 39 DaclePID pidX(0.5,0.0,0.01,dt_pid);
Gurvan 0:9cb9445a11f0 40 DaclePID pidY(0.5,0.0,0.01,dt_pid);
Gurvan 0:9cb9445a11f0 41
Gurvan 0:9cb9445a11f0 42
Gurvan 0:9cb9445a11f0 43 //---------- DEFINITION DES VARIABLES ---------------
Gurvan 0:9cb9445a11f0 44
Gurvan 0:9cb9445a11f0 45 float yaw;
Gurvan 0:9cb9445a11f0 46 float pitch; //rotation autour de l'axe X/rouge
Gurvan 0:9cb9445a11f0 47 float roll; //rotation autour de l'axe Y/vert
Gurvan 0:9cb9445a11f0 48 float ypr[3];// = {yaw,pitch,roll};
Gurvan 0:9cb9445a11f0 49
Gurvan 0:9cb9445a11f0 50 float commande_pidX;
Gurvan 0:9cb9445a11f0 51 float commande_pidY;
Gurvan 0:9cb9445a11f0 52
Gurvan 0:9cb9445a11f0 53 int _period_us=18502; // periode de la PWM en microseconde
Gurvan 0:9cb9445a11f0 54
Gurvan 0:9cb9445a11f0 55 float consigne_throttle = 0.0635; //PWM minimale
Gurvan 0:9cb9445a11f0 56
Gurvan 0:9cb9445a11f0 57
Gurvan 0:9cb9445a11f0 58 //----------------- SERIAL DEGBUG -------------------
Gurvan 0:9cb9445a11f0 59 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 60 Serial debugSerial(USBTX, USBRX);
Gurvan 0:9cb9445a11f0 61 #endif
Gurvan 0:9cb9445a11f0 62
Gurvan 0:9cb9445a11f0 63
Gurvan 0:9cb9445a11f0 64 //--------------- DEBUT PROGRAMME -------------------
Gurvan 0:9cb9445a11f0 65 int main() {
Gurvan 0:9cb9445a11f0 66 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 67 debugSerial.baud(921600);
Gurvan 0:9cb9445a11f0 68 debugSerial.printf("Initialisation du programme\r\n");
Gurvan 0:9cb9445a11f0 69 #endif
Gurvan 0:9cb9445a11f0 70
Gurvan 0:9cb9445a11f0 71 //-- CAPTEURS --
Gurvan 0:9cb9445a11f0 72 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 73 debugSerial.printf("Initialisation capteurs\r\n");
Gurvan 0:9cb9445a11f0 74 #endif
Gurvan 0:9cb9445a11f0 75 imu.init();
Gurvan 0:9cb9445a11f0 76 while((abs(ypr[2])<0.001) || (abs(ypr[1])<0.001) ){
Gurvan 0:9cb9445a11f0 77 imu.getYawPitchRollRad(ypr);
Gurvan 0:9cb9445a11f0 78 ledG=!ledG;
Gurvan 0:9cb9445a11f0 79 wait_ms(50);
Gurvan 0:9cb9445a11f0 80 }
Gurvan 0:9cb9445a11f0 81 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 82 debugSerial.printf("Fin initialisation capteurs\r\n");
Gurvan 0:9cb9445a11f0 83 #endif
Gurvan 0:9cb9445a11f0 84
Gurvan 0:9cb9445a11f0 85 //--- ESCs ---
Gurvan 0:9cb9445a11f0 86 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 87 debugSerial.printf("Initialisation des ESCs\r\n");
Gurvan 0:9cb9445a11f0 88 #endif
Gurvan 0:9cb9445a11f0 89 esc1.period_us(_period_us);
Gurvan 0:9cb9445a11f0 90 esc2.period_us(_period_us);
Gurvan 0:9cb9445a11f0 91 esc3.period_us(_period_us);
Gurvan 0:9cb9445a11f0 92 esc4.period_us(_period_us);
Gurvan 0:9cb9445a11f0 93
Gurvan 0:9cb9445a11f0 94 esc1.write(0.0635);
Gurvan 0:9cb9445a11f0 95 esc2.write(0.0635);
Gurvan 0:9cb9445a11f0 96 esc3.write(0.0635);
Gurvan 0:9cb9445a11f0 97 esc4.write(0.0635);
Gurvan 0:9cb9445a11f0 98
Gurvan 0:9cb9445a11f0 99 wait(1);
Gurvan 0:9cb9445a11f0 100 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 101 debugSerial.printf("Fin initialisation des ESCs\r\n");
Gurvan 0:9cb9445a11f0 102 #endif
Gurvan 0:9cb9445a11f0 103
Gurvan 0:9cb9445a11f0 104 //---- PIDs ----
Gurvan 0:9cb9445a11f0 105 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 106 debugSerial.printf("Initialisation des PIDs\r\n");
Gurvan 0:9cb9445a11f0 107 #endif
Gurvan 0:9cb9445a11f0 108 pidTimer.start();
Gurvan 0:9cb9445a11f0 109 //pidX.setInputLimits(-1.57,1.57);
Gurvan 0:9cb9445a11f0 110 //pidY.setInputLimits(-1.57,1.57);
Gurvan 0:9cb9445a11f0 111 pidX.setInputLimits(-0.60,0.60);
Gurvan 0:9cb9445a11f0 112 pidY.setInputLimits(-0.60,0.60);
Gurvan 0:9cb9445a11f0 113 //pidX.setOutputLimits(-1.0,1.0);
Gurvan 0:9cb9445a11f0 114 //pidY.setOutputLimits(-1.0,1.0);
Gurvan 0:9cb9445a11f0 115
Gurvan 0:9cb9445a11f0 116 pidX.setOutputLimits(-0.30*0.0385,0.30*0.0385);
Gurvan 0:9cb9445a11f0 117 pidY.setOutputLimits(-0.30*0.0385,0.30*0.0385);
Gurvan 0:9cb9445a11f0 118
Gurvan 0:9cb9445a11f0 119 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 120 debugSerial.printf("Fin initialisation des PIDs\r\n");
Gurvan 0:9cb9445a11f0 121 #endif
Gurvan 0:9cb9445a11f0 122
Gurvan 0:9cb9445a11f0 123
Gurvan 0:9cb9445a11f0 124 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 125 debugSerial.printf("Fin initialisation du programme\r\n");
Gurvan 0:9cb9445a11f0 126 debugSerial.printf("Entrée dans la boucle principale\r\n");
Gurvan 0:9cb9445a11f0 127 #endif
Gurvan 0:9cb9445a11f0 128 while(1) {
Gurvan 0:9cb9445a11f0 129 //------ RECUPERATION DONNEES TELECOMMANDE ----------
Gurvan 0:9cb9445a11f0 130 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 131 debugSerial.printf("Recuperation des donnees telecommande\r\n");
Gurvan 0:9cb9445a11f0 132 #endif
Gurvan 0:9cb9445a11f0 133 consigne_throttle =min( telecommande_throttle.dutycycle(),0.090);
Gurvan 0:9cb9445a11f0 134 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 135 debugSerial.printf("Throttle: %f\r\n",consigne_throttle);
Gurvan 0:9cb9445a11f0 136 wait_ms(100);
Gurvan 0:9cb9445a11f0 137 #endif
Gurvan 0:9cb9445a11f0 138
Gurvan 0:9cb9445a11f0 139 //------ RECUPERATION DONNEES CAPTEURS ----------
Gurvan 0:9cb9445a11f0 140 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 141 debugSerial.printf("Recuperation des donnees capteurs\r\n");
Gurvan 0:9cb9445a11f0 142 #endif
Gurvan 0:9cb9445a11f0 143 imu.getYawPitchRollRad(ypr);
Gurvan 0:9cb9445a11f0 144 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 145 debugSerial.printf("Yaw: %f;, Pitch: %f, Roll: %f\r\n",ypr[0],ypr[1],ypr[2]);
Gurvan 0:9cb9445a11f0 146 wait_ms(5);
Gurvan 0:9cb9445a11f0 147 #endif
Gurvan 0:9cb9445a11f0 148 //--------------- CALCUL PIDs -------------------
Gurvan 0:9cb9445a11f0 149 while(pidTimer.read() < dt_pid);
Gurvan 0:9cb9445a11f0 150 pidTimer.reset();
Gurvan 0:9cb9445a11f0 151 commande_pidX=clip(-0.50*0.0385,0.50*0.0385,pidX.dacalcul(ypr[2])); //roll
Gurvan 0:9cb9445a11f0 152 commande_pidY=clip(-0.50*0.0385,0.50*0.0385,pidY.dacalcul(ypr[1])); //pitch
Gurvan 0:9cb9445a11f0 153 #ifdef DEBUG
Gurvan 0:9cb9445a11f0 154 debugSerial.printf("Commande PID: %f\r\n",commande_pidX);
Gurvan 0:9cb9445a11f0 155 wait_ms(5);
Gurvan 0:9cb9445a11f0 156 #endif
Gurvan 0:9cb9445a11f0 157
Gurvan 0:9cb9445a11f0 158 //--------------- COMMANDE ESCs -----------------
Gurvan 0:9cb9445a11f0 159
Gurvan 0:9cb9445a11f0 160 esc1.write(clip(0.0635,0.102,consigne_throttle - commande_pidY));
Gurvan 0:9cb9445a11f0 161 esc2.write(clip(0.0635,0.102,consigne_throttle - commande_pidX));
Gurvan 0:9cb9445a11f0 162 esc3.write(clip(0.0635,0.102,consigne_throttle + commande_pidY));
Gurvan 0:9cb9445a11f0 163 esc4.write(clip(0.0635,0.102,consigne_throttle + commande_pidX));
Gurvan 0:9cb9445a11f0 164
Gurvan 0:9cb9445a11f0 165 }
Gurvan 0:9cb9445a11f0 166 }