mainboard_can_ricardo.c

microcontroller pid implementation

#include <p18f4580.h>
#include <stdlib.h>
#include <stdio.h>
#include <delays.h>
#include <pwm.h>
#include <timers.h>
#include <usart.h>
#include <adc.h>
#include <spi.h>

#pragma config OSC=HSPLL
#pragma config PWRT = OFF
#pragma config WDT = OFF
#pragma config MCLRE = ON

#define sentido PORTAbits.RA3    //=0
#define FRENTE 0
#define TRAS 1

#define CAN_FULL 0x01
#define CAN_EMPTY 0x00

#define BUF0_FULL 0x03
#define BUF0_EMPTY 0x04

void config(void);
void comando(void);
void WriteDac(void);
void read_encoder(void);
void int_rx(void);      //FUN钦ES DAS
void desempate(void);   //   INTERRUP钦ES
void pid(void);

//Interrup玢o TIMER 0
#pragma code HighVector=0x08
void interrupt_high(void)
{
   _asm 
   goto desempate
   _endasm
}

//Interrup玢o recep玢o USART
#pragma code low_vector=0x18
void interrupt_low(void)
{
   _asm goto 
      int_rx 
   _endasm
}
#pragma code

unsigned char trama[20];
unsigned char indice=0,nt=0;
unsigned int vel_to_dac=0;
unsigned char Print_enc_vel=0;
unsigned long contador=0;      //guarda o valor actual lido do encoder
unsigned long ult_contador=0;   //guarda o ultimo valor lido do encoder
unsigned char vel_to_print = 0;
unsigned char i=0;
unsigned int velref=0;
unsigned int aux=0; 
//---vari醰eis relacionadas com o controlador PID---//
unsigned int velocidade_ant=0;
signed int erro=0;
signed int erro_ant=0;
signed int erro_ant_ant=0;
signed int P=0;
signed long  P_grande=0;
signed int I=0;
signed long I_grande=0;
signed int D=0;
signed long D_grande=0;
//---Valores de KP,KI e KD
//--Kp=Kp*100 devido a erros de c醠culo multiplica-se por 100 e no fim dos c醠culos divide-se 
signed long Kp=230;
//signed long Kp=20;
//signed long Kp=50;
//--Ki=Ki*100*0.2 devido a erros de c醠culo multiplica-se por 100 e por 0.2(amostragem).No final das
//divide-se tudo por 100
//signed long Ki=10;
//--Ki=Ki*100*0.04
signed long Ki=13;
//--Kd=(Kd/0.2)*100
//signed long Kd=420; 
//signed long Kd=30;
//------------------------------------------------//

unsigned char CAN_RECEIVE;
unsigned char BUFFER;
unsigned char vel_from_can=0;
unsigned char last_vel_from_can=0;

//vari醰eis usadas com mensagens de CAN recebidas
unsigned long idR;         //ID da mensagem recebida
unsigned char dataR[8];      //Dados da mensagem recebida
unsigned char datalenR;      //Tamanho da mensagem recebida
      
unsigned char std_ext;      //vari醰el que verifica o id da mensagem (STANDARD ou EXTENDED)
unsigned int idO;         //vari醰el que guarda o id de ORIGEM
unsigned int idD;         //vari醰el que guarda o id de DESTINO
unsigned int idF;         //vari醰el que guarda o id de FUN敲O

//signed long velocidade=0;         //vari醰el que guarda valor da velocidade
unsigned int velocidade=0;

unsigned char encoder1=0;
unsigned char encoder2=0;
unsigned char encoder3=0;
unsigned char encoder4=0;

//vari醰el que limpa os contadores e a velocidade antes de iniciar
unsigned int sinal_inicio=1;

unsigned char envia_dados_sinaletica=0;


