Está en la página 1de 6

C:\Users\user\Desktop\backup usb 081215\MT418\configs\proyecto.c sbado, 12 de diciembre de 2015 10:49 a.m.

#include "DSP2833x_Device.h"
#include "DSP2833x_Examples.h"
#include "lab7.h"
#include "monitorF28335.h"

// Perodo de muestreo
double Ts;

// Seales relacionadas al control


double error, error_anterior;
double senal, senal_integral, senal_derivativa, senal_proporcional;
long int senal_pwm_entera;
double Kp, Ki, Kd;
double res_encoder = 108.0; // resolucin del encoder
long int pulsos, pulsos_ant;
double pulsos_double,theta_r, theta;
int theta_r_int;
long int cont_int;

void tmr0_isr(void);

void main(void)
{
/*****************************************************/
/*CONFIGURACION INICIAL*/
/*****************************************************/

DINT;
DRTM;
InitSysCtrl();
EALLOW;
SysCtrlRegs.HISPCP.all = 0x3;
EDIS;

/*****************************************************/
/*CONFIGURACION PWM 1*/
/*****************************************************/
// Se usa solo el PWM1B porque el PWM1A necesita de ms configuraciones
// En lugar de usar el PWM1A se usar el PWM2B

EALLOW;
// Detiene los EPWM
SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 0;
EDIS;

// Multiplexor de GPIO -> EPWMA1 (O)


EALLOW;
GpioCtrlRegs.GPAPUD.bit.GPIO0 = 0; // Enable pull-up on GPIO0 (EPWM1A)
GpioCtrlRegs.GPAPUD.bit.GPIO1 = 0; // Enable pull-up on GPIO1 (EPWM1B)
GpioCtrlRegs.GPAMUX1.bit.GPIO0 = 1; // 0->GPIO, x->(revisar datasheet)
GpioCtrlRegs.GPAMUX1.bit.GPIO1 = 1; // 0->GPIO, x->(revisar datasheet)
EDIS;

// Tasa del prescalador del SYSCLKOUT


// TBCLK = SYSCLKOUT/(1*10) = 15Mhz
EPwm1Regs.TBCTL.bit.HSPCLKDIV = 5;
EPwm1Regs.TBCTL.bit.CLKDIV = 0;
// Determina el periodo 15000 TBCLKs
// T = (TBPRD+1)*(1/TBCLK) = 15000*1/(15Mhz) = 1ms
EPwm1Regs.TBPRD = 14999;

// La fase es 0
EPwm1Regs.TBPHS.half.TBPHS = 0x0000;
// Deshabilitar carga de fase
-1-
C:\Users\user\Desktop\backup usb 081215\MT418\configs\proyecto.c sbado, 12 de diciembre de 2015 10:49 a.m.
EPwm1Regs.TBCTL.bit.PHSEN = 0;

// Contador inicio
EPwm1Regs.TBCTR = 0x0000;

// Valor inicial de comparacin


EPwm1Regs.CMPB = 0;

// Configurar modo de conteo


// Contar hacia arriba y al llegar al periodo igual a cero
EPwm1Regs.TBCTL.bit.CTRMODE = 0;

// Configurar shadowing
// Modo shadow (la escritura se hace sobre la sombra)
EPwm1Regs.CMPCTL.bit.SHDWAMODE = 0;
// La sincronizacin sombra->real se hace cuando el CTR es 0
EPwm1Regs.CMPCTL.bit.LOADAMODE = 0;

// Configurar acciones
// Contador es CMPA -> Pone a 0 la senal
EPwm1Regs.AQCTLB.bit.CBU = 1;
// Contador es 0 -> Pone a 1 la senal
EPwm1Regs.AQCTLB.bit.ZRO = 2;

EPwm1Regs.AQCTLB.bit.CBD = 0;

// Deshabilita el mdulo de alta resolucin (HRPWM)


EPwm1Regs.HRCNFG.bit.all = 0;
EPwm1Regs.HRCNFG.bit.EDGMODE = 0;

/*****************************************************/
/*CONFIGURACION PWM 2*/
/*****************************************************/

// Multiplexor de GPIO -> EPWM2 (O)


EALLOW;
GpioCtrlRegs.GPAPUD.bit.GPIO2 = 0;
GpioCtrlRegs.GPAPUD.bit.GPIO3 = 0;
GpioCtrlRegs.GPAMUX1.bit.GPIO2 = 1;
GpioCtrlRegs.GPAMUX1.bit.GPIO3 = 1;
EDIS;

// Tasa del prescalador del SYSCLKOUT


// TBCLK = SYSCLKOUT/(1*10) = 15Mhz
EPwm2Regs.TBCTL.bit.HSPCLKDIV = 5;
EPwm2Regs.TBCTL.bit.CLKDIV = 0;
// Determina el periodo 15000 TBCLKs
// T = (TBPRD+1)*(1/TBCLK) = 15000*1/(15Mhz) = 1ms
EPwm2Regs.TBPRD = 14999;

// La fase es 0
EPwm2Regs.TBPHS.half.TBPHS = 0x0000;
// Deshabilitar carga de fase
EPwm2Regs.TBCTL.bit.PHSEN = 0;

// Contador inicio
EPwm2Regs.TBCTR = 0x0000;

// Valor inicial de comparacin


EPwm2Regs.CMPB = 0;

// Configurar modo de conteo


// Contar hacia arriba y al llegar al periodo igual a cero
EPwm2Regs.TBCTL.bit.CTRMODE = 0;
-2-
C:\Users\user\Desktop\backup usb 081215\MT418\configs\proyecto.c sbado, 12 de diciembre de 2015 10:49 a.m.

// Configurar shadowing
// Modo shadow (la escritura se hace sobre la sombra)
EPwm2Regs.CMPCTL.bit.SHDWAMODE = 0;
// La sincronizacin sombra->real se hace cuando el CTR es 0
EPwm2Regs.CMPCTL.bit.LOADAMODE = 0;

// Configurar acciones
// Contador es CMPA -> Pone a 0 la senal
EPwm2Regs.AQCTLB.bit.CBU = 1;
// Contador es 0 -> Pone a 1 la senal
EPwm2Regs.AQCTLB.bit.ZRO = 2;

EPwm2Regs.AQCTLB.bit.CBD = 0;

EALLOW;
// Sincroniza todos los EPWM y los activa
SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 1;
EDIS;

/*****************************************************/
/*CONFIGURACIN DEL EQEP*/
/*****************************************************/

// Configuracin de multiplexores para entradas EQEP


EALLOW;
GpioCtrlRegs.GPAMUX2.bit.GPIO20 = 1; // EQEP1A (I)
GpioCtrlRegs.GPAMUX2.bit.GPIO21 = 1; // EQEP1B (I)
GpioCtrlRegs.GPAMUX2.bit.GPIO22 = 1; // EQEP1S (I/O)
GpioCtrlRegs.GPAMUX2.bit.GPIO23 = 1; // EQEP1I (I/O)
EDIS;

// Conteo en modo cuadratura


EQep1Regs.QDECCTL.bit.QSRC = 0;
// Deshabilitar salida de comparacin de posicin
EQep1Regs.QDECCTL.bit.SOEN = 0;
// Uso de INDEX como salida de comparacin
EQep1Regs.QDECCTL.bit.SPSEL = 0;
// Duplica la resolucin del encoder (cuenta los 2 flancos)
EQep1Regs.QDECCTL.bit.XCR = 0;
// Deshabilita la puerta de entrada de INDEX
EQep1Regs.QDECCTL.bit.IGATE = 0;
// Polaridad normal de QEPA, QEPB, QEPI, QEPS
EQep1Regs.QDECCTL.bit.QAP = 0;
EQep1Regs.QDECCTL.bit.QBP = 0;
EQep1Regs.QDECCTL.bit.QIP = 0;
EQep1Regs.QDECCTL.bit.QSP = 0;
// El contador de posicin se resetea (a 0) con un evento en INDEX
EQep1Regs.QEPCTL.bit.PCRM=0;
// Inicializa el contador de posicin cuando hay un flanco de subida en el STROBE
EQep1Regs.QEPCTL.bit.SEI=2;
// Nada cuando evento de INDEX ocurre
EQep1Regs.QEPCTL.bit.IEI=0;
// Hacer nada, si fuera 1 inicializa el contador de posicin
EQep1Regs.QEPCTL.bit.SWI=0;
// QPOSCNT->QPOSSLAT, cuando flanco de subida en STROBE
EQep1Regs.QEPCTL.bit.SEL=0;
// Reservado
EQep1Regs.QEPCTL.bit.IEL=0;
// Habilitar el contador
EQep1Regs.QEPCTL.bit.QPEN=1;
// Modo de captura (leer datasheet)
EQep1Regs.QEPCTL.bit.QCLM=0;
// Deshabilitar el EQEP Unit Timer
-3-
C:\Users\user\Desktop\backup usb 081215\MT418\configs\proyecto.c sbado, 12 de diciembre de 2015 10:49 a.m.
EQep1Regs.QEPCTL.bit.UTE=1;
// Deshabilitar el EQEP WDT Timer
EQep1Regs.QEPCTL.bit.WDE=0;
// Valor mximo del contador de posicin
EQep1Regs.QPOSMAX=0x00ffffff;
// Perodo del EQEP Unit Timer
EQep1Regs.QUPRD=0x0000ffff;
// Valor de inicializacin del contador de posicin
EQep1Regs.QPOSINIT=0;

/*****************************************************/
/*VALORES A INICIALIZAR*/
/*****************************************************/

// Perodo de muestreo
Ts = 0.001;
// Constantes del controlador
Kp = 0.4;
Ki = 1.5;
Kd = 0.0;
// Seal integral
senal_integral = 0.0;
// Seales de referencia (las dos son equivalentes)
// Double: esta se usa para las operaciones de clculo de error
// y seal de control
theta_r = 500.0;
// Entera: esta se usa para recibir el valor de la PC
theta_r_int = 500;
cont_int = 0;

/*****************************************************/
/*CONFIGURACION TIMER*/
/*****************************************************/

// Inicializa los CpuTimers


InitCpuTimers();
// Configura frecuencia y nmero de conteos: 150MHz, 1000000uSeg = 1segundo
ConfigCpuTimer(&CpuTimer0, 150, 1000);
// Habilita interrupcin
CpuTimer0Regs.TCR.bit.TIE = 1;
// 'Enciende' el timer
CpuTimer0Regs.TCR.bit.TSS = 0;

/*****************************************************/
/*CONFIGURACION INTERRUPCIONES*/
/*****************************************************/

InitPieCtrl();
IER = 0x0000;
IFR = 0x0000;
InitPieVectTable();

EALLOW;
// Direcciona la interrupcin TINT0 hacia la direccin &cpu_timer0_isr
PieVectTable.TINT0 = &tmr0_isr;
EDIS;

// Habilitar interrupcin N6 de grupo 1 (INT1.6)


PieCtrlRegs.PIEIER1.bit.INTx7 = 1; // a nivel perifrico
IER |= M_INT1; // a nivel CPU

/*****************************************************/
/*INICIALIZACIN DEL MOVIMIENTO DE LA ARTICULACIN*/
/*****************************************************/
-4-
C:\Users\user\Desktop\backup usb 081215\MT418\configs\proyecto.c sbado, 12 de diciembre de 2015 10:49 a.m.

// Valor de contador inicial (que no sea cero)


EQep1Regs.QPOSCNT=1;
// Se le da al motor un movimiento lento (casi 440 RPM)
EPwm1Regs.CMPB = 1500;
// Esperar al fin de carrera (fin de carrera->STROBE->QPOSCNT=0)
do
{

}
while(EQep1Regs.QPOSCNT!=0);
// Detiene el motor
EPwm1Regs.CMPB = 0;

/*****************************************************/
/*CONFIGURACION COMUNICACIN SERIAL*/
/*****************************************************/

// Inicializa los pines SCIA para ser usados con su funcin SCI
InitSciaGpio();
// Inicializacin de los registros del mdulo de comunicacin serial
IniciarSerial();

/*****************************************************/
/*CONFIGURACION FINAL*/
/*****************************************************/

EINT; //habilita interrupciones


ERTM;

/*****************************************************/
/*BUCLE INFINITO*/
/*****************************************************/

do
{
IteraSerial();
}
while(1); //Itera infinitamente
}

