File content as of revision 0:95f924550159:

/*PROYECTO DEL PENDULO INVERTIDO
 * TARJETA= NUCLEO STM32F446
   ENCODER EJE X PB_8 CH1  PB_9 CH2
   ENCODER PENDULO PB_4 PB_5
   
   FIN DE CARRERA MOTOR IZQ = A1 
   FIN DERECHA DER  = A0
   
   POLOLULU
   
   nD2 = D6
   M1DIR=D7
   M1PWM= D11 (TIMER 1)
   NsF=D12
   GND=D13
   VCC=VCC
 */   



/*------------- THANKS DAVID LOWE FOR YOUR HELP-Your API/HAL use full ---------------------------------------
 * Using STM32's counter peripherals to interface rotary encoders.
 * Encoders are supported on F4xx's TIM1,2,3,4,5. TIM2 & TIM5 have 32bit count, others 16bit.
 * Beware mbed uses TIM5 for system timer, SPI needs TIM1, others used for PWM.
 * Check your platform's PeripheralPins.c & PeripheralNames.h if you need both PWM & encoders.
 *
 * Edit HAL_TIM_Encoder_MspInitFx.cpp to suit your mcu & board's available pinouts & pullups/downs.
 *
 * Thanks to:
 * http://petoknm.wordpress.com/2015/01/05/rotary-encoder-and-stm32/
 *
 * References:
 * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/user_manual/DM00122015.pdf
 * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/reference_manual/DM00096844.pdf
 * http://www.st.com/web/en/resource/technical/document/application_note/DM00042534.pdf
 * http://www.st.com/web/en/resource/technical/document/datasheet/DM00102166.pdf
 * 
 * David Lowe Jan 2015
 *
 
 *Se complementa con el Timer 8 en la tarjeta nucleo
 *Efectivamente el Timer 5 no se debe usar.
 *Se Ajustaron los pines para la tarjeta para usar otros para los PWM
  
 */

#include "mbed.h"
#include "Encoder.h"


/****************************ENCODER INICIALIZACION *************************/
//STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h
#ifdef TARGET_STM32F3
#define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT)
#define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__)            (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR))
#endif

TIM_Encoder_InitTypeDef encoder2, encoder3, encoder4,encoder5, encoder8;
TIM_HandleTypeDef  timer2,  timer3;

/*************************************************************************/

/*CONFIGURAMOS EL SHIELD DEL POLOLULU
 Al coincidir con los pines del timer se modifica el codigo
*/

DigitalOut M1DIR(D7);
PwmOut M1PWM(PA_7);
//AnalogIn   M1FB(A0);

//DigitalOut M2DIR(D8);
//PwmOut M2PWM(D10);
//DigitalIn  M2FB(A1);

DigitalOut nD2(D6,1);
DigitalIn nSF(D12);




/***********PUERTO SERIAL*****************/

Serial pc(SERIAL_TX, SERIAL_RX);


/*VALORES POR DEFECTO*/

float VEL=0.0;

/*VALORES DE CONFIGURACION DE LOS PINES*/

InterruptIn FINIZQR(A1);
InterruptIn FINDERM(A0);
//DigitalIn FINIZQR(D5);
//DigitalIn FINDERM(D6);

int alarma=0;
DigitalOut INDICA(LED2);
DigitalIn  INIT(PC_13);


void toggle1(void); 
void toggle2(void); 
void inicializa(void);
void centrar(void);

/*-------------VARIABLES GLOBALES----------------------*/
       
    float X=0;
    float POS=0;
    float A=0;
    float ANG=0;
    float LIMIN=300;//SI no inicializa toca manual 220 
    float LIMAX=7000;  //Si no inciializa toca 
    
    
    float XMIN=0; //en las pruebas realizadas el 0 +200 pulsos
    float XMAX=7200;; //en las pruebas aprox 7949
    float MITAD=0;
    int DATAPC=0;
    int LEIDO=0;
    
    
    
