main.c

Position PID algorithm control motor by dsPIC

#include "p30f2010.h"
#include "delay.h"
#include "pwm12.h"
#include <stdio.h>
#include "IO.h"
#include "uart.h"
#include "timer1.h"
#include "encoder.h"
#include "adc10.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 int ADCValue; 

/*********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 d_term;
signed long last_error;
signed long PWM;
extern signed long max_pwm_position;
signed long error_pos;
signed long sum_error_pos;
signed long last_error_pos;

void pid_set_position(signed long set_position);
signed long pid_theta_cal(signed long ball_position);
/****************************************/
int main (void)
{
UART1_init();
PWM12_init();
timer1_init();
QEI_init();
adc10_initAN2();
ADCON1bits.ADON = 1;// turn ADC ON
//_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_set_position(1600);
}
return 0;
}

void pid_set_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);
}

signed long pid_theta_cal(signed long ball_position)
{
//signed long err_pos;
//signed long sum_err_pos;
//signed long last_err_pos;
//signed long p_term_pos;
//signed long i_term_pos;
//signed long d_term_pos;

signed long set_position=0;

return set_position;
};