#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
[Link]=0; // N2 = 2
[Link]=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 ([Link] != 0b011);
// Wait for PLL to lock
while([Link]!=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
[Link] = 0; // Disable QEI Module
[Link] = 0; // Clear any count errors
[Link] = 0; // Continue operation during sleep
[Link] = 0; // QEA and QEB not swapped
[Link] = 0; // Normal I/O pin operation
[Link] = 1; // Count error interrupts disabled
[Link] = 0; // Digital filters output disabled for QEn pins
POS1CNT = 0; // Reset position counter
[Link] = 7; // (x4 mode) with position counter reset by match
(MAXxCNT)
MAX1CNT=0xFFFF;
// Initialize QEI 2 eripheral encoder 4x
[Link] = 0; // Disable QEI Module
[Link] = 0; // Clear any count errors
[Link] = 0; // Continue operation during sleep
[Link] = 0; // QEA and QEB not swapped
[Link] = 0; // Normal I/O pin operation
[Link] = 1; // Count error interrupts disabled
[Link] = 0; // Digital filters output disabled for QEn pins
POS2CNT = 0; // Reset position counter
[Link] = 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;
[Link]=1;
[Link]=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
[Link] = 2; // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
[Link] = 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
[Link] = 1; // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
// Configure PWM
[Link] = 0b00; //for free running mode
[Link] = 0b00; //prescale=1:1
[Link] = 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;
[Link] = 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
[Link] = 1; // Enable PWM time base
// Initialize the Input Capture Module
[Link] = 0b00; // Disable Input Capture 1 module
[Link] = 1; // Select Timer2 as the IC1 Time base
[Link] = 0b01; // Interrupt on every second capture event
[Link] = 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
[Link] = 1; // Turn on Timer 2
}
