/****************************************************************************
DC MOTOR SPEED CONTROL
PICUINO APPLICATION BOARD
https://sites.google.com/site/picuino
****************************************************************************/
#include <p18cxxx.h>
#include <stdio.h>
/****************************************************************************
HARDWARE DEFINITIONS
****************************************************************************/
#define FOSC 20000000
#define BAUD 57600
#define DCMOT_TRIS_AN0 TRISAbits.TRISA0
#define DCMOT_TRIS_AN1 TRISAbits.TRISA1
#define DCMOT_TRIS_AN2 TRISAbits.TRISA2
#define DCMOT_TRIS_ENABLE TRISCbits.TRISC0
#define DCMOT_ENABLE LATCbits.LATC0
#define DCMOT_TRIS_MOT1 TRISCbits.TRISC1
#define DCMOT_MOT1 LATCbits.LATC1
#define DCMOT_TRIS_MOT2 TRISCbits.TRISC2
#define DCMOT_MOT2 LATCbits.LATC2
#define DCMOT_TRIS_LED1 TRISBbits.TRISB7
#define DCMOT_LED1 LATBbits.LATB7
/****************************************************************************
INTERRUPTS
****************************************************************************/
#define TMR0_COUNT (FOSC/((unsigned long)4*4*250*1000)) // Number of carrys per second
unsigned char milli_second, sys_clk;
void isr_main(void);
#pragma code HIGH_INTERRUPT_VECTOR = 0x0008
void High_ISR (void) {
_asm goto isr_main _endasm
}
#pragma interrupt isr_main
void isr_main(void) {
if (INTCONbits.TMR0IF==1) { // if TIMER0 overflow interrupt
INTCONbits.TMR0IF = 0; // Clear Timer0 interrupt flag
TMR0L += 6+3;
sys_clk--;
if (sys_clk==0) {
sys_clk = TMR0_COUNT;
if (++milli_second == 10) {
milli_second = 0;
}
}
}
}
/*
Initialize isr routine and Timer0
*/
void isr_init(void) {
INTCON = 0b11100000; // Enable interrupts
T0CON = 0b11000001; // Prescaler = 4
TMR0L = 6+3;
sys_clk = TMR0_COUNT;
milli_second = 0;
}
/****************************************************************************
ADC FUNCTIONS
****************************************************************************/
/*
Read Analog Input AN0 or AN1
*/
unsigned short adc_read(unsigned char channel) {
union {
struct {
unsigned char lob;
unsigned char hib;
};
unsigned short word;
} adc_val;
// Configure ADC
ADCON0 = 0b00000001 + (channel<<2); // ADC on
ADCON1 = 0b00001100; // Analog inputs = AN0, AN1, AN2
ADCON2 = 0b10010101; // TAD
// Make an ADC conversion
ADCON0bits.GO = 1; // ADC Hold and Start conversion
while (ADCON0bits.GO==1); // Conversion
// Configure ADC
ADCON0 = 0b00000000; // ADC off
ADCON1 = 0b00001111; // All pins to digital inputs
// Return ADC conversion
adc_val.lob = ADRESL;
adc_val.hib = ADRESH;
return adc_val.word;
}
/****************************************************************************
RS232 FUNCTIONS
****************************************************************************/
/*
Initialize USART for RS232 comunications
*/
void rs232_init(void) {
PIE1bits.TXIE = 0; // Disable RS232 interrupts
BAUDCONbits.BRG16 = 0; // BRG16: 16-Bit Baud Rate Register Enable bit
SPBRGH = 0;
SPBRG = (FOSC/(16*BAUD))-1; // Real Baud = FOSC/(16*(SPBRG+1))
TXSTA = 0b00100110;
RCSTA = 0b10010000;
TRISCbits.TRISC6 = 0; // Enable TX output
}
/*
Send char to USART
*/
void rs232_putc(char c) {
TXREG = c;
}
/*
Return Hexadecimal value of a nibble
*/
char Hexdec(char nibble) {
nibble &= 0x0F;
if (nibble>9)
return ('A'-10) + nibble;
return '0' + nibble;
}
/****************************************************************************
PWM FUNCTIONS
****************************************************************************/
/*
Initialize and configure PWM module
D = Duty cycle in range 0 .. 200
*/
void pwm1_set(int D) {
if (D > 1000) D = 1000;
if (D<0) {
D = 0;
}
DCMOT_MOT1 = 1;
CCP1CON = 0b00001100;
if (D & 1) CCP1CON |= (1<<4);
if (D & 2) CCP1CON |= (1<<5);
D = D>>2;
CCPR1L = D;
}
void pwm1_init(void) {
T2CON = 0b00000100; // Timer2 on, prescaler = 1
PR2 = 250; // PWM Period = [(PR2) + 1] • TCY •(TMR2 Prescale Value)]
DCMOT_MOT1 = 0;
DCMOT_MOT2 = 0;
DCMOT_ENABLE = 0;
DCMOT_TRIS_MOT1 = 0;
DCMOT_TRIS_MOT2 = 0;
DCMOT_TRIS_ENABLE = 0;
}
/****************************************************************************
PID CONTROL
****************************************************************************/
#define DCMOT_RM 8 // Motor resistance in ohm
#define DCMOT_RS 100 // I sense resistance in milliohm
#define DCMOT_IMAX 2000 // Max motor current in milliamp
#define VDD 12
#define KI 100
#define PWM_STEPS 1000
#define INTEG_MAX 30000
signed int Vref, Vmot, Err, Err_old, Integ, Vo, Vo2, Imot, Imot_old;
void pid_control(void) {
char sample;
//*********************************
// BACK EMF SENSE
//*********************************
while (milli_second == 0) {
DCMOT_ENABLE = 0;
sample = 0;
}
if (milli_second == 1 && sample == 0) {
Vmot = adc_read(0);
DCMOT_ENABLE = 1;
sample = 1;
}
//*********************************
// PID Speed control
//*********************************
if (sample==1) {
sample = 1;
Vref = adc_read(2)/2; // External pot
Err = Vref - Vmot;
Integ += (Err + Err_old)/2;
Err_old = Err;
if (Integ>INTEG_MAX) Integ = INTEG_MAX;
if (Integ<-INTEG_MAX) Integ = -INTEG_MAX;
Vo = Vref + Integ/KI; // Integral control
}
//*********************************
// Current Control
//*********************************
Imot = adc_read(1);
Imot += adc_read(1);
Imot = Imot*250/DCMOT_RS; // Imot in milliamp
if (Imot > DCMOT_IMAX) {
DCMOT_LED1 = 1; // Max current led = 1
if (Vmot>Vref)
pwm1_set(Vmot);
else
pwm1_set(Vmot + ((long)DCMOT_IMAX*DCMOT_RM*PWM_STEPS/((long)VDD*1000)));
}
else {
DCMOT_LED1 = 0;
Vo2 = Vo + (long)Imot*DCMOT_RM*PWM_STEPS/(1000*VDD);
pwm1_set(Vo2);
}
Imot_old = Imot;
}
void pid_init() {
// PID inputs
CMCON = 0b00000111;
ADCON1 = 0b00001111;
DCMOT_TRIS_AN0 = 1;
DCMOT_TRIS_AN1 = 1;
DCMOT_TRIS_AN2 = 1;
// PID values
Integ = 0;
Err_old = 0;
}
/****************************************************************************
MAIN PROGRAM
****************************************************************************/
void main(void) {
char send;
//*********************************
// Initialize
//*********************************
rs232_init();
isr_init();
pwm1_init();
pid_init();
TRISB &= 0x0F;
fprintf (_H_USART
, "\r\nSystem OK.\r\n");
//*********************************
// DC MOTOR SPEED CONTROL
//*********************************
send = 0;
while(1) {
//*********************************
// PID control
//*********************************
pid_control();
//continue;
//*********************************
// Print PID values
//*********************************
#define PUTC(A, B) case A: rs232_putc(B); break;
if (PIR1bits.TXIF == 1) {
LATBbits.LATB6 = 1;
switch(send++) {
PUTC(1, '\r')
PUTC(2, '\n')
PUTC(3, 'V')
PUTC(4, 'm')
PUTC(5, '=')
PUTC(6, Hexdec(Vmot>>12))
PUTC(7, Hexdec(Vmot>>8))
PUTC(8, Hexdec(Vmot>>4))
PUTC(9, Hexdec(Vmot))
PUTC(10, '\t')
PUTC(11, 'I')
PUTC(12, 'm')
PUTC(13, '=')
PUTC(14, Hexdec(Imot>>12))
PUTC(15, Hexdec(Imot>>8))
PUTC(16, Hexdec(Imot>>4))
PUTC(17, Hexdec(Imot))
PUTC(18, '\t')
PUTC(19, 'I')
PUTC(20, 'n')
PUTC(21, 't')
PUTC(22, 'e')
PUTC(23, 'g')
PUTC(24, '=')
PUTC(25, Hexdec(Integ>>12))
PUTC(26, Hexdec(Integ>>8))
PUTC(27, Hexdec(Integ>>4))
PUTC(28, Hexdec(Integ))
PUTC(29, '\t')
PUTC(30, 'V')
PUTC(31, 'o')
PUTC(32, '=')
PUTC(33, Hexdec(Vo2>>12))
PUTC(34, Hexdec(Vo2>>8))
PUTC(35, Hexdec(Vo2>>4))
PUTC(36, Hexdec(Vo2))
PUTC(37, '\t')
default: send = 1;
} //End switch
LATBbits.LATB6 = 0;
} //End if
} //End while
}
