main.cpp

00001 /*PROYECTO DEL PENDULO INVERTIDO
00002  * TARJETA= NUCLEO STM32F446
00003    ENCODER EJE X PB_8 CH1  PB_9 CH2
00004    ENCODER PENDULO PB_4 PB_5
00005    
00006    FIN DE CARRERA MOTOR IZQ = A1 
00007    FIN DERECHA DER  = A0
00008    
00009    POLOLULU
00010    
00011    nD2 = D6
00012    M1DIR=D7
00013    M1PWM= D11 (TIMER 1)
00014    NsF=D12
00015    GND=D13
00016    VCC=VCC
00017  */   
00018 
00019 
00020 
00021 /*------------- THANKS DAVID LOWE FOR YOUR HELP-Your API/HAL use full ---------------------------------------
00022  * Using STM32's counter peripherals to interface rotary encoders.
00023  * Encoders are supported on F4xx's TIM1,2,3,4,5. TIM2 & TIM5 have 32bit count, others 16bit.
00024  * Beware mbed uses TIM5 for system timer, SPI needs TIM1, others used for PWM.
00025  * Check your platform's PeripheralPins.c & PeripheralNames.h if you need both PWM & encoders.
00026  *
00027  * Edit HAL_TIM_Encoder_MspInitFx.cpp to suit your mcu & board's available pinouts & pullups/downs.
00028  *
00029  * Thanks to:
00030  * http://petoknm.wordpress.com/2015/01/05/rotary-encoder-and-stm32/
00031  *
00032  * References:
00033  * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/user_manual/DM00122015.pdf
00034  * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/reference_manual/DM00096844.pdf
00035  * http://www.st.com/web/en/resource/technical/document/application_note/DM00042534.pdf
00036  * http://www.st.com/web/en/resource/technical/document/datasheet/DM00102166.pdf
00037  * 
00038  * David Lowe Jan 2015
00039  *
00040  
00041  *Se complementa con el Timer 8 en la tarjeta nucleo
00042  *Efectivamente el Timer 5 no se debe usar.
00043  *Se Ajustaron los pines para la tarjeta para usar otros para los PWM
00044   
00045  */
00046 
00047 #include "mbed.h"
00048 #include "Encoder.h"
00049 
00050 
00051 /****************************ENCODER INICIALIZACION *************************/
00052 //STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h
00053 #ifdef TARGET_STM32F3
00054 #define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT)
00055 #define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__)            (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR))
00056 #endif
00057 
00058 TIM_Encoder_InitTypeDef encoder2, encoder3, encoder4,encoder5, encoder8;
00059 TIM_HandleTypeDef  timer2,  timer3;
00060 
00061 /*************************************************************************/
00062 
00063 /*CONFIGURAMOS EL SHIELD DEL POLOLULU
00064  Al coincidir con los pines del timer se modifica el codigo
00065 */
00066 
00067 DigitalOut M1DIR(D7);
00068 PwmOut M1PWM(PA_7);
00069 //AnalogIn   M1FB(A0);
00070 
00071 //DigitalOut M2DIR(D8);
00072 //PwmOut M2PWM(D10);
00073 //DigitalIn  M2FB(A1);
00074 
00075 DigitalOut nD2(D6,1);
00076 DigitalIn nSF(D12);
00077 
00078 
00079 
00080 
00081 /***********PUERTO SERIAL*****************/
00082 
00083 Serial pc(SERIAL_TX, SERIAL_RX);
00084 
00085 
00086 /*VALORES POR DEFECTO*/
00087 
00088 float VEL=0.0;
00089 
00090 /*VALORES DE CONFIGURACION DE LOS PINES*/
00091 
00092 InterruptIn FINIZQR(A1);
00093 InterruptIn FINDERM(A0);
00094 //DigitalIn FINIZQR(D5);
00095 //DigitalIn FINDERM(D6);
00096 
00097 int alarma=0;
00098 DigitalOut INDICA(LED2);
00099 DigitalIn  INIT(PC_13);
00100 
00101 
00102 void toggle1(void); 
00103 void toggle2(void); 
00104 void inicializa(void);
00105 void centrar(void);
00106 
00107 /*-------------VARIABLES GLOBALES----------------------*/
00108        
00109     float X=0;
00110     float POS=0;
00111     float A=0;
00112     float ANG=0;
00113     float LIMIN=300;//SI no inicializa toca manual 220 
00114     float LIMAX=7000;  //Si no inciializa toca 
00115     
00116     
00117     float XMIN=0; //en las pruebas realizadas el 0 +200 pulsos
00118     float XMAX=7200;; //en las pruebas aprox 7949
00119     float MITAD=0;
00120     int DATAPC=0;
00121     int LEIDO=0;
00122     
00123     
00124     
00125 /*------------------VARIABLES DEL PID Discreto--------------------------*/
00126 
00127 
00128 float uc0,yn0,en0,cn0,uc1,yn1,cn1,en1,yn2,cn2,en2,yn3,cn3,en3;
00129     
00130 
00131 //Constantes
00132 
00133 float q0,q1,q2;
00134 
00135 /*-----------------PID DE PENDULO COMPLETO-------------------------------*/
00136 float PID_CODIGO(float MEDIDA,float SETPOINT,float DT);
00137 
00138 float KI;
00139 float KP;
00140 float KD; 
00141 float ERR;
00142 float PREVI_ERR;
00143 float INTEGRAL;
00144 float DERIVATIVO;
00145 
00146 float DESEADO=180;
00147 
00148 float TM=0.01;  // tiempo de muestreo
00149 
00150 
00151 float numero, cal_pwm;
00152 
00153 
00154 
00155 /*-------------------------------------------------PROGRAMA---------------------------------------------------------------*/
00156 
00157 int main()
00158 {
00159  float SALIDAPID;
00160  
00161  
00162  
00163  //inicializacion de las varibles   
00164     yn0=0;
00165     cn0=0;
00166     en0=0;
00167     yn1=0;
00168     cn1=0;
00169     en1=0;
00170     yn2=0;
00171     cn2=0;
00172     en2=0;
00173     yn3=0;
00174     cn3=0;
00175     en3=0;
00176 
00177     q0=0.0510;
00178     q1=-0.00102;
00179     q2=0;
00180 
00181     INTEGRAL=0.0;
00182      KI=0.5;
00183      KP=13;
00184      KD=0.25; 
00185      SALIDAPID=0.0;
00186       
00187     
00188     
00189     
00190     //Configuramos el puerto de 96000 a 115200 para mayor velocidad
00191     pc.baud(115200);    
00192 
00193 
00194     //counting on both A&B inputs, 4 ticks per cycle, full 32-bit count
00195     EncoderInit(&encoder2, &timer2, TIM2, 0xffffffff, TIM_ENCODERMODE_TI12);
00196  
00197     //counting on both A&B inputs, 4 ticks per cycle, full 32-bit count
00198     EncoderInit(&encoder3, &timer3, TIM3, 0xffff, TIM_ENCODERMODE_TI12);
00199  
00200         int16_t count3=0;
00201         int32_t count2=0;
00202         //int8_t dir2, dir3;
00203 
00204     M1PWM.period_ms(10.0); // set PWM period to 10 ms
00205     
00206      pc.printf("Listo iniciamos centrando el carro !\n");
00207      wait(0.1);
00208     
00209     
00210      inicializa();
00211      centrar();
00212      alarma=1;
00213      
00214      
00215 
00216      pc.printf("Centre el pendulo y Presione reset para continuar \n\r");
00217      //pc.printf("Press 'a' = aumentar, 'd' = disminuir   'i'=Izquierda 'o'=derecha 'r'=reset contadores 's'=para solo el PWM   \n\r");
00218      char c ='s';
00219      if (INIT==0){
00220                 
00221             yn0=0;
00222             cn0=0;
00223             en0=0;
00224             yn1=0;
00225             cn1=0;
00226             en1=0;
00227             yn2=0;
00228             cn2=0;
00229             en2=0;
00230             yn3=0;
00231             cn3=0;
00232             en3=0;
00233             alarma=0;
00234             VEL=0;
00235             numero=0;
00236             wait(0.5);               
00237                 }  
00238        
00239     
00240     //Ciclo principal
00241     while(1) {
00242    
00243 
00244      //----Deje habilitado la lectura del teclado para pruebas
00245         if(pc.readable()==1) {
00246                wait(0.1);
00247                c = pc.getc();
00248                pc.putc(c);
00249         
00250                
00251                     if((c == 'a') && (VEL < 0.99)) {
00252                              VEL += 0.01; 
00253                              M1PWM = VEL; 
00254                     }
00255                     if((c == 'd') && (VEL > 0.01)) { 
00256                         VEL -= 0.01; 
00257                         M1PWM = VEL; 
00258                     }
00259                     if (c == 'i') {
00260                     M1DIR=1; 
00261                     }    
00262                     if (c == 'o') {
00263                     M1DIR=0; 
00264                     }    
00265                     if (c == 'r') {
00266                     __HAL_TIM_SetCounter(&timer3, 0x0000);
00267                     __HAL_TIM_SetCounter(&timer2, 0x00000000);
00268                     }
00269                     if (c == 's') {
00270                     M1PWM = 0.0;
00271                     }
00272                                        
00273                     c='0';       
00274          }   
00275         //----fin rutina de teclado
00276         
00277          INDICA=alarma;
00278         
00279           FINIZQR.rise(&toggle1);
00280           FINDERM.rise(&toggle2);
00281         
00282         
00283         //Rutina de lectura de los encoder timer2=X y Timer3 = Angulo
00284         //X es de 100 huecos por revolucion = X4  400 pulsos por revolucion
00285         //X es de 500 huecos por revolucuon = X4  2000 pulsos por revolucion
00286         // X aproximado de 7950 +/10% por temas mecanicos de hay que sea variable el centro
00287         // MITAD = 3757 depende de la ubicacion de los sensores ((MAX-220)-(MIN+220))/2
00288         
00289         //count2=TIM2->CNT;
00290         //dir2=TIM2->CR1&TIM_CR1_DIR;
00291         count2=__HAL_TIM_GET_COUNTER(&timer2);
00292         //dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); //no se necesita para el calculo
00293 
00294         //count3=TIM3->CNT;
00295         //dir3=TIM3->CR1&TIM_CR1_DIR;
00296         count3=__HAL_TIM_GET_COUNTER(&timer3);
00297         //dir3 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer3); //no se necesita para el calculo
00298         
00299         
00300         
00301 
00302        X=count2;//sensor de posicion 
00303        A=count3;// sensor de angulo
00304        
00305        pc.printf("%6.0f %6.0f \t", X,A);
00306         
00307        if (INIT==0){
00308                 
00309             yn0=0;
00310             cn0=0;
00311             en0=0;
00312             yn1=0;
00313             cn1=0;
00314             en1=0;
00315             yn2=0;
00316             cn2=0;
00317             en2=0;
00318             yn3=0;
00319             cn3=0;
00320             en3=0;
00321             alarma=0;
00322             VEL=0;
00323             numero=0;
00324             wait(0.5);               
00325                 }
00326                       
00327     
00328      //factor de conversion por longitud/pulsos (720mm/7949 =0.090577 mm* pulso  LTOTAL/PULSOS
00329        POS=X*720/7949;
00330        
00331        
00332        //factor de conversion grados vuelta/pulsos (360grados/2000 pulsos)=0.18   grados por pulso
00333        ANG=A*0.18;
00334        
00335        pc.printf("POS: %6.3f  ANGULO:%6.3f \t", POS,ANG);
00336     
00337       
00338         if(alarma==0){
00339      
00340         SALIDAPID=PID_CODIGO(ANG, DESEADO, TM);
00341         
00342         VEL=SALIDAPID;
00343         
00344                 
00345         //MAXIMO DE LA TARJETA   
00346             
00347             if (VEL>100) {        //Salida PID si es mayor que el MAX 
00348                  VEL=99;
00349             }                          
00350             if (VEL<-100) {        //Salida PID si es mayor que el MAX 
00351                  VEL=-99;
00352             }                          
00353             
00354             if((VEL<=100)&&(VEL>=0)){
00355                        VEL=VEL*1;                //Error es positivo entonces gire a la izquierda lado opuesto del motor
00356                         M1DIR=0;
00357                        } 
00358             
00359             if((VEL<0)&&(VEL>-100)){
00360                        VEL=VEL*-1;                //Error es negativo entonces gire a la derecha lado opuesto del motor
00361                         M1DIR=1;
00362                        } 
00363               
00364                        
00365             if (abs(VEL)<10){      //Salida PID si es menor que el MIN 
00366                  VEL=1;                         
00367              }
00368                  
00369         M1PWM=(float)VEL/100;//para controlar con las teclas por silas moscas 
00370     
00371         
00372         }
00373         pc.printf("PID = %f \t", SALIDAPID);    
00374         pc.printf("REF:%6.2f ANG:%6.2f ERROR: %6.2f ACC:%6.2f  Itera: %.0f\t",DESEADO, ANG, ERR,VEL,numero);  
00375         
00376          
00377         
00378         
00379         //pc.printf("El PWM %1.3f \t ",VEL); 
00380         pc.printf("Alama: %i \n \r",alarma); 
00381         
00382         
00383         
00384         
00385         
00386         /*************************************************************************************************/
00387         numero=numero+1;
00388   
00389   
00390         /****seguridad***************/
00391         if ((X+300)>=LIMAX){
00392           VEL=0;
00393           M1PWM=0;
00394           alarma=1;
00395         }
00396         if ((X-300)<=LIMIN){
00397           VEL=0.0;
00398           M1PWM=0.0;
00399           alarma=1;
00400         }
00401         
00402         
00403         
00404         
00405         
00406     }
00407 }
00408 
00409 /*---------------------FUNCIONES-----------------------------------------------*/
00410  
00411  /*---------------------INTERUPPCIONES------------------------------------------*/
00412  
00413     void toggle1() {
00414            wait(0.25); 
00415            VEL = 0.0;
00416            M1PWM = 0.0;
00417            alarma=1;
00418            pc.printf("ALARMA FINAL IZQUIERDA\n\r");
00419     }
00420     
00421       void toggle2() {
00422            wait(0.25); 
00423            VEL = 0.0;
00424            alarma=1;
00425            M1PWM = 0.0;
00426            pc.printf("ALARMA FINAL DERECHA\n\r");
00427     }   
00428     
00429 /*--------------------------------FUCIONES DE INICIALIZACION------------------------------*/
00430 
00431 
00432 void inicializa(){
00433         pc.printf("Funcion de Inicializacion iniciada\n\r");
00434            
00435      M1PWM=0.1;
00436      M1DIR=1;
00437      while(FINDERM==0){
00438          pc.printf("BUSCANCO DERECHA DEL MOTOR\n\r");
00439           }
00440          pc.printf("ENCONTRADA - REINICIANDO EL CONTADOR EN X\n\r");
00441             __HAL_TIM_SetCounter(&timer2, 0x00000000);
00442        
00443      M1PWM=0.1;
00444      M1DIR=0;
00445           
00446       while(FINIZQR==0){
00447          pc.printf("BUSCANCO IZQUIERDA LADO OPUESTO\n\r");
00448           }  
00449        M1PWM=0;         
00450        XMAX=__HAL_TIM_GET_COUNTER(&timer2);
00451        M1PWM=0;
00452        XMIN=0;
00453        LIMIN=220;
00454        LIMAX=XMAX-220;
00455        MITAD=(LIMAX-LIMIN)/2; 
00456        pc.printf("Fijando los limites XMIN+220 y XMAX-220 por seguridad, XMIN:%6.0f XMAX:%6.0f MITAD:%6.1f \n\r",XMIN, XMAX, MITAD);
00457    
00458     }
00459 
00460 void centrar(){
00461      float error, pos;
00462      int parada;
00463      float itera;
00464      parada=1;
00465      itera=0;
00466      pos=0;
00467      error=0;
00468      
00469      float K1, K2, K3, uk;
00470             
00471      /*valores por defecto*/
00472      //Kp=0.03;
00473      //Ki=0;
00474      //Kd=0;
00475      
00476      
00477      /*parametros del PID discreto*/
00478     // K1=Kp+(Ki/2)*Ts+(Kd/Ts);
00479     // K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts);
00480     // K3=Kd/Ts;
00481      
00482      K1=0.19;
00483      K2=0;
00484      K3=0;
00485      
00486      
00487      
00488      pos=__HAL_TIM_GET_COUNTER(&timer2);
00489      error=MITAD-pos;
00490      
00491      while(parada==1){
00492  
00493             if (error>0){
00494                         //Error es positivo entonces gire a la izquierda lado opuesto del motor
00495                         M1DIR=0;
00496                        } 
00497             else if(error<=0){
00498                         //Error es negativo entonces gire a la derecha hacia el motor
00499                         M1DIR=1;
00500                        } 
00501             
00502             pos=__HAL_TIM_GET_COUNTER(&timer2);
00503             error=MITAD-pos;
00504             
00505             uk=error*K1;
00506                     
00507             
00508              
00509            pc.printf("BUSCA LA MITAD M:%6.0f P:%6.0f E:%6.1f Uk:%6.1f Itera:%6.0f\n\r",MITAD, pos, error,uk, itera);
00510     
00511             if (abs(uk)>100) {        //Salida  si es mayor que el MAX 
00512                  uk=80;}                          
00513                 else if (abs(uk)<20){      //Salida  si es menor que el MIN 
00514                 uk=5;                         
00515              }
00516                  
00517              M1PWM=(float)uk/100; //Accion de control mapeada a PWM; salida de la tarjeta.
00518         
00519                
00520            
00521             if (abs(error)<5){
00522                 parada=0;
00523                 }
00524             if (itera>1000){
00525                 parada=0;
00526                 }
00527      
00528             itera++;
00529        
00530        }     
00531        M1PWM=0.0;
00532        pc.printf("ENCONTRADO  MITAD:%f  POS:%f  Error: %f Itera: %f\n\r",MITAD, pos, error, itera);    
00533        
00534 
00535 }
00536 
00537 /******************************************PID*******************************************************/
00538 float PID_CODIGO(float MEDIDA,float SETPOINT,float DT){
00539     float OUTPID;
00540  
00541     ERR=SETPOINT-MEDIDA;
00542     INTEGRAL=INTEGRAL+ERR*DT;
00543     DERIVATIVO=(ERR-PREVI_ERR)/DT;
00544     OUTPID=KP*ERR-KI*INTEGRAL+KD*DERIVATIVO;
00545     PREVI_ERR=ERR;
00546 
00547     //wait(DT);
00548     return(OUTPID);
00549    
00550     
00551     //previous_error = 0
00552     //integral = 0 
00553     //start:
00554     //error = setpoint - measured_value
00555     //integral = integral + error*dt
00556     //derivative = (error - previous_error)/dt
00557     //output = Kp*error + Ki*integral + Kd*derivative
00558     //previous_error = error
00559     //wait(dt)
00560     //goto start
00561 
00562     
00563       
00564 }