pid_pic_corso_2k6.c

#include <18F2520.h>
#include <float.h>

#device adc=10

/**************   Device Config     ******************/

#FUSES NOWDT                    //No Watch Dog Timer
#FUSES WDT128                   //Watch Dog Timer uses 1:128 Postscale
#FUSES INTRC_IO                 //Internal RC Osc, no CLKOUT
#FUSES NOPROTECT                //Code not protected from reading
#FUSES BROWNOUT                 //Reset when brownout detected
#FUSES BORV20                   //Brownout reset at 2.0V
#FUSES NOPUT                    //No Power Up Timer
#FUSES NOCPD                    //No EE protection
#FUSES STVREN                   //Stack full/underflow will cause reset
#FUSES NODEBUG                  //No Debug mode for ICD
#FUSES LVP                      //Low Voltage Programming on B3(PIC16) or B5(PIC18)
#FUSES NOWRT                    //Program memory not write protected
#FUSES NOWRTD                   //Data EEPROM not write protected
#FUSES IESO                     //Internal External Switch Over mode enabled
#FUSES FCMEN                    //Fail-safe clock monitor enabled
#FUSES PBADEN                   //PORTB pins are configured as analog input channels on RESET
#FUSES NOWRTC                   //configuration not registers write protected
#FUSES NOWRTB                   //Boot block not write protected
#FUSES NOEBTR                   //Memory not protected from table reads
#FUSES NOEBTRB                  //Boot block not protected from table reads
#FUSES NOCPB                    //No Boot Block code protection
#FUSES LPT1OSC                  //Timer1 configured for low-power operation
#FUSES MCLR                     //Master Clear pin enabled
#FUSES NOXINST                  //Extended set extension and Indexed Addressing mode disabled (Legacy mode)

#use delay(clock=16000000)
#use rs232(baud=57600,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=9)

#byte T0CON = 0xFD5


/**************   Defines    ******************/
#define IN_CLK    PIN_C0
#define PULS_A    PIN_B0
#define PULS_B    PIN_A4

#define TIMEOUT_RELOAD     3  // 60Hz ovvero 13ms

/**************   Typedefs   ******************/
typedef unsigned int16  WORD;
typedef unsigned int32  DWORD;

/**************   Variables   ******************/

int8  bReadRPMTimeout = TIMEOUT_RELOAD;
WORD  wTimer0Counts;

WORD  velocita;
WORD  wMotorRPMSetpoint;

float fPWM_duty      = 0.0;
float fPWM_duty_prev = 0.0;    //previus cycle PWM value

float fMotorRPM      =0.0;
int8  bTimer1_Overflows=0;
DWORD tempo;

WORD tempo_ciclo_ms;
WORD delay_todo_ms;

int8  bSelect_Input=0;
int8  bSelect_Input_prev=0xFF;

/**************   PID  Variables ******************/
float err   =  0.0;  //errore
float err1  =  0.0;  //errore ciclo precedente
float err2  =  0.0;  //errore due cicli precedenti

float K_P   =  0.01;
float K_I   =  0.001;
float K_D   =  0.001;

#define PID_TIME_CYCLE (100) // 100 ms //attenzione max val 255
#define tempo_pid      PID_TIME_CYCLE  //constant time mode

/**************   Prototypes   ******************/
void Measure_Speed();
void LoopTimeWait();
void User_Interface();
void PID_algo();
void Update_PWM();



/**************   Interrupts   ******************/
#int_TIMER1
void TIMER1_isr()
{
   bTimer1_Overflows ++;
   output_toggle(PIN_B3);

   if(bReadRPMTimeout)
        bReadRPMTimeout--;
}



/**************   Main   ******************/

void main()
{


/**************   Peripherals Config   ******************/
   setup_adc_ports(AN0|VSS_VDD);
   setup_adc(ADC_CLOCK_DIV_64|ADC_TAD_MUL_0);
   setup_wdt(WDT_OFF);

   setup_timer_0(RTCC_INTERNAL);
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);      //T1 65ms interrput (15Hz)
   setup_timer_2(T2_DIV_BY_16,255,1);           //PWM 977Hz 10bit res
   setup_timer_3(T3_DISABLED|T3_DIV_BY_1);

   setup_ccp1(CCP_PWM);
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);
   setup_low_volt_detect(FALSE);

   setup_oscillator(OSC_4MHZ|OSC_NORMAL|OSC_31250|OSC_PLL_ON);    //clock CPU a 16Mhz

/**************   I/O Config   ******************/
   input(IN_CLK);
   output_high(PIN_B1);
   output_high(PIN_B2);

   setup_uart(57600);

   printf("\r\n\r\n");
     printf("HELLO TEST PID MOTOR CONTROL\r\n");

   set_timer1(0);


   enable_interrupts(INT_TIMER1);
   enable_interrupts(GLOBAL);


/**************   Main loop   ******************/
   while(1)
   {

        Measure_Speed();
        
        LoopTimeWait();
        
        User_Interface();
        
        PID_algo();
        
        Update_PWM();
        
         //   printf("vel=%lu-setpt=%lu\r\n",velocita,wMotorRPMSetpoint);
         //   printf("vel=%f-setpt=%lu\r\n",fMotorRPM,wMotorRPMSetpoint);
        
                                        printf("velocita=%f-wMotorRPMSetpoint=%lu-fPWM_dutycycle=%f-tempo=%lu \r\n",fMotorRPM,wMotorRPMSetpoint,fPWM_duty,tempo);
                        
   }
}


/*********************************************************/
/********************   Functions   **********************/
/*********************************************************/