/*------------------VARIABLES DEL PID Discreto--------------------------*/


float uc0,yn0,en0,cn0,uc1,yn1,cn1,en1,yn2,cn2,en2,yn3,cn3,en3;
    

//Constantes

float q0,q1,q2;

/*-----------------PID DE PENDULO COMPLETO-------------------------------*/
float PID_CODIGO(float MEDIDA,float SETPOINT,float DT);

float KI;
float KP;
float KD; 
float ERR;
float PREVI_ERR;
float INTEGRAL;
float DERIVATIVO;

float DESEADO=180;

float TM=0.01;  // tiempo de muestreo


float numero, cal_pwm;



/*-------------------------------------------------PROGRAMA---------------------------------------------------------------*/

int main()
{
 float SALIDAPID;
 
 
 
 //inicializacion de las varibles   
    yn0=0;
    cn0=0;
    en0=0;
    yn1=0;
    cn1=0;
    en1=0;
    yn2=0;
    cn2=0;
    en2=0;
    yn3=0;
    cn3=0;
    en3=0;

    q0=0.0510;
    q1=-0.00102;
    q2=0;

    INTEGRAL=0.0;
     KI=0.5;
     KP=13;
     KD=0.25; 
     SALIDAPID=0.0;
      
    
    
    
    //Configuramos el puerto de 96000 a 115200 para mayor velocidad
    pc.baud(115200);    


    //counting on both A&B inputs, 4 ticks per cycle, full 32-bit count
    EncoderInit(&encoder2, &timer2, TIM2, 0xffffffff, TIM_ENCODERMODE_TI12);
 
    //counting on both A&B inputs, 4 ticks per cycle, full 32-bit count
    EncoderInit(&encoder3, &timer3, TIM3, 0xffff, TIM_ENCODERMODE_TI12);
 
        int16_t count3=0;
        int32_t count2=0;
        //int8_t dir2, dir3;

    M1PWM.period_ms(10.0); // set PWM period to 10 ms
    
     pc.printf("Listo iniciamos centrando el carro !\n");
     wait(0.1);
    
    
     inicializa();
     centrar();
     alarma=1;
     
     

     pc.printf("Centre el pendulo y Presione reset para continuar \n\r");
     //pc.printf("Press 'a' = aumentar, 'd' = disminuir   'i'=Izquierda 'o'=derecha 'r'=reset contadores 's'=para solo el PWM   \n\r");
     char c ='s';
     if (INIT==0){
                
            yn0=0;
            cn0=0;
            en0=0;
            yn1=0;
            cn1=0;
            en1=0;
            yn2=0;
            cn2=0;
            en2=0;
            yn3=0;
            cn3=0;
            en3=0;
            alarma=0;
            VEL=0;
            numero=0;
            wait(0.5);               
                }  
       
    
    //Ciclo principal
    while(1) {
   

     //----Deje habilitado la lectura del teclado para pruebas
        if(pc.readable()==1) {
               wait(0.1);
               c = pc.getc();
               pc.putc(c);
        
               
                    if((c == 'a') && (VEL < 0.99)) {
                             VEL += 0.01; 
                             M1PWM = VEL; 
                    }
                    if((c == 'd') && (VEL > 0.01)) { 
                        VEL -= 0.01; 
                        M1PWM = VEL; 
                    }
                    if (c == 'i') {
                    M1DIR=1; 
                    }    
                    if (c == 'o') {
                    M1DIR=0; 
                    }    
                    if (c == 'r') {
                    __HAL_TIM_SetCounter(&timer3, 0x0000);
                    __HAL_TIM_SetCounter(&timer2, 0x00000000);
                    }
                    if (c == 's') {
                    M1PWM = 0.0;
                    }
                                       
                    c='0';       
         }   
        //----fin rutina de teclado
        
         INDICA=alarma;
        
          FINIZQR.rise(&toggle1);
          FINDERM.rise(&toggle2);
        
        
        //Rutina de lectura de los encoder timer2=X y Timer3 = Angulo
        //X es de 100 huecos por revolucion = X4  400 pulsos por revolucion
        //X es de 500 huecos por revolucuon = X4  2000 pulsos por revolucion
        // X aproximado de 7950 +/10% por temas mecanicos de hay que sea variable el centro
        // MITAD = 3757 depende de la ubicacion de los sensores ((MAX-220)-(MIN+220))/2
        
        //count2=TIM2->CNT;
        //dir2=TIM2->CR1&TIM_CR1_DIR;
        count2=__HAL_TIM_GET_COUNTER(&timer2);
        //dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); //no se necesita para el calculo

        //count3=TIM3->CNT;
        //dir3=TIM3->CR1&TIM_CR1_DIR;
        count3=__HAL_TIM_GET_COUNTER(&timer3);
        //dir3 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer3); //no se necesita para el calculo
        
        
        

       X=count2;//sensor de posicion 
       A=count3;// sensor de angulo
       
       pc.printf("%6.0f %6.0f \t", X,A);
        
       if (INIT==0){
                
            yn0=0;
            cn0=0;
            en0=0;
            yn1=0;
            cn1=0;
            en1=0;
            yn2=0;
            cn2=0;
            en2=0;
            yn3=0;
            cn3=0;
            en3=0;
            alarma=0;
            VEL=0;
            numero=0;
            wait(0.5);               
                }
                      
    
     //factor de conversion por longitud/pulsos (720mm/7949 =0.090577 mm* pulso  LTOTAL/PULSOS
       POS=X*720/7949;
       
       
       //factor de conversion grados vuelta/pulsos (360grados/2000 pulsos)=0.18   grados por pulso
       ANG=A*0.18;
       
       pc.printf("POS: %6.3f  ANGULO:%6.3f \t", POS,ANG);
    
      
        if(alarma==0){
     
        SALIDAPID=PID_CODIGO(ANG, DESEADO, TM);
        
        VEL=SALIDAPID;
        
                
        //MAXIMO DE LA TARJETA   
            
            if (VEL>100) {        //Salida PID si es mayor que el MAX 
                 VEL=99;
            }                          
            if (VEL<-100) {        //Salida PID si es mayor que el MAX 
                 VEL=-99;
            }                          
            
            if((VEL<=100)&&(VEL>=0)){
                       VEL=VEL*1;                //Error es positivo entonces gire a la izquierda lado opuesto del motor
                        M1DIR=0;
                       } 
            
            if((VEL<0)&&(VEL>-100)){
                       VEL=VEL*-1;                //Error es negativo entonces gire a la derecha lado opuesto del motor
                        M1DIR=1;
                       } 
              
                       
            if (abs(VEL)<10){      //Salida PID si es menor que el MIN 
                 VEL=1;                         
             }
                 
        M1PWM=(float)VEL/100;//para controlar con las teclas por silas moscas 
    
        
        }
        pc.printf("PID = %f \t", SALIDAPID);    
        pc.printf("REF:%6.2f ANG:%6.2f ERROR: %6.2f ACC:%6.2f  Itera: %.0f\t",DESEADO, ANG, ERR,VEL,numero);  
        
         
        
        
        //pc.printf("El PWM %1.3f \t ",VEL); 
        pc.printf("Alama: %i \n \r",alarma); 
        
        
        
        
        
        /*************************************************************************************************/
        numero=numero+1;
  
  
        /****seguridad***************/
        if ((X+300)>=LIMAX){
          VEL=0;
          M1PWM=0;
          alarma=1;
        }
        if ((X-300)<=LIMIN){
          VEL=0.0;
          M1PWM=0.0;
          alarma=1;
        }
        
        
        
        
        
    }
}