#pragma interrupt desempate
void desempate(void){


    //-----Interrup玢o do timer 0 para ver a velocidade actual e actualiza-la-----//
   if (INTCONbits.TMR0IF == 1)
   {
      
     //valor a por os valores no High byte e Low byte do timer 0 para uma temporiza玢o de 200ms
     TMR0H = 194;
     TMR0L = 246;

     //TMR0H = 243;
     //TMR0L = 202;

      //Leitura do valor do encoder para determinar a velocidade actual 
      ult_contador = contador;
      read_encoder();
      envia_dados_sinaletica = 1;
      vel_to_print = 1;

     //C醠culo da velocidade actual->multiplica玢o para obter a velocidade em mm/s
     velocidade = (unsigned int)(46 * (contador - ult_contador)/50);

        //printf(">velocidade=%u\n\r",velocidade);

  		if(velocidade>velref-150 && velocidade<=velref+150)
  		{
         printf(">velocidade porreira!!!\n\r");

         aux=velref-velocidade;

         velocidade=velocidade+aux;           
           
   		 vel_to_dac=((((unsigned int)velocidade)*28)/100);

   		 WriteDac();

   		 return 0;
  		}	
 
      //Actualiza玢o da velocidade ao chamar o controlador PID
     pid();
     
          //C醠culo da velocidade para enviar para o DAC
     vel_to_dac=((((unsigned int)velocidade)*28)/100);

  
    //Escrita do valor de velocidade no DAC 
      WriteDac();
      
     INTCONbits.TMR0IF = 0;   //clear interrupt flag
      
   }
      //----Recebimento dos valores de velocidade por can----//
   if(PIR3bits.RXB0IF == 1)
   {
      PIR3bits.RXB0IF = 0;

      // Check if buffer is ready.
      if ( RXB0CONbits.RXFUL )
      {
            if ( COMSTATbits.RXB0OVFL )   COMSTATbits.RXB0OVFL = 0; // Forget any previous overflow
         
            std_ext = RXB0SIDLbits.EXID; //1 = extended message; 0 = standard message
            datalenR = RXB0DLC;   //n鷐ero de bytes da mensagem recebida
                  
            dataR[0] = RXB0D0;   //dados recebidos
//            dataR[1] = RXB0D1;
//            dataR[2] = RXB0D2;
//            dataR[3] = RXB0D3;
//            dataR[4] = RXB0D4;
//            dataR[5] = RXB0D5;
//            dataR[6] = RXB0D6;
//            dataR[7] = RXB0D7;
      
            RXB0CONbits.RXFUL = 0;      //Clear Receive Full Status bit (Buffer 0)
      
            idO = (RXB0SIDH & 0xf);         //id da mensagem recebida
            idD = ((RXB0SIDH & 0xf0) >> 4);
            idF = (RXB0SIDL >> 5);
               
            
            if(idD == 0xF)
            {
               if(dataR[0] == 0x02)      // TIME SLOT =2? envia vel em mm/s para WebSIM
               {
                     //Check if the Tx Buffer 0 is ready to transmit
                     if(TXB0CONbits.TXREQ == 0)
                     {
                        //(0x08 = 0000 0001 000 -> Destino: PC; Fonte: Main (0x1); Fun玢o: )
                        
                        //velocidade = 0x0101;
                        TXB0SIDH = 0b00000001;   //ID da mensagem
                        TXB0SIDL = 0b00000000;   //SIDH: SID10 - SID3; SIDL: SID2 - SID0 mais o bit EXIDE = 0 -> message will transmit standard ID
                        TXB0D0 = (velocidade/256);      //DATA da mensagem
                        TXB0D1 = (velocidade % 256);
                        TXB0DLC = 0x02;         //TAMANHO da mensagem
                        TXB0CONbits.TXREQ = 1;   //Set message for transmission
                     }
               }//end if(dataR[0] == 0x02)

				if(dataR[0] == 0x03)      // TIME SLOT =3? envia vel em mm/s e pulsos encoder para sinal閠ica
                {

					if(envia_dados_sinaletica == 1)
					{
	                    //Check if the Tx Buffer 0 is ready to transmit
	                    if(TXB0CONbits.TXREQ == 0)
	                    {
	                        //(0x308 = 0110 0001 000 -> Destino: Sinal閠ica; Fonte: Main (0x1); Fun玢o: )
	                        
	                        TXB0SIDH = 0b01100001;   //ID da mensagem
	                        TXB0SIDL = 0b00000000;   //SIDH: SID10 - SID3; SIDL: SID2 - SID0 mais o bit EXIDE = 0 -> message will transmit standard ID
							TXB0D0 = 0xFF;
							TXB0D1 = 0x0F;
	                        TXB0D2 = (velocidade/256);
	                        TXB0D3 = (velocidade % 256);
							TXB0D4 = encoder1;	//8 bits MSB
							TXB0D5 = encoder2;
							TXB0D6 = encoder3;
							TXB0D7 = encoder4;	//8 bits LSB
	                        TXB0DLC = 0x08;         //TAMANHO da mensagem
	                        TXB0CONbits.TXREQ = 1;   //Set message for transmission
						}

						envia_dados_sinaletica = 0;
					}
				}//end if(dataR[0] == 0x03)
            } // fecha time slot 
   
            //Mensagem recebida do PC - Velocidade
            //(0x80 = 0001 0000 000 -> Destino: Mainboard; Fonte: PC; Fun玢o: )
            
 /*           if(idD == 0x1)
            {

            	vel_from_can= dataR[0];   
               
                if (vel_from_can != last_vel_from_can)
                {   
                     if(dataR[0] <101)
					 {
                        sentido=FRENTE;
                        velref =  dataR[0];
                        vel_to_dac=(unsigned int)((velref*28)/100);
                        WriteDac();
						      
                     }
                                 
                     if (dataR[0] >100)
                     {
                        sentido=TRAS;
                        velref =  dataR[0]-100;                        
                        vel_to_dac=(unsigned int)((velref*28)/100);
						WriteDac();
						
                     }

                     last_vel_from_can = vel_from_can;
                  }      

            }*/    // CAN de Velocidade vindo do CAN      
      }         //fecha tratamento de teste de recep玢o CAN
   }            //fecha decis鉶 CAN

}

#pragma interruptlow int_rx
void int_rx (void)
{
   unsigned char recebeu=0;
   //PIR1bits.RCIF=0; //quando a usart