Scribd
Upload
65 views5 pages

BAse

This document contains code for a microcontroller program that controls a motor using PID control. It includes functions to initialize clocks, pins, PWM, input capture and UART communication. The main program configures these elements and enters a loop that calculates motor position, velocity and PID output based on timer interrupts, and sends telemetry data over UART. The motor direction and duty cycle are set based on the PID output voltage signal.

Uploaded by

Jhon Perez
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
65 views5 pages

BAse

This document contains code for a microcontroller program that controls a motor using PID control. It includes functions to initialize clocks, pins, PWM, input capture and UART communication. The main program configures these elements and enters a loop that calculates motor position, velocity and PID output based on timer interrupts, and sends telemetry data over UART. The motor direction and duty cycle are set based on the PID output voltage signal.

Uploaded by

Jhon Perez
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 5

#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
}

You might also like