aquí pongo el código, en un principio sólo está funcionando con un controlador proporcional, si el error=0 o muy próximo a 0 funciona bien, pero si ya desplazo el motor un poco pues se vuelve loco se va para el lado opuesto al que lo muevo para corregir el error producido pero o no se para o se para en la posición que le da la gana.Si alguien tiene algún código para un 16f877 y me lo enseña se lo agradecería.
El programa y compilador que estoy usando actualmente es el CCS C, para las pruebas estoy usando una impresora antigua, he aprovechado el motor y el encoder, lo que necesito controlar es la velocidad mediante un PID y una PWM y la posición mediante el encoder
#include <16f877.h>
#fuses HS,NOWDT,PUT,BROWNOUT,NOLVP,PROTECT //Tipo de oscilador y frecuencia
#use delay (clock = 4000000, RESTART_WDT)
#USE DELAY
#use fast_io (A)
#use fast_io (B)
#use fast_io (C)
#use fast_io (D)
#use fast_io (E)
#use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8)
#byte porta=5
#byte portb=6
#byte portc=7
#byte portd=8
#byte porte=9
//Librerias
#include <math.h>
#include <float.h>
#define canald PORTD
//Se definen las constantes del controlador PID
signed long long kp=15;
float kd=2;
float ki=8;
signed long long max=100, min=-100;
float I= 0.5;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//float err_ant,err_ant1,err_ant2,err_ant3,sum_err;//A
//Definición de variables
//float sum_int=0;
//Condicion inicial
signed long long integral,proporcional,derivativo, out_PID;
signed long long err_act;//error actual =set_point - posicion
signed long long Duty ; //ciclo de la pwm
signed long long util;
unsigned char bit_ini,j;
signed long long leido = 0,estado = 0;
signed long long posicion= 32000;
unsigned char alarma ;
signed long long set_point= 32000;
//====================================================================
//int lee_alarma(){
//return canal&1;
//}
int lee_encoder() {
return canald&3;
}
void encoder () {
leido = lee_encoder();
if (estado == leido)
return ;
if (estado == 0){//00
if (leido==1){
posicion = posicion-1;
estado = leido;
return;
}
else
if (leido== 2){
posicion=posicion+1;
estado = leido;
return ;
}
else {
estado = leido;
return;
}
}
if (estado==1){
if (leido==3){
posicion=posicion-1;
estado=leido;
return ;
}
else
if (leido==0){
posicion=posicion+1;
estado=leido;
return ;
}
else{
estado=leido;
return;
}
}
if (estado==2){
if (leido==0){
posicion=posicion-1;
estado=leido;
return ;
}
else
if (leido==3){
posicion=posicion+1;
estado=leido;
return;
}
else {
estado=leido;
return;
}
}
if (estado==3){
if (leido==2){
posicion=posicion-1;
estado=leido;
return ;
}
else
if (leido==1){
posicion=posicion+1;
estado=leido;
return;
}
else {
estado=leido;
return;
}
}
}
//====================================================================
//Esta función aproxima el valor de la salida del controlador al entero más proximo
int32 aprox(float rta_controlador) //Funcion aprox
{
float parte_entera,parte_decimal;
int32 ciclo_pwm;
parte_decimal=modf(rta_controlador,&parte_entera);
if(parte_decimal>=0.5)
{
ciclo_pwm=ceil(rta_controlador); //aprox al entero mayor más cercano
}
else
{
ciclo_pwm=floor(rta_controlador); //aprox al entero menor más cercano
}
return (ciclo_pwm);//Regresa el valor de ciclo_pwm,que es un entero de 32 caracteres.
}
//====================================================================
//====================================================================
//------------------------Función principal------------------------------
void main(void){
alarma=0;
//portb=portc=0; //Reset para los puertos
//Configuracion de puertos (E/S)
set_tris_a(0xFF); //entrada
set_tris_b(0x00); //salida
set_tris_c(0x08);
set_tris_d(0xC0); //salida
set_tris_e(0x00); //salida
setup_timer_2(T2_DIV_BY_16, 255, 1); //Pre=1 Periodo=255 Pos=1
setup_ccp1(CCP_PWM);
//Configuracion contador PWM
enable_interrupts(INT_EXT);
//Int. sensor encoder
ext_int_edge( L_TO_H );
//Interrupcion flanco Bajo a alto
enable_interrupts(GLOBAL);
//Activar interrupciones globales
output_high(PIN_D2);//enciendo un led durante 10 milisegundos para
delay_ms(10); // en caso de que se produzca un reseteo del pic detectarlo
output_low(PIN_D2);
/* err_ant=err_act=err_ant1=err_ant2=err_ant3=
out_PID= integral= proporcional= derivativo= 0x00; //Condicion inicial
posicion= bit_ini=16000; //Bandera de inicio
*/
/* if(alarma==1)
{putc(01000001);
putc(1);} //si la alarma se activa envia por el
//puerto serie el texto A1
else
if(alarma==0)
{putc(01000001);
putc(0);} //sino se activa envia por el puerto serie el texto A0
*/
for(;;)
{
// set_point=getc();
encoder();
//set_point=getc();
//delay_ms(100);
//encoder();
//set_point=getc();
// delay_ms(1);
putc(80);//Valor Leido dec=50
encoder(); //leo el encoder y por puerto serie envio la posicion
putc(posicion);
encoder();
putc(posicion >> 8);
encoder();
putc(posicion >> 16);
encoder();
putc(posicion >> 24);
encoder();
/*set_point=getc();
delay_ms(1);
putc(leido);
encoder();
//set_point=getc();
delay_ms(1);
encoder();
delay_ms(10);
putc(01010000);//Valor PWM dec=80
encoder();
delay_ms(1);
putc(out_PID);
delay_ms(1);
encoder(); */
//set_point=getc();
}
//-----Implementación del PID-----------------------------------------//
for (;;) {
err_act = set_point - posicion; //Ecuacion para el error actual
if((10<err_act)&&(10>err_act)){
set_pwm1_duty(0);
output_high(PIN_B2);
continue;}
/* if(err_act>max){err_act=max;}
else{
if (err_act<min){err_act=min;}
}*/
/*---------------Sentido de giro del motor----------------------------------*/
/*--------------------------------------------------------------------------*/
//Si la posición es menor que nuestra consigna avanza hacia la derecha
if(posicion < set_point)
{output_low(PIN_B2);
output_low(PIN_B7);
}
else {
//En caso de que nuestra posición sea mayor que nuestra consigna avanza
//hacia la izquierda
if(posicion > set_point){
output_low(PIN_B2);
output_high(PIN_B7);
} }
//------------------PROPORCIONAL--------------------------------------
proporcional = kp*err_act; //Ecuacion de parte Proporcional
//------------------INTEGRAL------------------------------------------
/*
if(bit_ini==16000) //Si está en condicion inicial 0
{
integral = ki*err_act; //Ecuacion integral
sum_err=err_act;
sum_int=integral;
bit_ini=0xFF;
}
else
{
integral = I*(err_ant+err_act);
integral += sum_err;
integral *= ki;
sum_int+=integral;
sum_err += err_act;
err_ant =err_act;
}
//----------------DERIVATIVO------------------------------------------
derivativo = 3*(err_ant1-err_ant2);
derivativo += err_act;
derivativo -= err_ant3;
derivativo *= kd;
err_ant3=err_ant2; //Memorización para la parte derivativa
err_ant2=err_ant1;
err_ant1=err_act;
//--------------------------------------------------------------------
*/
out_PID = proporcional; //+ sum_int + derivativo;
if (out_PID <0)
out_PID = -1*out_PID;
//out_PID*=60;
Duty = aprox(out_PID);
if(Duty>1023) {
Duty=1022;
util=Duty;
set_pwm1_duty(util);}
else{
util=Duty;
set_pwm1_duty(util);}
}
}
