MSoft Micro 87LPC766 - Universidad Autónoma de Madrid

X:\ProyecW\1050460 Brazo oscilante\C51\Primer micro grabado 28 Nov 2005\1050460_main.c 11/28
//_________________________________________________________________________________
// UNIVERSIDAD AUTONOMA DE MADRID. SEGAINVEX-Electronica
//
// Proyecto: Control para un motor de CC con realimentación mediante encoder digital
// y salida PWM.
// Micro: Philips 87LPC768
//
// Nº Orden:1050460
// Realizado:Patricio Coronado Collado
// Fichero: 1050460.c
//
// Fechas:
// Inicio: 14 Septiembre 2005
// Final: 28 Noviembre 2005
// Versión: 1.0
//_________________________________________________________________________________
// DESCRIPCIÓN
//
// El sistema actua un motor cc de Maxon con reductora y encoder
// Motor: RE36 70W de Maxon
// Reductora:GP42C 230:1 de Maxon
// Encoder:HEDS5540
// Utiliza un C.I. de potencia:LMD18200T
// Y un lector de encoder HCTL2020
//
// Utiliza un convertidor A/D para leer la consigna de ángulo, 90º a 120º,
// otro A/D para la consigna de velocidad, 1 a 10 rpms. Un PWM para la salida
// Una interrupción para leer los pasos del encoder y otra linea para la
// dirección.
//
// El sistema mueve el eje de un portamuestras un ángulo de 90 a 120º
// cambiando el sentido, y a una velocidad de 1 a 10 rpms
//
//__________________________________________________________________________________
// CODIGO
#ifndef __1050460_main__
#define __1050460_main__
//...................................................................................
// Ficheros include
//...................................................................................
#include <intrins.h>
#include <math.h>
#include <segainc.h>
#include <REG768.H>
#include "1050460.h" // Declaraciones, tipos, variables, ctes etc.
//...................................................................................
//--------------------------------- MAIN ----------------------------------------void main( void ) {
//
// INHABILITA EL MOVIMIENTO DEL MOTOR
FrenoMotor = 1; // Hecha el freno del driver-motor con el bit de freno
PosicionCero=FALSE; // El sistema al encender por defecto no está en la posición de inic
io
//
// CONFIGURA LOS RECURSOS DEL MICROCONTROLADO
inicializa_sistema();
// Hace 2 conversiónes para empezar
while(ADCS || ADCI);
adc_start(VELOCIDAD);
while(ADCS || ADCI);
adc_start(ANGULO);
// Hace 2 conversiónes para empezar
while(ADCS || ADCI);
adc_start(VELOCIDAD);
while(ADCS || ADCI);
adc_start(ANGULO);
Velocidad = VelocidadMADC;// Consigna de velocidad para buscar el cero
//
// SITUA EL MOTOR EN LA POSICIÓN DE REFERENCIA
// Arranca el motor
PWMCON0 |= 0x80;
// Activa el PWM
FrenoMotor = 0; // habilita el driver del motor
// Sentido
Page: 1
X:\ProyecW\1050460 Brazo oscilante\C51\Primer micro grabado 28 Nov 2005\1050460_main.c 11/28
PWMCON0 &= 0xFD;// Sentido del PWM CW
Sentido = CW;
// Regulador
MarchaParo = PARO; // Comienza a barrer el ángulo de consigna desde parado en cero
AceleraRegulador = SI;
FrenadoRegulador = NO; // Para que el regulador no haga un frenado del sistema
Tiempo = 0; // Variable del regulador par frenar suave
TR1 = 1;
// Arranca el timer para empezar a muestrear
ET1=1;
//
if (PinCero) {
while(PosicionCero==FALSE){/* Espera a estar en 0 grados (interrupción ex0) */ }
}
else {
// Si ya está en cero...
IE0=1; // Hace la int externa 0 (la de la posición de cero)
}
// ACTIVA LA INTERRUPCION DEL TOPE DE SEGURIDAD
IE1 = 0;
// Limpia el Flag de la interrupción externa 1
IT1 = 1;
// Interrupción externa INT1 activa por flanco
EX1 = 1;
// Activa interr externa INT1 Rebasamiento de angulo
// Una vez que está en la posición de referencia inicia el movimiento
//
//.............................. BUCLE PRINCIPAL .....................................
// Inicio
while(1){
// feed the watchdog
FEED_WD // Actualiza el WatchDog
while(ADCS || ADCI);
adc_start(VELOCIDAD);
while(ADCS || ADCI);
adc_start(ANGULO);
//SI ESTÁ PARADO COMPRUEBA PIN MARCHA PARO
PinMarchaParo = 1;
// Si hay que arrancar desde parado actualiza variables de ángulo y velocidad
if(MarchaParo == PARO && PinMarchaParo == 1) {
// Consigna de velocidad por si se ha cambiado durante el paro
Velocidad = VelocidadMADC;
// Consigna de ángulo
Coef1Recarga = (unsigned int)Coef1*AnguloADC;
if(AnguloADC <= 122) { // Angulos menores de 102.45 grados
NVueltasT0 = 1;
RecargaT0=Coef2A-Coef1Recarga;
} // del if
else /* AnguloADCc > 106 */ {
NVueltasT0 = 2;
RecargaT0=Coef2B-Coef1Recarga;
}
// Recarga el timer 0 para que recorra el angulo correcto
TL0 = L(RecargaT0);
TH0 = H(RecargaT0);
TF0 = 0; //Limpia el flag de interrupción del timer 0 y..
ET0 = 1; //Activa la interrupción del timer 0 para el rebote
// Prepara y arranca el regulador
TR1 = 1; // Arranca el regulador (Timer 1)
ET1 = 1; //
PinLed = LED_ON;
MarchaParo = MARCHA;
}
}// del while(1)
}// del main
//----------------------------------------------------------------------------------//
FUNCION PARA INICIALIZAR EL SISTEMA
//----------------------------------------------------------------------------------void inicializa_sistema(void)
{
// ESTABLECE LA PRIORIDAD DE LAS INTERRUPCIONES pagina 24 del datasheet
// Angulo cero : Externa0
// set isr priority level = 3
IP0 &= 0xFE; // XXXXXXX0
IP0H &= 0xFE;
IP0 |= 0x01;
IP0H |= 0x01;
// Angulo tope : Externa1
// set isr priority level = 3
Page: 2
X:\ProyecW\1050460 Brazo oscilante\C51\Primer micro grabado 28 Nov 2005\1050460_main.c 11/28
IP0 &= 0xFB; // XXXXX0XX
IP0H &= 0xFB;
IP0 |= 0x04;
IP0H |= 0x04;
// Angulo final : Timer0
// set isr priority level 3
IP0 &= 0xFD; // XXXXXX0X
IP0H &= 0xFD;
IP0 |= 0x02;
IP0H |= 0x02;
// Regulador : Timer 1
// set isr priority level 2
IP0 &= 0xF7;// XXXXXX0X
IP0H &= 0xF7;
IP0H |= 0x08;
// ADC
// set isr priority level 0
IP1 &= 0xEF; // XXX0XXXX
IP1H &= 0xEF;
// Comparadores
// set isr priority Level 3 for comparator 1
IP1 &= 0xDB;// XX0XX0XX
IP1H &= 0xDB;
IP1 |= 0x20;
IP1H |= 0x20;
// WatchDog (no necesita)
//...............................................................................
// INICIALIZA WATCHDOG
// feed the watchdog
FEED_WD // Actualiza el WatchDog
// set up watchdog timer WDinterval
WDinterval &= 0x07;
WDCON = WDinterval;
//...............................................................................
// INICIALIZA COMPARADOR 1
// disable digital inputs on pins used
PT0AD = 0x48;
// set input pins to input mode
P0M2 &= 0xF7;
P0M1 |= 0x08;
// Comparator outputs don't used
// P0M2 |= 0x00;
// P0M1 &= 0xFF;
// configure comparator 1
// Positive input - P0.3 (CIN1B)
// Negative input - Vref
// Comparator output enabled
// Comparator enabled
CMP1 = 0x38;
// configure comparator 2
// Comparator not enabled
CMP2 = 0x00;
//...................................................................................
// INICIALIZA ADC
RCCLK = 0; // 0 selecciona el clk de la CPU
// disable digital input on channel pins
PT0AD |= 0x30; //XX11-XXXX
// disable digital output on channel pins (high impedance)
P0M1 |= 0x30; //XX11-XXXX pines 4 y 5 del puerto 0
P0M2 &= 0xCF; //XX00-XXXX
ENADC = 1;
// Habilita la interrupción
//
//...................................................................................
// INICIALIZA PWM
CNSW0 = 0xFC;
// Frecuencia PWM FPWM a 0X1FC = 508
CNSW1 = 0X01;
//
CPSW0 =
CPSW1 =
CPSW2 =
CPSW3 =
CPSW4 =
PWMCON1
PWMCON0
Page: 3
0xFF;
0x00;
0x00;
0x00;
0x00;
&= 0x00;
|= 0x40;
// PWM0 al 50% CPSW = 0XFF = 255
// Todos las salidas PWM0 totalmente a 1
// Estos son los bit 9 y 10 de los PWM todos a 1
// Frenado desactivado.
// XFER=1 Transferir de registros "Shadows" a los activos
X:\ProyecW\1050460 Brazo oscilante\C51\Primer micro grabado 28 Nov 2005\1050460_main.c 11/28
//
//...................................................................................
// CONFIGURA EL TIMER 0 COMO CONTADOR
TR0 = 0; //Detiene el timer 0
TL0 = TL0_CUENTA; // Pone a cero el valor del contador
TH0 = TH0_CUENTA;
TMOD &= 0xf0; //Limpia los bits de configuración del timer 0 del TMOD
TMOD |= 0x05; // Configura el timer 0 como contador con entrada por el pin T0 modo 0, gat
e = 1
TR0 = 1; // Arranca el timer 0
//
//...................................................................................
// CONFIGURA EL TIMER 1 PARA PERIODO DE MUESTREO
TR1 = 0;
// stop timer 1 if it is already running init initial count value
TL1 = TL1_2mS; // recarga para Ts = 2mS
TH1 = TH1_2mS;
TMOD &= 0x0f;
// clear all timer 1 bits
TMOD |= 0x10;
// select timer mode 1 and options
//
//...................................................................................
// CONFIGURA PUERTOS
// P0.4 entrada A/D (configurado en función específica)
// P0.5 entrada A/D (configurado en función específica)
// P0.3 entrada comparador (configurado en función específica)
// P0.2 entrada comparador (configurado en función específica)
// P0.0 salida comparador (configurado en función específica)
// P0.1 salida PWM (configurado en función específica)
// P1.2 siempre salida open drain (escribir 1 para usar como entrada)
UpDown = 1;// P1^2 = 1 para usar como interrupción
// P1.0 salida rst 2020
P1M1 &= 0xFE; //XXXXXXX0
P1M2 &= 0xFE; //XXXXXXX0
// P1.4 interrupción 1 modo 00 standar 8051 quasibidireccional
P1M1 &= 0xEF; //XXX0XXXX
P1M2 &= 0xEF; //XXX0XXXX
// P1.1 tope 1 modo standar 8051 quasibidireccional
P1M1 &= 0xFD; //XXXXXX0X
P1M2 &= 0xFD; //XXXXXX0X
// P1.3 tope 2 int0 modo standar 8051 quasibidireccional
P1M1 &= 0xF7; //XXXX0XXX
P1M2 &= 0xF7; //XXXX0XXX
//
// Activa interrupciones
EC1 = 0;
// No enable comparator 1 interrupt
EC2 = 0;
// No enable comparator 2 interrupt
IT0 = 1;
// Interrupción externa INT0 activa por flanco
EX0 = 1;
// Activa interr externa INT0 (Angulo Cero)
IT1 = 0;
// Interrupción externa INT1 activa por flanco
EX1 = 0;
// Activa interr externa INT1 Rebasamiento de angulo
EAD = 1;
// Si enable adc interrupt
EA = 1;
// Si permitimos interrupciones activas
CMP1 &= 0xfe;
// clear comparator interrupt flags
EC1 = 1; // Enable comparator 1 interrupt
}
//
//----------------------------------------------------------------------------------//
FUNCIONES PARA MANEJAR EL PWM0
//...................................................................................
// Funcion para cargar el PWM0
// Controla la velocidad del motor. Si PWM_DC es 0 va a 76,6 rps de velocidad
// Si PWM_DC es 255 va a 0 rps de velocidad
//
void carga_pwm0(unsigned int PWM_DC) {
if (PWM_DC == 0) PWM_DC = 1; // Evita que el registro CPSW se ponga a 0
if (PWM_DC >= 255 ) PWM_DC = 255; // El rango es de 1 a 255
CPSW0 = L(PWM_DC);
// Cargo la parte baja del nuevo valor
CPSW4 |= H(PWM_DC); // Ahora translado sólo los dos bits útiles
// XFER=1 Transferir de registros "shadows" a los activos
PWMCON0 |= 0x40;
}
//...................................................................................
// Funcion para hacer una conversión
unsigned char adc_start(unsigned char canal)
{
Page: 4
X:\ProyecW\1050460 Brazo oscilante\C51\Primer micro grabado 28 Nov 2005\1050460_main.c 11/28
// check if conversion is already in progress
if (ADCS || ADCI) return ADC_BUSY;
// clear channel selection bits
ADCON &= 0xFC; //XXXX-XX00
// select channel for conversion
ADCON |= canal; //XXXX-XX00
// start conversion
ADCS = 1;
return ADC_STARTED;
}
//...................................................................................
// Funcion para leer una conversión
void lee_adc(void) interrupt 11 {
switch (ADCON & 0x03 /* canal del convertidor */) {
case VELOCIDAD:
VelocidadMADC = DAC0;
//
Velfloat= 0.4*VelocidadADC + 0.4*VelocidadMADC_1 + 0.4*VelocidadMADC_2;
//
VelocidadMADC = (unsigned char)Velfloat;
//
VelocidadMADC_2 = VelocidadMADC_1;
//
VelocidadMADC_1 = VelocidadMADC;
break;
case ANGULO:
AnguloADC = DAC0;
break;
default:break;
}
// clear interrupt flag to allow another conversion
ADCI = 0;
}
//----------------------------------------------------------------------------------//
REGULADOR
// Función para dar servicio a la int timer 1. Periodo de muestreo Ts
// Esta función calcula la velocidad del eje y la posición angulares
//
void timer1_muestreo(void) interrupt 3 using 1 //(banco de registros)
{
unsigned int ContadorPulsosTs,PulsosTs,CargaPWM;
static unsigned int ContadorPulsosTs_1;
float RpsConsigna,RpsReal,ek,Kek;
static float ak_1,ak;
// feed the watchdog
FEED_WD // Actualiza el WatchDog
// Actualiza variable para procesos de aceleración y frenado
Tiempo++;
// Recarga el timer
TL1 = TL1_2mS ;
TH1 = TH1_2mS ;
// Lee los pulsos dados por el encoder
L(ContadorPulsosTs)=TL0;
H(ContadorPulsosTs)=TH0;
// Calcula los pulsos producidos en un Ts
PulsosTs=ContadorPulsosTs-ContadorPulsosTs_1;
// Almacena la cuenta antigua
ContadorPulsosTs_1=ContadorPulsosTs;
// Como el encoder da 2000 pulsos*revolución
// La Cuenta entre muestreos es pulsos en un Ts.=> Cuenta/Ts es pulsos por segundo pps
// pps/2000 es revoluciones/segundo rps
// Para Ts = 2mS, rps = PulsosTs*/(2000) = Cuenta/4 => rps
// Luego para Ts = 2mS una cuenta es 1/4 rps del eje del motor
// Calcula la velocidad en rpsMOTOR
RpsReal=PulsosTs/4;
// Calcula la velocidad de consigna
// Para pasar de 0->255 a 3.833->38.33 rpsMOTOR (X 60/230)=> 1->10rpm REDUCTORA
RpsConsigna=Velocidad*0.1353+3.8333;
//
// MODULA LA CONSIGNA PARA HACER FRENADO
if (FrenadoRegulador==SI) {
RpsConsigna=RpsConsigna*CoefVel[N_COEFICIENTES-Tiempo]; // Modula la consigna
if(Tiempo > N_COEFICIENTES) { // Si ha pasado el tiempo de frenada
Tiempo = 0;
// Tiempo a cero
ak_1 = 0.0; // Inicializa variables del regulador
ContadorPulsosTs_1 = 0;
if(Sentido == CW) { // Si está parando en la barrera óptica...
PWMCON0 |= 0x02; // Cambia el sentido a CCW
Page: 5
X:\ProyecW\1050460 Brazo oscilante\C51\Primer micro grabado 28 Nov 2005\1050460_main.c 11/28
Sentido = CCW;
// Informa ala sistema que va a girar en sentido antihorario
TL0 = L(RecargaT0); // Recarga el timer 0 para que recorra el angulo correcto
TH0 = H(RecargaT0);
TF0 = 0; //Limpia el flag de interrupción del timer 0 y..
ET0 = 1; //Activa la interrupción del timer 0 para el rebote
// COMPRUEBA PIN MARCHA PARO
PinMarchaParo = 1;
if(PinMarchaParo == 0) {
TR1 = 0; // Para el regulador (Timer 1)
ET1 = 0; //
PinLed = LED_OFF;
MarchaParo = PARO;
}
} //if(Sentido == CW)
else {/*Sentido == CW*/ // Si está parando en el Timer 0....
IE0=0; // Limpia el flag de la int ex 0 del paso por cero
EX0 = 1; // Activa la interrupción del paso por cero
PWMCON0 &= 0xFD;// Sentido del PWM CW
Sentido = CW; // Informa al sistema que está girando en sentido horario
} // else
FrenadoRegulador = NO;
AceleraRegulador = SI;
}// if(Tiempo > 25) ha pasado el tiempo de frenada
} // if (FrenadoRegulador == SI)
// // MODULA LA CONSIGNA PARA HACER ACELERACION
// Proceso de aceleración
if(AceleraRegulador == TRUE ) {
RpsConsigna=RpsConsigna*CoefVel[Tiempo]; // Calculo de la velocidad de consigna
if(Tiempo > N_COEFICIENTES) {
AceleraRegulador = FALSE; // Si ha pasado el tiempo de aceleración
}
}
//.......................................................
// Punto de suma y error
ek=RpsConsigna-RpsReal;
// Error por ganancia integral
Kek=ek*10.0;
// Integrador
ak=TS*Kek+ak_1; // ak/ek=Ts/(z-1)
// Antiwindup
if(ak>24.0)ak=ak_1;
if(ak<0.0)ak=ak_1;
// Actualización de actuación anterior
ak_1=ak;
// ak está en voltios. Pasa a PWM de modo que:
// 0V=>a 255 y 24V=>0. La pendiente es -10.625:
// Pasa de voltios a formato PWM
ak=ak*10.625;
// Convierto a formato int que es lo que entiende el PWM
CargaPWM=(int)ak;
// Para el cambio de signo...
// Necesito complementar
CargaPWM=~CargaPWM;
// Y trimar a 10 bits
CargaPWM=CargaPWM & 0X00FF;
// Cargo el PWM
carga_pwm0(CargaPWM); // Proceso normal
}
//----------------------------------------------------------------------------------// Funcion que da sevicio al paso por cero
void interrupcion_posicion_cero(void) interrupt 0 using 1
{
unsigned int AnguloADCc,CorreccionAngulo; // Angulo ADC corregido con la velocidad
// Este if es solo para poner el eje de la reductora en posición conocida
if(PosicionCero == FALSE) {
PosicionCero = TRUE;
}
// Calcula los valores de recarga del timer
Velocidad = VelocidadMADC;// Consigna de velocidad para la siguiente vuelta
// Programa el timer 0 para que avance 120 - 1.8 = 118.2 grados
// más una corrección que va de 0 grados a máxima velocidad a 1.8 grados a mínima
// de esta forma compensa el revasamiento de ángulo en el transitorio de frenado
// que es de 15 periodos de muestreo
Page: 6
X:\ProyecW\1050460 Brazo oscilante\C51\Primer micro grabado 28 Nov 2005\1050460_main.c 11/28
// CorreccionAngulo = VelocidadMADC/20+1;
CorreccionAngulo = 32-VelocidadMADC/8;
AnguloADCc = AnguloADC+CorreccionAngulo; // Angulo corregido con la velociad
Coef1Recarga = (unsigned int)Coef1*AnguloADCc;
if(AnguloADCc <= 122) { // Angulos menores de 102.45 grados
NVueltasT0 = 1;
RecargaT0=Coef2A-Coef1Recarga;
} // del if
else /* AnguloADCc > 106 */ {
NVueltasT0 = 2;
RecargaT0=Coef2B-Coef1Recarga;
}
// EX0 = 0; // Desactiva la interrupción del paso por cero
Tiempo = 0;
// Empieza a calcular el tiempo
FrenadoRegulador = SI; // Cada vez que llegue a cero comienza a frenar
}
//----------------------------------------------------------------------------------// Función para dar servicio a la int contador 0.
// Se produce una int. cuando el eje de la reductora se ha movido 1/7666 de grado
void timer0_contador(void) interrupt 1 using 2
{
if (NVueltasT0==0) { // Si ha llegado al final del angulo....
Tiempo = 0;
// Empieza a calcular el tiempo
FrenadoRegulador = SI; // Cada vez que llegue a cero comienza a frenar
ET0 = 0; //desactiva la interrupción del timer 0
}
else NVueltasT0--;
}
//----------------------------------------------------------------------------------// Funcion que da sevicio a la int ex 1 del tope de seguridad
void interrupcion_tope_marcha_paro(void) interrupt 2 using 1
{
unsigned int ContadorError;
TR1 = 0; // Para el regulador (Timer 1)
ET1 = 0; // Inhabilita la interrupción del Timer 1
TF1 = 0; // y limpia el flag
carga_pwm0(255); // Parada del motor
while (PinMarchaParo == MARCHA) {
//
while(ADCS || ADCI); adc_start(ANGULO);
FEED_WD // Actualiza el WatchDog
ContadorError ++;
if (ContadorError == 50000) PinLed = !PinLed;
// feed the watchdog
}
rst(); // Hace un reset al sistema
}
//----------------------------------------------------------------------------------// Funcion que da sevicio a la int del comparador 1 que se activa por sobrecorriente
void interrupcion_sobrecorriente(void) interrupt 12 using 3
{
// Si hay sobrecorriente es que se ha estrellado contra la pared de la campana
unsigned int ContadorErrorCorriente;
//
PinOut = !PinOut;
FrenoMotor = 1; // Parada del motor
while (PinMarchaParo == MARCHA) {
ContadorErrorCorriente ++;
if (ContadorErrorCorriente == 10000)
// feed the watchdog
FEED_WD // Actualiza el WatchDog
}
rst(); // Hace un reset al sistema
CMP1 &= 0xfe; // clear interrupt flag
PinLed = !PinLed;
}
//----------------------------------------------------------------------------------// Funcion que da hace un reset del sistema (Función de Mariano Cuenca Alba)
void rst() {
void (code *Reset)();
Reset=0;
Reset();
}
Page: 7
X:\ProyecW\1050460 Brazo oscilante\C51\Primer micro grabado 28 Nov 2005\1050460_main.c 11/28
//________________________________ FIN _________________________________________________
#endif //#ifndef __1050460_main__
Page: 8