#define __PCH__ 1
#include <18F452.h>
#device adc=10
#fuses HS,NOWDT,NOPROTECT,NOLVP
#use delay(clock=20000000)
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7)
#include <stdlib.h>
//puerto B
#define DIRR PIN_B2
#define DIRL PIN_B3
#define ENCODERL PIN_B4
#define ENCODERR PIN_B5
//puerto C
#define PWMR PIN_C1
#define PWML PIN_C2
//puerto D
#define MOTORL1 PIN_D0
#define MOTORL2 PIN_D1
#define MOTORR1 PIN_D2
#define MOTORR2 PIN_D3
#define LED PIN_D7
//constantes sensores
#define X_OFFSET 540.0
#define Y_OFFSET 490.0
#define GYRO_OFFSET 402.0
#define MOD 247.0
//otras constantes
#define FORWARD 0
#define BACKWARD 1
#define STOP 2
#define N_SAMPLES 64
#define PRELOAD_TMR1 65043 //10 KHz prescaler=1
#define PR2 107
#define UT_MAX 800
//direcciones eeprom
#define KP_DIR 0x00
#define KI_DIR 0x08
#define KD_DIR 0x10
#define RT_DIR 0x18
//variables globales----------------
float Kp=0.0,Ki=0.0,Kd=0.0;
struct BytesPID
{
float rT; //Valor del Set Point
float eT; //Valor del Error
float iT; //Valor del termino Integral
float dT; //Valor del termino Derivativo
float yT; //Valor real
float uT; //Valor de salida del controlador
float iT0; //Valor del termino integral en t0
float eT0; //Valor del termino del error en t0
}PID = {0,0,0,0,0,0,0,0};
long int accel_x=0,gyro_rate=0;
//variables para el filtro
float P[2][2] = {{ 1, 0 },{ 0, 1 }};
float R_angle = 0.6;
float Q_angle = 0.001;
float Q_gyro = 0.003;
float q_bias=0, angle=0, gyro=0, dt=0.0005,acc_angle=0;
float pwm_R=0, pwm_L=0;
short flag_samples=0;
//funciones-------------------------
void init();
void get_samples();
void calculate_angle();
void refresh_pid();
void set_motors();
void state_update();
void kalman_update(float angle_m);
void write_float_eeprom(long int n, float data);
float read_float_eeprom(long int n);
#int_timer1
void timer1_isr()
{
flag_samples=1;
set_timer1(PRELOAD_TMR1);
}
#int_rda
void serial_isr()
{
char key[16];
if(kbhit())
{
switch(key[0])
{
case 'p':
break;
case 'i':
break;
case 'd':
break;
case 'r':
break;
case 'a':
printf("angle = %f\r\n",angle
); break;
case 'g':
write_float_eeprom(KP_DIR, Kp);
write_float_eeprom(KI_DIR, Ki);
write_float_eeprom(KD_DIR, Kd);
write_float_eeprom(RT_DIR, PID.rT);
printf("datos guardados\r\n"); break;
case 'l':
printf("rT=%f\nkp=%f\nki=%f\nkd=%f\n\r\n",read_float_eeprom
(RT_DIR
),read_float_eeprom
(KP_DIR
),read_float_eeprom
(KI_DIR
),read_float_eeprom
(KD_DIR
)); break;
case 'h':
printf("p<valor>: define la constante kp\ni<valor>: define la constante ki\nd<valor>: define la constante kd\nr<valor>: define el setpoint rT\na: devuelve el anguloactual\ng: guarda las constantes en la eeprom\nl: muestra los valores guardados en la eeprom\n\r\n"); break;
default:
printf("error: comando desconocido\r\n"); break;
}
}
}
void main()
{
delay_ms(100);
init();
while(TRUE)
{
if(flag_samples)
{
get_samples();
calculate_angle();
if(angle < 0.7 && angle > -0.7)
{
refresh_pid();
pwm_R = -PID.uT;
pwm_L = -PID.uT;
}
else
{
pwm_R = 0;
pwm_L = 0;
}
set_motors();
flag_samples = 0;
}
output_toggle(LED);
delay_ms(500);
}
}
void init()
{
//E/S digitales
set_tris_a(0b111111); //RA5,RA4,ref3V3,gyro,accelY,accelX
set_tris_b(0b11111100); //RB7,RB6,encoderR,encoderL,dirL,dirR,RB1,RB0
set_tris_c(0b10000000); //rx,tx,RC5,RC4,RC3,pwm1,pwm2,RC0
set_tris_d(0b00000000); //led,RD6,RD5,RD4,motorR2,motorR1,motorL2,motorL1
//PWM
setup_timer_2(T2_DIV_BY_4, PR2, 1);
setup_ccp1(CCP_PWM); // Configure CCP1 as a PWM
setup_ccp2(CCP_PWM); // Configure CCP2 as a PWM
//Timer1
setup_timer_1 ( T1_INTERNAL | T1_DIV_BY_1 );
//entradas analogicas
setup_adc_ports(AN0_AN1_AN2_AN4_VSS_VREF);
setup_adc(ADC_CLOCK_INTERNAL);
//interrupciones
enable_interrupts(INT_RDA);
enable_interrupts(INT_TIMER1);
disable_interrupts(GLOBAL);
set_timer1(PRELOAD_TMR1);
Kp = read_float_eeprom(KP_DIR);
Ki = read_float_eeprom(KI_DIR);
Kd = read_float_eeprom(KD_DIR);
PID.rT = read_float_eeprom(RT_DIR);
enable_interrupts(GLOBAL);
}
void get_samples()
{
int i;
accel_x=0;
gyro_rate=0;
for(i=0;i<N_SAMPLES;i++)
{
set_adc_channel(0);
delay_us(20);
accel_x += read_adc();
set_adc_channel(2);
delay_us(20);
gyro_rate += read_adc();
}
accel_x /= N_SAMPLES;
gyro_rate /= N_SAMPLES;
}
void calculate_angle()
{
acc_angle = ((float)accel_x - X_OFFSET) / mod;
gyro = -((float)gyro_rate - GYRO_OFFSET);
state_update();
kalman_update(acc_angle);
//printf("%f,%f,%f,\n",acc_angle,gyro,angle);
}
void refresh_pid()
{
PID.yT = angle;
PID.eT = PID.rT - PID.yT; //Calculo del error
PID.iT = PID.eT + PID.iT0; //Calculo de la integral
PID.dT = PID.eT - PID.eT0; //Calculo de la derivada
PID.uT = Kp * PID.eT + Ki * PID.iT + Kd * PID.dT; //Calculo de la señal de control
//Evitamos que uT se sature
if(PID.uT > UT_MAX)
PID.uT = UT_MAX;
if(PID.uT < -UT_MAX)
PID.uT = -UT_MAX;
PID.iT0 = PID.iT; //Actualizamos la integral
PID.eT0 = PID.eT; //Actualizamos el error
}
void set_motors()
{
int dutyR=0, dutyL=0;
if(pwm_R >= 0)
{
dutyR = (int)pwm_R;
output_high(MOTORR2);
output_low(MOTORR1);
}
else
{
dutyR = (int)(-pwm_R);
output_high(MOTORR1);
output_low(MOTORR2);
}
if(pwm_L >= 0)
{
dutyL = (int)pwm_L;
output_high(MOTORL2);
output_low(MOTORL1);
}
else
{
dutyL = (int)(-pwm_L);
output_high(MOTORL1);
output_low(MOTORL2);
}
set_pwm1_duty(dutyR);
set_pwm2_duty(dutyL);
}
void state_update() //Prediccion
{
float Pdot[4];
Pdot[0] = - P[0][1] - P[1][0];
Pdot[1] = -P[1][1];
//Pdot[2] = -P[1][1];
//Pdot[3] = Q_gyro;
gyro -= q_bias;
angle += gyro * dt ;
P[0][0] += (Pdot[0] * dt) + (P[1][1]*dt*dt) + Q_angle;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[1] * dt;
P[1][1] += Q_gyro;
}
void kalman_update( float angle_m )
{
float K_0=0;
float K_1=0;
K_0 = P[0][0] / (R_angle + P[0][0]);
K_1 = P[1][0] / (R_angle + P[0][0]);
P[0][0] -= K_0 * P[0][0];
P[0][1] -= K_0 * P[0][1];
P[1][0] -= K_1 * P[0][0];
P[1][1] -= K_1 * P[0][1];
angle += K_0 * (angle_m - angle);
q_bias += K_1 * (angle_m - angle);
}
void write_float_eeprom(long int n, float data)
{
int i;
for (i = 0; i < 4; i++)
write_eeprom(i + n, *(((int8*)&data + i))) ;
}
float read_float_eeprom(long int n)
{
int i;
float data;
for (i = 0; i < 4; i++)
*((int8*)&data + i) = read_eeprom(i + n);
return(data);
}