main.cpp

00001 #include "stddef.h"
00002    
00003 const float dt = 0.005;
00004 
00005 Gyroscope gyroscope(new L3G4200D(p5, p6, p7, p8), 0.0175, dt);;
00006 Accelerometer accelerometer(new ADXL345(p5, p6, p7, p11), 256);;
00007 ComplementaryFilter ComplementaryFilterX(dt, 0.9995, 0.0005);;
00008 ComplementaryFilter ComplementaryFilterY(dt, 0.9995, 0.0005);;
00009 PID pidX(0.0007f, 0.008f, 0.0004f, dt);
00010 PID pidY(0.0007f, 0.008f, 0.0004f, dt);
00011 
00012 bool compute_pid        = false;
00013 bool motors_armed       = false;
00014 bool start_update_loop  = false;
00015 
00016 float accX          = 0.0;
00017 float accY          = 0.0;
00018 float gyroX         = 0.0;
00019 float gyroY         = 0.0;
00020 float setPoint_x    = 0.0;
00021 float setPoint_y    = 0.0;
00022 float control_x     = 0.0;
00023 float control_y     = 0.0;
00024 
00025 float ComplementarAngle_x = 0.0;
00026 float ComplementarAngle_y = 0.0;
00027 
00028 float heading = 0.0;
00029 float altura = 0.0;
00030 
00031 int state = 0;
00032 //float motorCoefA[5] = {1.4071, -2.7620, 1.9864, 0.3664, 0.0150};
00033 //float motorCoefB[5] = {1.4433, -2.8580, 2.0790, 0.3207, 0.0111};
00034 //float motorCoefC[5] = {1.4736, -2.8773, 2.1176, 0.2639, 0.0131};
00035 //float motorCoefD[5] = {1.5427, -2.9893, 2.1539, 0.2750, 0.0119};
00036 
00037 //Motor motorA(p26, 42, motorCoefA, 15, 60);
00038 //Motor motorC(p24, 42, motorCoefC, 15, 60);
00039 //Motor motorB(p25, 42, motorCoefB, 15, 60);
00040 //Motor motorD(p23, 42, motorCoefD, 15, 60);
00041 Motor motorA(p26, 42);
00042 Motor motorC(p24, 42);
00043 Motor motorB(p25, 42);
00044 Motor motorD(p23, 42);
00045 
00046 //Dummy
00047 DigitalOut dummy(p21);
00048 
00049 DigitalOut powerLed(LED1);
00050 DigitalOut initLed(LED2);
00051 DigitalOut statusLed1(LED3);
00052 DigitalOut statusLed2(LED4);
00053 
00054 Ticker updater;
00055 
00056 volatile unsigned int sysCount;
00057 volatile unsigned int pwmCount;
00058 volatile unsigned int mainCount;
00059 volatile unsigned int loop_time;
00060 
00061 //SerialBuffered pc(USBTX, USBRX);
00062 SerialBuffered pc(p28, p27);
00063 char buffer[64];
00064 char cmd = CMD_WAITING;
00065 /*
00066  * Sets the Motors
00067  */
00068 void motors_setPower()
00069 {
00070 
00071     motorB.setPowerLin( FLIGHT_THRESHOLD - (control_x*100) + heading + altura );
00072     motorD.setPowerLin( FLIGHT_THRESHOLD + (control_x*100) + heading + altura );
00073     
00074     motorA.setPowerLin( FLIGHT_THRESHOLD - (control_y*100) - heading + altura );
00075     motorC.setPowerLin( FLIGHT_THRESHOLD + (control_y*100) - heading + altura );
00076 
00077 }
00078 
00079 /*
00080  * Update Loop
00081  * Periodically Occurs every 5 milliseconds
00082  */
00083 void update_lood()
00084 {
00085     gyroscope.update();
00086     accelerometer.update();
00087 
00088     accX  =        accelerometer.getDegreesAngleX();
00089     accY  =        accelerometer.getDegreesAngleY();
00090     gyroX = (-1) * gyroscope.getDegreesX();
00091     gyroY =        gyroscope.getDegreesY();
00092     
00093     //gyroAngle = gyroscope->getAngleY();
00094     
00095     ComplementarAngle_x = ComplementaryFilterX.update(accY, gyroX);
00096     ComplementarAngle_y = ComplementaryFilterY.update(accX, gyroY);
00097 
00098 
00099     if(compute_pid) 
00100     {
00101         control_x = pidX.compute(setPoint_x, ComplementarAngle_x);
00102         control_y = pidY.compute(setPoint_y, ComplementarAngle_y);
00103     }
00104 
00105     motors_setPower();
00106 }
00107 
00108 /*
00109  * PWM Loop
00110  * Periodically Occurs every 1 millisecond
00111  */
00112 void pwm_lood()
00113 {
00114     if(pwmCount == 2 && !motors_armed) 
00115     {
00116         dummy = 0;
00117     } 
00118     else if(pwmCount == 4 && motors_armed) 
00119     {
00120         dummy = 0;
00121     } 
00122     else if(pwmCount == 40 )//tempo igual a '20' e '0' (termina os 20 mili e inicia os próximos 20 mili) 
00123     { 
00124         dummy = 1;
00125         pwmCount = 0;
00126     }
00127 }
00128 
00129 /*
00130  * Main Loop
00131  * Handle the update loop and the pwm loop
00132  */
00133 void update()
00134 {
00135     sysCount+=1;
00136     pwmCount+=1;
00137     if(sysCount >= 10 && start_update_loop) 
00138     {
00139         update_lood();
00140         sysCount = 0;
00141     }
00142 
00143     pwm_lood();
00144 }
00145 
00146 bool setPointAdjustment(int currentLoopTime)
00147 {   
00148     bool reset = false;
00149     /* 2 graus por segundo
00150        2 - 1
00151     taxa - 0.010 -> loop_time     */
00152     
00153     float taxa = 0.04; 
00154     
00155     switch(state) /* Máquina de estados */
00156     {   
00157         case 0:         
00158         case 6:
00159         case 7: 
00160         case 8:
00161         case 14: 
00162         case 15:  
00163             if(setPoint_x < 0)
00164                 setPoint_x += taxa;
00165             else if(setPoint_x > 0)
00166                 setPoint_x -= taxa; 
00167                  
00168             if(setPoint_y < 0)
00169                 setPoint_y += taxa;
00170             else if(setPoint_y > 0)
00171                 setPoint_y -= taxa; 
00172         break;
00173         case 1:
00174         case 2:
00175             if(setPoint_x < 20)
00176                 setPoint_x += taxa;
00177             else if(setPoint_x > 20)
00178                 setPoint_x -= taxa;            
00179         break;
00180         case 3:
00181         case 4:
00182         case 5:
00183             if(setPoint_x < -20)
00184                 setPoint_x += taxa;
00185             else if(setPoint_x > -20)
00186                 setPoint_x -= taxa;
00187         break;
00188         case 9:
00189         case 10:
00190             if(setPoint_y < 20)
00191                 setPoint_y += taxa;
00192             else if(setPoint_y > 20)
00193                 setPoint_y -= taxa;            
00194         break;
00195         case 11:
00196         case 12:
00197         case 13:
00198             if(setPoint_y < -20)
00199                 setPoint_y += taxa;
00200             else if(setPoint_y > -20)
00201                 setPoint_y -= taxa;    
00202         break;
00203         
00204     }
00205     
00206     
00207     if(currentLoopTime >= 5000)
00208     {
00209         state++;
00210         if(state > 15) 
00211             state = 0;
00212         reset = true;
00213     }
00214     return reset;
00215 }
00216 
00217 /*
00218  * Main
00219  */
00220 int main()
00221 {
00222     pc.baud(PC_BAUD_RATE);
00223     
00224     //Leds de controle
00225     powerLed = 0;
00226     initLed  = 0;
00227     statusLed1 = 0;
00228     statusLed2 = 0;
00229     
00230     //Contadores
00231     sysCount = 0;
00232     pwmCount = 0;
00233     mainCount = 0;
00234     
00235     //Variáveis lógicas
00236     start_update_loop = false;
00237     motors_armed = false;
00238     /*
00239     gyroscope = new Gyroscope(new L3G4200D(p5, p6, p7, p8), 0.0175, dt);
00240     accelerometer = new Accelerometer(new ADXL345(p5, p6, p7, p11), 256);
00241     ComplementaryFilterX = new ComplementaryFilter(dt, 0.9995, 0.0005);
00242     ComplementaryFilterY = new ComplementaryFilter(dt, 0.9995, 0.0005);
00243     */
00244     powerLed = 1;
00245 
00246     wait(10);
00247     accelerometer.updateZeroRates();
00248     gyroscope.updateZeroRates();
00249 
00250     initLed  = 1;
00251 
00252     motors_setPower();
00253     updater.attach(&update, 0.0005);
00254     wait(2);
00255     motors_armed = true;
00256     wait(7);
00257 
00258     //msg_update = true;
00259     compute_pid = true;
00260     start_update_loop = true;
00261     loop_time = 10;
00262     int i = 0;
00263     int j = 0;
00264     while (true) 
00265     {
00266         wait_ms(loop_time);
00267         j += loop_time;
00268         if(setPointAdjustment(j))
00269             j = 0;
00270         
00271         /*
00272         if (pc.readable())
00273             cmd = pc.getc();
00274 
00275         switch (cmd) 
00276         {
00277             case CMD_UP:
00278             heading+= 0.5;
00279             sprintf((char*)buffer, "heading %f\r\n", heading);    
00280             pc.writeText(buffer);
00281             break;
00282             
00283             case CMD_DOWN:
00284             heading-= 0.5;
00285             sprintf((char*)buffer, "heading %f\r\n", heading);    
00286             pc.writeText(buffer);
00287             break;
00288             
00289             case CMD_UP2:
00290             altura += 0.5;
00291             sprintf((char*)buffer, "altura %f\r\n", altura);    
00292             pc.writeText(buffer);
00293             break;
00294             
00295             case CMD_DOWN2:
00296             altura -= 0.5;
00297             sprintf((char*)buffer, "altura %f\r\n", altura);    
00298             pc.writeText(buffer);
00299             break; 
00300             
00301         }
00302         
00303         cmd = CMD_WAITING;
00304         */
00305         sprintf((char*)buffer, "%f %f %f %f \n", ComplementarAngle_x, setPoint_x, ComplementarAngle_y, setPoint_y);    
00306         pc.writeText(buffer);
00307         
00308         i += loop_time;
00309         if(i >= 200)
00310         {   statusLed2 = statusLed1;
00311             statusLed1 = !statusLed1;
00312             i = 0;
00313         }
00314     }
00315 }