/*---------------------FUNCIONES-----------------------------------------------*/
 
 /*---------------------INTERUPPCIONES------------------------------------------*/
 
    void toggle1() {
           wait(0.25); 
           VEL = 0.0;
           M1PWM = 0.0;
           alarma=1;
           pc.printf("ALARMA FINAL IZQUIERDA\n\r");
    }
    
      void toggle2() {
           wait(0.25); 
           VEL = 0.0;
           alarma=1;
           M1PWM = 0.0;
           pc.printf("ALARMA FINAL DERECHA\n\r");
    }   
    
/*--------------------------------FUCIONES DE INICIALIZACION------------------------------*/


void inicializa(){
        pc.printf("Funcion de Inicializacion iniciada\n\r");
           
     M1PWM=0.1;
     M1DIR=1;
     while(FINDERM==0){
         pc.printf("BUSCANCO DERECHA DEL MOTOR\n\r");
          }
         pc.printf("ENCONTRADA - REINICIANDO EL CONTADOR EN X\n\r");
            __HAL_TIM_SetCounter(&timer2, 0x00000000);
       
     M1PWM=0.1;
     M1DIR=0;
          
      while(FINIZQR==0){
         pc.printf("BUSCANCO IZQUIERDA LADO OPUESTO\n\r");
          }  
       M1PWM=0;         
       XMAX=__HAL_TIM_GET_COUNTER(&timer2);
       M1PWM=0;
       XMIN=0;
       LIMIN=220;
       LIMAX=XMAX-220;
       MITAD=(LIMAX-LIMIN)/2; 
       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);
   
    }

