#include <p18f4550.h> /*Afegim la lliberira del nostre PIC*/
#include <pwm.h> /*Afegim les llibreries referents al modulador d'amplada d'impulsos*/
#include <timers.h> /*Afegim les llibreries dels timers*/
#include <adc.h> /*Afegim la llibreria del conversor analògic digital*/
#pragma config WDT = ON , WDTPS = 128 /*Activem el watchdog i posem el watchdog postscaler a 1:128,*/
#pragma config OSC = HS /*Seleccionem l'oscil·lador HS*/
#pragma config BOR = ON /*El Brown-out resset només per hardware, l'SBOREN està deshabilitat*/
#pragma config BORV = 2 /*Escollim 2 bits pel Brown-out*/
#pragma config VREGEN = ON /*Habilitem el regulador de voltatge de l'USB*/
#pragma config MCLRE = ON /*MCLR habilitat per tant el pin RE3 queda deshabilitat*/
#pragma config PLLDIV = 5 /*Dividim per 5 (Per un cristall de 20 MHz)*/
//Definim les variables globals
char direccio,velocitat,gir;
int lent=300,normal=800,rapid=1000,aturat=0,consigna,velreal_a,velreal_b;
//Funció de retard per causar el gir de 30 graus
void delay_gir30 (void)
{
int i;
for (i = 0; i < 100; i++)
;
}
//Funció de retard per causar el gir de 60 graus
void delay_gir60 (void)
{
int i;
for (i = 0; i < 1000; i++)
;
}
//Funció de retard per causar el gir de 90 graus
void delay_gir90 (void)
{
int i;
for (i = 0; i < 10000; i++)
;
}
void control_PID (void) /* Funció de control amb PID */
{
float u_a,u_2_ant_a,e_a,e_ant_a,e_2_ant_a,u_escalat_a,u_b,u_2_ant_b,e_b,e_ant_b,e_2_ant_b,u_escalat_b; /*Declarem variables locals*/
int K1,K2,K3,K; /*Declarem les constants del PID*/
K1=1.4; /* Els hi donem una valors trobats empíricament*/
K2=-2.8;
K3=1.3;
K=2;
e_a=velreal_a-consigna; /*Aquest és l’error del motor 1*/
e_b=velreal_b-consigna; /*Aquest és l’error del motor 2*/
u_a=u_2_ant_a+K1*e_a+K2*e_ant_a+K3*e_2_ant_a; /*Duem a terme el control PID del motor 1*/
u_escalat_a=u_a*K;
u_b=u_2_ant_b+K1*e_b+K2*e_ant_b+K3*e_2_ant_b; /*Duem a terme el control PID*/
u_escalat_b=u_b*K;
SetDCPWM1(u_escalat_a);
SetDCPWM1(u_escalat_b);
if(u_escalat_a>1023) /*Marquem els marges del motor 1*/
{
u_escalat_a=1020;
}
if(u_escalat_a<1)
{
u_escalat_a=1;
}
if(u_escalat_b>1023) /*Marquem els marges del motor 2*/
{
u_escalat_b=1020;
}
if(u_escalat_b<1)
{
u_escalat_b=1;
}
u_2_ant_a=u_ant_a; /*Actualitzem els senyals*/
u_ant_a=u_a;
e_2_ant_a=e_ant_a;
e_ant_a=e_a;
u_2_ant_b=u_ant_b; /*Actualitzem els senyals*/
u_ant_b=u_b;
e_2_ant_b=e_ant_b;
e_ant_b=e_b;
}
void lectura_velreals (void) /*Aquesta funció ens servirà per llegir les velocitats dels dos motors.*/
{
int result_1,result_2,result_3,result_4; /*Declarem variables enteres*/
// Configurem la conversió: la freqüència a la qual treballarà la conversió és la meitat de la del pic,
// posem la justificació de la conversió a la dreta, escollim el temps d'adquisició, preparem els canals que
// seran utilitzats que són el canal 0,1,2 i 3 i finalment deshabilitem la interrupció per final de conversió.
OpenADC(ADC_FOSC_2&ADC_RIGHT_JUST&ADC_12_TAD&ADC_CH0&ADC_CH1&ADC_CH2&ADC_CH3&ADC_INT_OFF);
SetChanADC(ADC_CH0);
ConvertADC(); /*Comencem la conversió.*/
WhileADC(BusyADC()
; /*Ens esperem fins que acabi la conversió.*/
result_1 = ReadADC();
SetChanADC(ADC_CH1);
ConvertADC(); /*Comencem la conversió.*/
WhileADC(BusyADC()
; /*Ens esperem fins que acabi la conversió.*/
result_2 = ReadADC();
SetChanADC(ADC_CH2);
ConvertADC(); /*Comencem la conversió.*/
WhileADC(BusyADC()
; /*Ens esperem fins que acabi la conversió.*/
result_3 = ReadADC();
SetChanADC(ADC_CH3);
ConvertADC(); /*Comencem la conversió.*/
WhileADC(BusyADC()
; /*Ens esperem fins que acabi la conversió.*/
result_4 = ReadADC();
velreal_a=result_1-result_2; /*Busquem el valor en borns del motor 1, o sigui, la tensió equivalent a la velocitat actual.*/
velreal_b=result_3-result_4; /*Busquem el valor en borns del motor 2, o sigui, la tensió equivalent a la velocitat actual.*/
CloseADC(); /*Deshabilitem el conversor un cop hem fet la conversió.*/
}
void lectura_bateria (void) /*Es preten saber el nivell de càrrega de la bateria*/
{
int nivell_bateria;
// Configurem la conversió: la freqüència a la qual treballarà la conversió és la meitat de la del pic,
// posem la justificació de la conversió a la dreta, escollim el temps d'adquisició, preparem els canals que
// seran utilitzats que són el canal 1,2 i 3 i finalment deshabilitem la interrupció per final de conversió.
OpenADC(ADC_FOSC_2&ADC_RIGHT_JUST&ADC_12_TAD&ADC_CH5&ADC_INT_OFF);
SetChanADC(ADC_CH5);
ConvertADC(); /*Comencem la conversió.*/
WhileADC(BusyADC()
; /*Ens esperem fins que acabi la conversió.*/
nivell_bateria = ReadADC();
if (nivell_bateria<=1000)
{
PORTDbits.RD0 = 1; /*Si el nivell de bateria és inferior al desitjat encenem el led 3*/
}
}
void recepcio_dades (void) /*Aquesta funció configura el recptor inalàmbric i llegeix el que n'hi arriba tot posant-ho en una variable.*/
{
int recepcio[4], n; /* Declarem una estructura i una variable entera.*/
// Configurem el mòdul receptor/transmissor en mode recepció.
PORTDbits.RD4 = 1; /*Posem TX del Radometrix a nivell alt.*/
PORTDbits.RD5 = 0; /*Posem RX del Radometrix a nivell alt.*/
PORTCbits.RC6 = 0; /*Posem TXD a nivell baix car així ens ho indica el fabricant si només volem treballar com a receptor.*/
// Codifiquem el missatge de tal manera que es començarà a llegir si primer arriba una seqüència tipus 10101010. Llavors la informació
// arribarà en quatre bits. Els dos primers ens indiquen la velocitat de tal manera que 00 significa aturat, 01 significa lent, 10 significa
// normal i 11 significa ràpid. Els altre dos bits següents ens indiquen la direcció de tal manera que 00 significa endavant, 01 significa
// enrera, 10 significa gir a dretes i 11 significa gir a esquerres.
if (PORTCbits.RC7 == 1;) /*Si arriba un 1 per la pota RC7 esperarem que la següent sigui un 0*/
{
if (PORTCbits.RC7 == 0;) /*Si arriba un 1 per la pota RC7 esperarem que la següent sigui un 0*/
{
if (PORTCbits.RC7 == 1;) /*Si arriba un 0 per la pota RC7 esperarem que la següent sigui un 1*/
{
if (PORTCbits.RC7 == 0;) /*Si arriba un 1 per la pota RC7 esperarem que la següent sigui un 0*/
{
if (PORTCbits.RC7 == 1;) /*Si arriba un 0 per la pota RC7 esperarem que la següent sigui un 1*/
{
if (PORTCbits.RC7 == 0;) /*Si arriba un 1 per la pota RC7 esperarem que la següent sigui un 0*/
{
if (PORTCbits.RC7 == 1;) /*Si arriba un 0 per la pota RC7 esperarem que la següent sigui un 1*/
{
if (PORTCbits.RC7 == 0;) /*Si arriba un 1 per la pota RC7 esperarem que la següent sigui un 0*/
{
for (n=0;n<=4;n++) /*Si*/
{
recepcio[n] = PORTCbits.RC7; /*Anem col·locant dins l'estructura recepcio els 4 bits*/
}
}
}
}
}
}
}
}
}
//En funció del valor dels bits rebuts actuem d'una maner o altre
if ((recepcio[0]==0)&&((recepcio[1]==0))
{
velocitat=0; /*Posem la variable velocitat igual a zero que significarà aturat.*/
}
if ((recepcio[0]==1)&&((recepcio[1]==0))
{
velocitat=1; /*Posem la variable velocitat igual a u que significarà lent.*/
}
if ((recepcio[0]==0)&&((recepcio[1]==1))
{
velocitat=2; /*Posem la variable velocitat igual a dos que significarà normal.*/
}
if ((recepcio[0]==1)&&((recepcio[1]==1))
{
velocitat=3; /*Posem la variable velocitat igual a tres que significarà ràpid.*/
}
if ((recepcio[2]==0)&&((recepcio[3]==0))
{
direccio=1; /*Posem la variable direccio igual a u que significarà endavant.*/
}
if ((recepcio[2]==1)&&((recepcio[3]==0))
{
direccio=2; /*Posem la variable direcció igual a dos que significarà enrera.*/
}
if ((recepcio[2]==0)&&((recepcio[3]==1))
{
direccio=3; /*Posem la variable direcció igual a tres que significarà gir a dretes.*/
}
if ((recepcio[2]==1)&&((recepcio[3]==1))
{
direccio=4; /*Posem la variable direcció igual a qautre que significarà gir a esquerres.*/
}
}
void main (void)
{
//Manca configurar uns interrupció perquè cada cop que ens arriba quelcom interessant via radiofreqüència anem a la funció recepció
// Configurem els ports.
TRISA = 0b00001111; /*Configurem com a entrades RA0, RA1, RA2, RA3 i la resta com a sortides.*/
TRISB = 0b00000111; /*Configurem com a entrades RB0, RB1, RB2 i la resta com a sortides.*/
TRISC = 0b10110001; /*Configurem com a entrades RC0, RC4, RC5, RC7 i la resta com a sortides.*/
TRISD = 0b00001111; /*Configurem com a entrades RD0, RD1, RD2, RD3 i la resta com a sortides.*/
// Configurem el mòdul receptor/transmissor en mode consum baix.
PORTDbits.RD4 = 1; /*Posem TX del Radometrix a nivell alt.*/
PORTDbits.RD5 = 1; /*Posem RX del Radometrix a nivell alt.*/
// Configurem el timer 2 a fi i efecte d'utilitzar el PWM.La configuració és la següent: Interrupció deshabilitada, prescaler 1:1 i postescaler 1:8.
OpenTimer2( TIMER_INT_OFF &T2_PS_1_1 &T2_POST_1_8 );
// Aquest valor determina la freqüència del PWM1 mitjançant la següent fòrmula: PWM period =[(period ) + 1] x 4 x TOSC x TMR2 prescaler
// Pot ser un valor des de 0x00 fins a 0xff.
OpenPWM1(0xff);
// Aquest valor determina la freqüència del PWM2 mitjançant la següent fòrmula: PWM period =[(period ) + 1] x 4 x TOSC x TMR2 prescaler
// Pot ser un valor des de 0x00 fins a 0xff.
OpenPWM2(0xff);
// El valor del dutycicle pot ser un nombre de 10 bits, per tant entre 0 i 1023. El duty cicle pot ser calculat mitjançant la següent
// fòrmula: PWM x Duty cycle = (DCx<9:0>) x TOSC. On DCx<9:0> són els 10 bits especificats a la funció.Aquesta funció escriu el nou valor del duty cicle del PWM1.
// Incicialment el posem a zero.
SetDCPWM1(0);
// El valor del dutycicle pot ser un nombre de 10 bits.El duty cicle pot ser calculat mitjançant la següent fòrumula: PWM x Duty cycle = (DCx<9:0>) x TOSC
// On DCx<9:0> són els 10 bits especificats a la funció.Aquesta funció escriu el nou valor del duty cicle del PWM2. Incicialment el posem a zero.
SetDCPWM2(0);
lectura_bateria(); /*Cridem la funció que ens controla l'estat de la bateria.*/
While (1) /*El programa romandrà dins d'aquest llaç fins que no apaguem l'aparell*/
{
// Quan "direccio" sigui igual a 1 significarà "endavant". LLavors haurem d'actuar amb la mateixa velocitat als dos motors i aquesta podrà ser
// lenta, normal o ràpida en funció de si "velocitat" és igual a 1, a 2 o a 3.
if (direccio==1)
{
// Mitjançant la combinació del led 4 i el led 5 indicarem en quin dels quatre moviments estem, en aquest cas 00 és endavant
PORTDbits.RD1 = 0;
PORTDbits.RD2 = 0;
// Configurerm les dues rodes girant a dretes. O sigui el robotet endavant.
PORTDbits.RD7 = 1; /*Posem la pota RD7 a nivell alt. Posem el motor 1 gir a dretes, o sigui, endavant.*/
PORTDbits.RD6 = 1; /*Posem la pota RD6 a nivell alt. Posem el motor 2 gir a dretes, o sigui, endavant.*/
if(velocitat==0)
{
consigna=aturat; /* Posem la consigna a aturat*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el control de velocitat en funció de la consigna*/
}
if(velocitat==1)
{
consigna=lent; /* Posem la consigna a velocitat lenta*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el control de velocitat en funció de la consigna*/
}
if(velocitat==2)
{
consigna=normal; /* Posem la consigna a velocitat normal*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el controlde velocitat en funció de la consigna*/
}
if(velocitat==3)
{
consigna=rapid; /* Posem la consigna a velocitat ràpida*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el controlde velocitat en funció de la consigna*/
}
}
// Quan "direccio" sigui igual a 2 significarà "enrera". LLavors haurem d'actuar amb la mateixa velocitat als dos motors però en sentit contrari.
// Aquesta velocitat podrà ser lenta, normal o ràpida en funció de si "velocitat" és igual a 1, a 2 o a 3.
if (direccio==2)
{
// Mitjançant la combinació del led 4 i el led 5 indicarem en quin dels quatre moviments estem, en aquest cas 01 és enrera
PORTDbits.RD1 = 0;
PORTDbits.RD2 = 1;
//Configurerm les dues rodes girant a esquerres. O sigui el robotet enrera.
PORTDbits.RD7 = 0; /*Posem la pota RD7 a nivell baix. Posem el motor 1 gir a esquerres, o sigui, enrera.*/
PORTDbits.RD6 = 0; /*Posem la pota RD6 a nivell baix. Posem el motor 2 gir a esquerres, o sigui, enrera.*/
if(velocitat==0)
{
consigna=aturat; /* Posem la consigna a aturat*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el control de velocitat en funció de la consigna*/
}
if(velocitat==1)
{
consigna=lent; /* Posem la consigna a velocitat lenta*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el controlde velocitat en funció de la consigna*/
}
if(velocitat==2)
{
consigna=normal; /* Posem la consigna a velocitat normal*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el controlde velocitat en funció de la consigna*/
}
if(velocitat==3)
{
consigna=rapid; /* Posem la consigna a velocitat ràpida*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el controlde velocitat en funció de la consigna*/
}
}
//Quan "direccio" sigui igual a 3 significarà "gir a dretes". LLavors el motor A anirà endavant i el B enrera, però tant un com l'altre a velocitat lenta
//durant més o menys temps en funció del que volguem girar.
if (direccio==3)
{
// Mitjançant la combinació del led 4 i el led 5 indicarem en quin dels quatre moviments estem, en aquest cas 10 és gir a dretes
PORTDbits.RD1 = 1;
PORTDbits.RD2 = 0;
//Configurerm el motor 1 a gir dretes i el motor 2 a gir a esquerres. O sigui, el robot girarà a dretes.
PORTDbits.RD7 = 1; /*Posem la pota RD7 a nivell alt. Posem el motor 1 gir a dretes, o sigui, endavant.*/
PORTDbits.RD6 = 0; /*Posem la pota RD6 a nivell baix. Posem el motor 2 gir a esquerres, o sigui, enrera.*/
if(gir==1) /*Quan la variable gir sigui igual a 1 farem un gir curt de 30 graus*/
{
consigna=lent; /* Posem la consigna a velocitat lenta*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el controlde velocitat en funció de la consigna*/
delay_gir30(); /*Durant aquest temps girarem*/
}
if(gir==2) /*Quan la variable gir sigui igual a 2 farem un gir mig de 60 graus*/
{
consigna=lent; /* Posem la consigna a velocitat lenta*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el controlde velocitat en funció de la consigna*/
delay_gir60(); /*Durant aquest temps girarem*/
}
if(gir==3) /*Quan la variable gir sigui igual a 3 farem un gir llarg de 90 graus*/
{
consigna=lent; /* Posem la consigna a velocitat lenta*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el controlde velocitat en funció de la consigna*/
delay_gir90(); /*Durant aquest temps girarem*/
}
}
//Quan "direccio" sigui igual a 4 significarà "gir a esquerres". LLavors el motor A anirà enrera i el B endavant, però tant un com l'altre a velocitat lenta
// durant més o menys temps en funció del que volguem girar.
if (direccio==4)
{
// Mitjançant la combinació del led 4 i el led 5 indicarem en quin dels quatre moviments estem, en aquest cas 11 és gir a esquerres
PORTDbits.RD1 = 1;
PORTDbits.RD2 = 1;
//Configurerm el motor 1 a gir esquerres i el motor 2 a gir a dretes. O sigui, el robot girarà a esquerres.
PORTDbits.RD7 = 0; /*Posem la pota RD7 a nivell baix. Posem el motor 1 gir a esquerres, o sigui, enrera.*/
PORTDbits.RD6 = 1; /*Posem la pota RD6 a nivell alt. Posem el motor 2 gir a dretes, o sigui, endavant.*/
if(gir==1) /*Quan la variable gir sigui igual a 1 farem un gir curt de 30 graus*/
{
consigna=lent; /* Posem la consigna a velocitat lenta*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el controlde velocitat en funció de la consigna*/
delay_gir30(); /*Durant aquest temps girarem*/
}
if(gir==2) /*Quan la variable gir sigui igual a 2 farem un gir mig de 60 graus*/
{
consigna=lent; /* Posem la consigna a velocitat lenta*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el controlde velocitat en funció de la consigna*/
delay_gir60(); /*Durant aquest temps girarem*/
}
if(gir==3) /*Quan la variable gir sigui igual a 3 farem un gir llarg de 90 graus*/
{
consigna=lent; /* Posem la consigna a velocitat lenta*/
lectura_velreals(); /*Llegim les velocitats reals tant d'un motor com de l'altre*/
control_PID(); /*Duguem a terme el control de velocitat en funció de la consigna*/
delay_gir90(); /*Durant aquest temps girarem*/
}
}
}
CloseTimer2(); //Desactivem el timer 2 un cop ja ha fet el servei
}