interrupt void tmr0_isr(void)


{
// Pulsos ledos con respecto a la referencia
pulsos = EQep1Regs.QPOSCNT;
// Conversin de pulsos a grados sexagesimales
pulsos_double=pulsos;
theta = pulsos_double/(4*res_encoder)*360.0;
// Error
theta_r=theta_r_int;
error = theta_r-theta;

// Mtodo del trapecio para la componente integral


senal_integral = senal_integral+(error+error_anterior)*Ts/2.0;
// Suma de todas las componentes (comp. derivativa por Euler)
senal = Kp*error+Kd*(error-error_anterior)/Ts+Ki*senal_integral;
// Escalamiento de angulo a PWM
/* Equivalencias:
3V en el actuador <-> 2200/60*360*0.001 por perodo de muestreo
3V en el actuador <-> CMPA = 15000 - 1 */
senal_pwm_entera = senal/(2200/60*360*0.001)*15000;

// Limitador de velocidad (que tambin sirve como anti-windup)


if (senal_pwm_entera>8000)
-5-
C:\Users\user\Desktop\backup usb 081215\MT418\configs\proyecto.c sbado, 12 de diciembre de 2015 10:49 a.m.
{
senal_pwm_entera=8000;
}
else if (senal_pwm_entera<-8000)
{
senal_pwm_entera=-8000;
}

// Salida
if (senal_pwm_entera>0)
{
EPwm1Regs.CMPB = senal_pwm_entera;
EPwm2Regs.CMPB = 0;
}
else
{
EPwm2Regs.CMPB = -senal_pwm_entera;
EPwm1Regs.CMPB = 0;
}

// Actualizaciones
error_anterior = error;

pulsos_ant = pulsos;

// Acknowledge interrupt to PIE


PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
return;
}

-6-