void centrar(){
     float error, pos;
     int parada;
     float itera;
     parada=1;
     itera=0;
     pos=0;
     error=0;
     
     float K1, K2, K3, uk;
            
     /*valores por defecto*/
     //Kp=0.03;
     //Ki=0;
     //Kd=0;
     
     
     /*parametros del PID discreto*/
    // K1=Kp+(Ki/2)*Ts+(Kd/Ts);
    // K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts);
    // K3=Kd/Ts;
     
     K1=0.19;
     K2=0;
     K3=0;
     
     
     
     pos=__HAL_TIM_GET_COUNTER(&timer2);
     error=MITAD-pos;
     
     while(parada==1){
 
            if (error>0){
                        //Error es positivo entonces gire a la izquierda lado opuesto del motor
                        M1DIR=0;
                       } 
            else if(error<=0){
                        //Error es negativo entonces gire a la derecha hacia el motor
                        M1DIR=1;
                       } 
            
            pos=__HAL_TIM_GET_COUNTER(&timer2);
            error=MITAD-pos;
            
            uk=error*K1;
                    
            
             
           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);
    
            if (abs(uk)>100) {        //Salida  si es mayor que el MAX 
                 uk=80;}                          
                else if (abs(uk)<20){      //Salida  si es menor que el MIN 
                uk=5;                         
             }
                 
             M1PWM=(float)uk/100; //Accion de control mapeada a PWM; salida de la tarjeta.
        
               
           
            if (abs(error)<5){
                parada=0;
                }
            if (itera>1000){
                parada=0;
                }
     
            itera++;
       
       }     
       M1PWM=0.0;
       pc.printf("ENCONTRADO  MITAD:%f  POS:%f  Error: %f Itera: %f\n\r",MITAD, pos, error, itera);    
       

}

/******************************************PID*******************************************************/
float PID_CODIGO(float MEDIDA,float SETPOINT,float DT){
    float OUTPID;
 
    ERR=SETPOINT-MEDIDA;
    INTEGRAL=INTEGRAL+ERR*DT;
    DERIVATIVO=(ERR-PREVI_ERR)/DT;
    OUTPID=KP*ERR-KI*INTEGRAL+KD*DERIVATIVO;
    PREVI_ERR=ERR;

    //wait(DT);
    return(OUTPID);
   
    
    //previous_error = 0
    //integral = 0 
    //start:
    //error = setpoint - measured_value
    //integral = integral + error*dt
    //derivative = (error - previous_error)/dt
    //output = Kp*error + Ki*integral + Kd*derivative
    //previous_error = error
    //wait(dt)
    //goto start

    
      
}