/**************   Measure Motor Speed   ******************/
void Measure_Speed()
{
    bReadRPMTimeout = TIMEOUT_RELOAD;

    T0CON &= 0x7F;       //timer OFF
    set_timer0(0);

    while(input(IN_CLK) && bReadRPMTimeout);       //wait for H2L
    delay_cycles(5); 

    while(!input(IN_CLK) && bReadRPMTimeout);      //wait for L2H    1st positive edge
    T0CON |= 0x80;    //timer ON
    delay_cycles(5); 

    while(input(IN_CLK) && bReadRPMTimeout);       //wait for H2L
    delay_cycles(5); 

    while(!input(IN_CLK) && bReadRPMTimeout);      //wait for L2H   2nd positive edge
    T0CON &= 0x7F;       //timer OFF

    if(!bReadRPMTimeout)
        fMotorRPM = 0;
    else
    {
       wTimer0Counts = get_timer0();
       velocita = wTimer0Counts;
        
//     fMotorRPM = 60*(1*1000*1000)/((float)velocita * 16.0);//rpm
       fMotorRPM = (2*1000*1000)/((float)wTimer0Counts);    //rpm
       fMotorRPM /=   16;   //Number of holes
       fMotorRPM *=   60;   //Hz to RPM
     }
}

/**************   Measure Loop Time   ******************/
void LoopTimeWait()
{
    
    disable_interrupts(INT_TIMER1);
        tempo =  (DWORD)get_timer1();
        tempo += ((DWORD)bTimer1_Overflows)<<16; // * 65536;
    enable_interrupts(INT_TIMER1);
    
    tempo_ciclo_ms = tempo / (2*1000);  //tempo ms
    
    printf("t=%lu-OWF=%u-tcy=%lu ",tempo,bTimer1_Overflows,tempo_ciclo_ms);
    //            bTimer1_Overflows=0;
      
    if(PID_TIME_CYCLE > tempo_ciclo_ms)
    {
       delay_todo_ms = (PID_TIME_CYCLE - tempo_ciclo_ms);
       printf("d=%lu ",delay_todo_ms);
       while (delay_todo_ms > 255)
       {
          delay_ms(255);
          delay_todo_ms -=255;
       }
       delay_ms((int8)(delay_todo_ms));
    }
    
    disable_interrupts(INT_TIMER1);
        set_timer1(0);
        bTimer1_Overflows=0;
    enable_interrupts(INT_TIMER1);
}

/**************   PID Algorithm   ******************/
void PID_algo()
{
    err2=err1;                    //errore due cicli precedenti
    err1=err;                     //errore ciclo precedente
    err=(float)wMotorRPMSetpoint-fMotorRPM;     //errore
    
    if(0==bSelect_Input)        //PID mode
    {
        fPWM_duty +=  K_P * err;                                   //P
        fPWM_duty +=  K_I *  tempo_pid * (err-err1);               //I
        fPWM_duty +=  K_D * (err - 2*err1 + err2) / tempo_pid;     //D
    }
    else if (1==bSelect_Input)  //P mode
    {
        fPWM_duty +=  K_P * err;                                   //P
    }
    else if (2==bSelect_Input)  //I mode
    {
     fPWM_duty +=  K_I *  tempo_pid * (err-err1);                  //I
    }
    else if (3==bSelect_Input)  //D mode
    {
     fPWM_duty +=  K_D * (err - 2*err1 + err2) / tempo_pid;        //D
    }
    
    
    //limit fPWM_duty 0-1023
    if (fPWM_duty > 1023)
       fPWM_duty = 1023;
    
    if (fPWM_duty < 0.0)
       fPWM_duty = 0;
}

/**************   Set PWM Output   ******************/
void Update_PWM()
{
   //update PWM only if fPWM_duty changes
   if (fPWM_duty != fPWM_duty_prev)
   {
      set_pwm1_duty((WORD)fPWM_duty);
      fPWM_duty_prev = fPWM_duty;
   }
}


/**************   User Interface - Buttons   ******************/
void User_Interface()
{
    if(input(PULS_A))      //button polling
    {
        wMotorRPMSetpoint = read_adc();   //range 0-3048
        wMotorRPMSetpoint += read_adc();  
        wMotorRPMSetpoint += read_adc();  
    }
    else
    {
        K_P = (float)read_adc()/1000.0;     //0-1
        K_I = K_P/10.0;                     //0-0,1
        K_D = K_I;                          //0-0,1
        
        printf("P=%f-D=%f-I=%f\r\n",K_P,K_D,K_I);
    }
    
    
    if(!input(PULS_B))      //button polling
    {
      while(!input(PULS_B)); //wait for button release
    
      bSelect_Input++;
      if(bSelect_Input>3)
         bSelect_Input=0;
    }
    
    if (bSelect_Input != bSelect_Input_prev)
    {
        bSelect_Input_prev = bSelect_Input;
        
        if(0==bSelect_Input)        //PID mode
        {
            output_low(PIN_B1);
            output_low(PIN_B2);
            printf("Proporzionale-integrativo-derivativo\r\n");
        }
        else if (1==bSelect_Input)  //P mode
        {
            output_low(PIN_B1);
            output_high(PIN_B2);
            printf("Proporzionale\r\n");
        }
        else if (2==bSelect_Input)  //I mode
        {
            output_high(PIN_B1);
            output_low(PIN_B2);
            printf("Integrativo\r\n");
        }
        else if (3==bSelect_Input)  //D mode
        {
            output_high(PIN_B1);
            output_high(PIN_B2);
            printf("Derivativo\r\n");
        }

                delay_ms(500);
        }
    
}

---