#include <hcs12dp256.h> #include <string.h> #include <stdio.h> #include <float.h> #include <math.

h>

void programar_PWM(void); // subrutina de programar PWM void captura (void); void FREC(void); void ADC (void); // subrutina de programar captura de pulsos encoder // subrutina cambiar frecuencia a 24 MHz // subrutina programar ADC

void accion_de_control1(void); // subrutina accion de control para motor 1 void accion_de_control2(void); // subrutina accion de control para motor 2 void rever(void); void giro(void); // subrutina de reversa o adelante // subrutina para cambiar derecha e izquierda

int Wref,Wl,a,b,c,d,SP,Wl2,SP2,l; unsigned int z; float ukT,ukTa,kpd=2,kid=2,Wr,Wra,ukT2,ukTa2,Wr2,Wra2;

void accion_de_control1(void){ ukT=-kpd*(Wr-Wra)+kid*(SP-Wr)+ukTa; // ecuacion de acccion de control ukt= accion de control if(ukT>32000){ // limite superior de velocidad ukT=32000; } else if(ukT<0){ // limite inferior de velocidad

ukT=0; } z=ukT; // a Z se asigna la accon de control PWMDTY7=z & 0x00ff; // los 8 bits se le asignan al ciclo de trabajo del PWMD7 PWMDTY6=z >> 8; // corrimiento de 8 bits ukTa=ukT; // accion de control anterior se guarda accion de control Wra=Wr; // velocidad real anterior } void accion_de_control2(void){ ukT2=-kpd*(Wr2-Wra2)+kid*(SP2-Wr2)+ukTa2; //accion de control 2 para motor 2 if(ukT2>32000){ // limite superior de velocidad ukT2=32000; } else if(ukT2<0){ // limite inferior de velocidad ukT2=0; } z=ukT2; PWMDTY5=z & 0x00ff; PWMDTY4=z >> 8; ukTa2=ukT2; //accion de control anterior Wra2=Wr2; } // velocidad real anterior

/*cambiar frecuencia*/ void FREC(void) {

REFDV =1; SYNR =2;

// frecuencia de 24 MHz f=2*16 MHz*(SYNR+1)/(REFDV+1)

asm("nop\n" "nop\n" "nop\n" "nop\n"); #define LOCK 0x08 while(!(CRGFLG & LOCK)) // espera asta q el micro acepta la frecuencia ; #define PLLSEL 0x80 CLKSEL |= PLLSEL; // se le carga ala etiketa un valor // para elegir la frecuencia calculada

} /*progrmar adc*/ void ADC (void){

ATD0CTL2=0X80; ATD0CTL3=0X40; ATD0CTL4=0X60; ATD0CTL5=0X30; } /*pwm*/ void programar_PWM(void){ PWMCTL=0xc0; PWME=0xA0; PWMPOL=0xA0; PWMCLK=0x00; // concatenar PWM // habilitacion delos PWM (7 y 5) // polaridad de salida (alto o bajo) // seleccion de timer A-SA y B-SB

PWMPRCLK=0x00; // divicion de frecuencia del timer A y B PWMCAE=0x00; // alineaciondel PWM hacia la izquierda y hacia el centro

PWMSCLB=0X01; PWMPER7=0X00; PWMDTY7=0X00;

// divicion del timer SB //periodo del PWM 7 // ciclo del trabajo se varia para mover un motor 7

PWMPER6=0X80; // periodo de PWM 6 PWMDTY6=0X00; // ciclo de trabajo 6 PWMPER5=0X00; // periodo del PWM 5 PWMDTY5=0X00; // ciclo de trabajo 5 PWMPER4=0X80; // periodo del PWM 4 PWMDTY4=0X00; // ciclo de trabajo 4

/*captura de datos*/ void captura (void){ ICPAR |=0x03; // abilitar capturaddores DLYCT |=0x01; // retardo TCTL4 |=0x0A; // flancos TIOS |=0x00; // input capture ICSYS |=0x07; // sobreescritura de almacenamiento ICOVW=0xff; // sobreescritura

void main(){ FREC(); COPCTL = 0x00; //perro guardian

DDRH=0xf0; // puerto captura(); programar_PWM(); SCI0BD=156; // configuracion de velocidad en baulios de comuninicacion serial SCI0CR2 = 0x0c; // activar bidireccional RTICTL |= 0x43; // tiempo de muestreo esta trabajando 2.048mS

CRGFLG |= 0x80; // limpiar bandera CRGINT |= 0x80; // habilita la interrupcion real asm("cli"); while(1){ Wr=Wl; // velocidad real Wr, velocidad real 1 W1 Wr2=Wl2; // velocidad real 2 Wr2 accion_de_control1(); accion_de_control2(); if(l==1){ // se hace esta acion cada 2 mS while ((SCI0SR1 & 0x80) == 0) ; SCI0DRL =120; while ((SCI0SR1 & 0x80) == 0) ; SCI0DRL =Wr; while ((SCI0SR1 & 0x80) == 0) //que se espere asta q deje de transmitir ; SCI0DRL =Wr2; l=0; //habilita las interrupciones

while ((SCI0SR1 & 0x80) == 0) ; Wref=SCI0DRL; rever(); giro(); } } void time(void){ MCCTL |=0x10; // registro para hacerla captura latch Wl=PA0H; // captura de pulsos 1

Wl2=PA1H; //captura de pulsos 2 l=1; CRGFLG |= 0x80; // se resetea la bandera asm("rti"); } void rever(void){ a=PTH & 0x0f; if(a==0x01){ SP=0; SP2=0; b=1; } else if(a==0x02){ SP=0; // regresa al programa retorna de donde brinco

SP2=0; b=2; } else if(Wr<=1 && b==1){ PTH=0x30; b=3; } else if(Wr<=1 && b==2){ PTH=0x00; b=3; } else if(a==0 && b==3){ SP=Wref; SP2=Wref; } }

void giro(void){ a= PTH & 0x0c; if(a==0x04){ SP=0; } if(a==0x08){ SP2=0; }