main.c

PID control speed of motor using dspIC30f2010

#include "p30f2010.h"
#include "delay.h"
#include "pwm12.h"
#include <stdio.h>
#include "IO.h"
#include "uart.h"
#include "timer1.h"
#include "timer2.h"
#include "encoder.h"
#include "incap.h"


_FOSC(CSW_FSCM_OFF & XT_PLL16);
//XTAL 7.3728MHz
//Fcy=29 491 200 Hz
_FWDT(WDT_OFF);    //Turn off Watchdog Timer
_FBORPOR(MCLR_EN & PWRT_OFF & RST_PWMPIN & PWMxH_ACT_HI & PWMxL_ACT_HI);
 /* Enable MCLR reset pin and turn off the power-up timers. 
PWM output active low on reset*/
 
_FGS(CODE_PROT_OFF); //Disable Code Protection

extern unsigned char tx_buffer[7];
extern unsigned char rx_data;
extern unsigned char command_ID;
extern unsigned int KP_coeff;
extern unsigned int KI_coeff;
extern unsigned int KD_coeff;
extern signed long encoder_value;//current value of encoder
extern unsigned long cap_value;//
extern signed long set_encoder_value;
/***************PID variables*********************/
#define PID_POSITION_SCALE_FACTOR	1024
#define I_POSITION_SCALE_FACTOR	4096
#define MAX_PWM_POSITION	2800
signed long error;
signed long sum_error;
signed long p_term;
signed long i_term;
signed long PWM;
extern signed long max_pwm_position;
void pid_position(signed long set_position);
/************************************************/
int main (void)
{
UART1_init();
PWM12_init();
timer1_init();
timer2_init();
QEI_init();
cap2_init();
//_TRISB2=OUTPUT;
///_TRISD1=INPUT; never config indepentdent like this to avoid conflict IO
//set_PWM12(0);
while (1)
{
//read_encoder();
set_PWM12(KD_coeff);
//delay_ms(10);
//pid_position(1600);
}
return 0;
}

void pid_position(signed long set_position)
{
read_encoder();
error=set_position-encoder_value;
sum_error=sum_error+error;

p_term=error*KP_coeff;
i_term=sum_error*KI_coeff/I_POSITION_SCALE_FACTOR;
if (i_term>MAX_LONG) i_term=MAX_LONG;
if (i_term<-MAX_LONG) i_term=-MAX_LONG;
//PWM output
	PWM=(p_term+i_term)/PID_POSITION_SCALE_FACTOR;
	if (PWM>max_pwm_position) set_PWM12(max_pwm_position);
	else if (PWM<-max_pwm_position) set_PWM12(-max_pwm_position);
	else set_PWM12(PWM);
}