#include <libpic30.
h>
#include <p33FJ128MC802.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <float.h>
_FOSCSEL(FNOSC_FRC & IESO_OFF); // Internal FRC start-up without PLL,
// no Two Speed Start-up
_FOSC(FCKSM_CSECMD & OSCIOFNC_OFF & POSCMD_XT); // Clock switch enabled,
// Primarly Oscillator XT
_FWDT(FWDTEN_OFF); // Watchdog Timer disabled
_FPOR(FPWRT_PWR128); // Power-up Timer enabled 128 ms
_FICD(JTAGEN_OFF); // Disable JTAG
// Function prototypes
void configure_pins();
void __attribute__((__interrupt__, no_auto_psv)) _T1Interrupt(void);
int tempRX, pwm, cont=0, i=0, pos,pos2;
unsigned int timePeriod;
float u,vel,vel_ant,ang,ang_ant,velocidad,posf;
int NDP=400;
void InitClock() {
// Configure PLL prescaler, PLL postscaler, PLL divisor: cristal externo 10MHz
PLLFBD=30; // M = 32
CLKDIVbits.PLLPOST=0; // N2 = 2
CLKDIVbits.PLLPRE=0; // N1 = 2
// Initiate Clock Switch to Primary Oscillator with PLL (NOSC = 0b011)
__builtin_write_OSCCONH(0x03);
__builtin_write_OSCCONL(OSCCON | 0x01);
// Wait for Clock switch to occur
while (OSCCONbits.COSC != 0b011);
// Wait for PLL to lock
while(OSCCONbits.LOCK!=1) {};
}
// Capture Interrupt Service Routine
void __attribute__((__interrupt__,auto_psv)) _IC1Interrupt(void)
{
unsigned int t1,t2;
t1=IC1BUF;
t2=IC1BUF;
IFS0bits.IC1IF=0;
if(t2>t1) timePeriod = t2-t1;
else timePeriod = (PR2 - t1) + t2;
if (timePeriod<=0) velocidad=0;
else velocidad=(17386.377/timePeriod); //vel=Fcy/(PPR*Prescaler*timePeriod),
Fcy=40000000
} //PPR=500 (encoder Pulses Per Revolution) //1200
//vel en rev/s
void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)
{
_U2RXIF = 0;
tempRX = U2RXREG;
switch (tempRX) {
//voltajes de pruebas
case 48 : u=3750; //si recibe "0"
break;
case 49 : u=600; //si recibe "1"
break;
case 50 : u=750; //si recibe "2"
break;
case 51 : u=1000; //si recibe "3"
break;
case 52 : u=1250; //si recibe "4"
break;
case 53 : u=1500; //si recibe "5"
break;
case 54 : u=1750; //si recibe "6"
break;
case 55 : u=2000; //si recibe "7"
break;
case 56 : u=2500; //si recibe "8"
break;
case 57 : u=3000; //si recibe "9"
break;
default: u=0;
break;
}
}
int main(void)
{
InitClock(); // This is the PLL settings
configure_pins();
__C30_UART=2;
while (1)
{
// pos1=(pos*1200)/360;
//__delay32(4000000); //0.1 seg delay
}
return 0;
}
// Timer 1 interrupt service routine
void __attribute__((__interrupt__, no_auto_psv)) _T1Interrupt(void)
{
IFS0bits.T1IF = 0; // Clear Timer 1 interrupt flag
LATBbits.LATB6 = ~LATBbits.LATB6; //led para monitorear ejecución de T1int
pos=POS1CNT;
pos2=POS2CNT;
posf=pos;
if(posf>32767){
posf=32768;
}
if(posf<-32767){
posf=-32768;
}
ang_ant=ang;
ang=posf/NDP;
vel_ant=vel;
vel=((ang-ang_ant)/0.02)*(-2*3.141592654);
if(vel>300 ||vel<-300){
vel=vel_ant;
}
printf("%2.3f , %2.3f, %2.3f ,%2.3f ,%2.3f\r\
n",u,posf,ang,vel,velocidad); //Con controlador
if (u>=0) {
pwm=u;
P1DC1 = pwm; //ciclo útil
LATBbits.LATB13 = 1; //bit de dirección positiva
}
if (u<0) {
pwm=u;
P1DC1 = -pwm; //ciclo útil
LATBbits.LATB13 = 0; //bit de dirección negativa
}
}
void configure_pins(){
//Configure Pins as Analog or Digital
AD1PCFGL = 48;
// Configure Digitals I/O directions
TRISB = 0xDF37; //DCF7 FCF7 led6 DCB7
// Configure Remappables Pins
__builtin_write_OSCCONL(OSCCON & ~(1<<6));
RPINR14 = 0x0B0A;
RPINR16 = 0x0908;
RPINR19 = 0x02;
RPOR1 = 0x0500;
RPINR7bits.IC1R=5;
__builtin_write_OSCCONL(OSCCON | (1<<6));
// Initialize QEI 1 Peripheral encoder 4x
QEI1CONbits.QEIM = 0; // Disable QEI Module
QEI1CONbits.CNTERR = 0; // Clear any count errors
QEI1CONbits.QEISIDL = 0; // Continue operation during sleep
QEI1CONbits.SWPAB = 0; // QEA and QEB not swapped
QEI1CONbits.PCDOUT = 0; // Normal I/O pin operation
DFLT1CONbits.CEID = 1; // Count error interrupts disabled
DFLT1CONbits.QEOUT = 0; // Digital filters output disabled for QEn pins
POS1CNT = 0; // Reset position counter
QEI1CONbits.QEIM = 7; // (x4 mode) with position counter reset by match
(MAXxCNT)
MAX1CNT=0xFFFF;
// Initialize QEI 2 eripheral encoder 4x
QEI2CONbits.QEIM = 0; // Disable QEI Module
QEI2CONbits.CNTERR = 0; // Clear any count errors
QEI2CONbits.QEISIDL = 0; // Continue operation during sleep
QEI2CONbits.SWPAB = 0; // QEA and QEB not swapped
QEI2CONbits.PCDOUT = 0; // Normal I/O pin operation
DFLT2CONbits.CEID = 1; // Count error interrupts disabled
DFLT2CONbits.QEOUT = 0; // Digital filters output disabled for QEn pins
POS2CNT = 0; // Reset position counter
QEI2CONbits.QEIM = 7; // (x4 mode) with position counter reset by match
(MAXxCNT)
MAX2CNT=0xFFFF;
// Setup UART
U2BRG = 64; // 38461.53846153846 baud, error=0.16%
_U2RXIE = 1; // Enable Interrupt
U2STA = 0x2400;
U2MODEbits.UARTEN=1;
U2STAbits.UTXEN=1;
// Configure Timer 1: Interrupción cada 20 mseg
T1CON = 0; // Timer reset
IFS0bits.T1IF = 0; // Reset Timer1 interrupt flag
IPC0bits.T1IP = 1; // Timer1 Interrupt priority level=4
IEC0bits.T1IE = 1; // Enable Timer1 interrupt
TMR1 = 0; // Reset Timer 1 counter
PR1 = 12500; // Set the Timer 1 period (max 65535)
// PR1=(Periodo en seg)*Fcy/Prescaler
T1CONbits.TCKPS = 2; // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
T1CONbits.TON = 1; // Turn on Timer 1
// Configure Timer 2: Para InputCapture1
T2CON = 0; // Timer reset
IFS0bits.T2IF = 0; // Reset Timer2 interrupt flag
TMR2 = 0; // Reset Timer 2 counter
PR2 = 64000; // periodo máximo en ticks: (max 65535)
// Vel mínima: 0.15625 rev/s
// Vel máxima: 156.25 rev/s
T2CONbits.TCKPS = 1; // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
// Configure PWM
P1TCONbits.PTMOD = 0b00; //for free running mode
P1TCONbits.PTCKPS = 0b00; //prescale=1:1
P1TCONbits.PTOPS = 0b00; // PWM time base input clock period is T_CY
P1TPER = 2047; // 19531.25 Hz y 12bits para 100% pwm en P1DC1
PWM1CON1bits.PMOD1 = 0;
PWM1CON1bits.PMOD2 = 0;
PWM1CON1bits.PMOD3 = 0;
PWM1CON1bits.PEN1H = 1;
PWM1CON1bits.PEN2H = 0;
PWM1CON1bits.PEN3H = 0;
PWM1CON1bits.PEN1L = 1;
PWM1CON1bits.PEN2L = 0;
PWM1CON1bits.PEN3L = 0;
PWM1CON2bits.IUE = 1;
P1OVDCONbits.POVD3H = 0;
P1OVDCONbits.POVD2H = 0;
P1OVDCONbits.POVD1H = 1;
P1OVDCONbits.POVD3L = 0;
P1OVDCONbits.POVD2L = 0;
P1OVDCONbits.POVD1L = 1;
P1DC1 = 0; // 0% duty cycle: 12bits -> 100% pwm (max is 4095)
P1TMR = 0; // Clear 15-bit PWM timer counter
P1TCONbits.PTEN = 1; // Enable PWM time base
// Initialize the Input Capture Module
IC1CONbits.ICM = 0b00; // Disable Input Capture 1 module
IC1CONbits.ICTMR = 1; // Select Timer2 as the IC1 Time base
IC1CONbits.ICI = 0b01; // Interrupt on every second capture event
IC1CONbits.ICM = 0b011; // Generate capture event on every Rising edge
// Enable Capture Interrupt And Timer2
IPC0bits.IC1IP = 2; // Setup IC1 interrupt priority level
IFS0bits.IC1IF = 0; // Clear IC1 Interrupt Status Flag
IEC0bits.IC1IE = 1; // Enable IC1 interrupt
T2CONbits.TON = 1; // Turn on Timer 2
}
