pid源代碼
#ifndef __PID__H
#define __PID__H
#pragma code
#ifdef DEBUG_PID
#define SEND_PID_STRING(STRING) send_string((STRING))
#define SEND_PID_UCHAR_HEXA(CHR) send_unsigned_char_as_hexa((CHR))
#define SEND_PID_INT(INT) send_int_as_hexa((INT))
#define SEND_PID_LONG_HEXA(LNG) send_long_as_hexa((LNG))
#define SEND_PID_BYTE(CHR) send_byte((CHR))
static char string_state[]="State: ";
static char string_control[]="CTRL_Term: ";
static char string_error_term[]="Err Term: ";
static unsigned short counter = 0;
static char new_line[]="\r\n";
static char string_int_term[]="Int Term: ";
static char string_pos_dif[]="Pos Dif: ";
static char string_des_vel[]="Des. Veloc:"; ? static char string_des_ask[]="Ask. Veloc:";
static char string_ask_acc[]="Ask. Accel:";
static char string_des_pos[]="Des. Pos:";
static char string_int_term_of[]="Int.Term. OverFlow!";
static char string_control_overflow[]="Cont.Term Overflow!";
static char string_control_term[]="Ctrl. Term:";
static char string_pwm[]="PWM: ";
#else
#define SEND_PID_STRING(STRING) 1
#define SEND_PID_UCHAR_HEXA(CHR) 1
#define SEND_PID_INT(INT) 1
#define SEND_PID_LONG_HEXA(LNG) 1
#define SEND_PID_BYTE(CHR) 1
#endif
#pragma udata
unsigned char traj_finished;
// Buffer to copy the current mesured position
signed long mesured_position_buf[2];
#pragma code
/********************************************************/
/* Timer stuff */
/********************************************************/
void init_timer (void)
{
timerH[0]=timerH[1]=0;
time_ptr = (unsigned char *)&time;
INTCONbits.TMR0IE = 1;
// Inlined, we need to change TMR0H before TMR0L (see datasheet)
TMR0H = 0;
TMR0L = 0;
T0CON = 0x07;// 16 bit timer, 1:256 prescaler, not started
INTCON2bits.TMR0IP =1; //TMR0 High Priority
}
void start_timer (void)
{
T0CONbits.TMR0ON = 1;
}
void stop_timer (void)
{
T0CONbits.TMR0ON = 0;
TMR0H = 0;
TMR0L = 0;
timerH[0]=timerH[1]=0;
}
//********************************************************
// Sampling stuff *
//********************************************************
void get_time (void)
{
// TODO do we need 32bits precision ?
// In that case we need to enable the tmr0 overflow interrupt,
// and increment a 16bits-wide counter in the interrupt routine.
//
*time_ptr = TMR0L;
*(time_ptr + 1) = TMR0H;
*(time_ptr + 2) = timerH[0];
*(time_ptr + 3) = timerH[1];
}
void get_sample (void)
{
// copy the mesured position to a buffer to calculate pid
mesured_position_buf[0] = mesured_position[0];
mesured_position_buf[1] = mesured_position[1];
}
void get_derivative_sample (void)
{
old_derivative[0] = new_derivative[0];
old_derivative[0] = new_derivative[0];
new_derivative[0] = error_term[0];
new_derivative[1] = error_term[1];
derivative_term[0] = new_derivative[0] - old_derivative[0];
derivative_term[1] = new_derivative[1] - old_derivative[1];
}
//*********************************************************
//* Pid stuff *
//*********************************************************
void desired_position_accel (void)
{
long temps = (time-time_offset[index]);
desired_velocity[index] = asked_accel[index]*(temps >> 16);
if(desired_velocity[index] >= (asked_velocity[index] >> 16)) {
state[index] = STATE_CNST;
desired_position[index] = asked_velocity[index] * (temps >> 17);
stop_distance[index] = desired_position[index];
time_to_stop[index] = (time-time_offset[index]);
}
else
desired_position[index] = desired_velocity[index] * (temps >> 1);
}
void desired_position_cnst (void)
{
long temps = (time-time_offset[index]);
desired_position[index] = stop_distance[index] +
asked_velocity[index]*((temps - time_to_stop[index]) >> 16);
if (desired_position[index] >= asked_position[index] - stop_distance[index] ) {
time_to_stop[index] = temps;
stop_distance[index] = desired_position[index];
state[index] = STATE_DECEL;
}
}
void desired_position_decel (void)
{
long temps = (time-time_offset[index]) - time_to_stop[index];
desired_position[index] = stop_distance[index] +
asked_velocity[index]*(temps >> 16)- asked_accel[index] * (temps
>> 16) * (temps>> 1);
if (desired_position[index] >= asked_position[index]) {
desired_position[index] = asked_position[index];
state[index] = STATE_STOP;
}
}
void calculate_desired_position (void)
{
switch (state[index]) {
case STATE_ACCEL:
desired_position_accel();
break;
case STATE_CNST:
desired_position_cnst();
break;
case STATE_DECEL:
desired_position_decel();
break;
default:
return;
}
}
void calculate_error_term (void)
{
position_difference = desired_position[index] - mesured_position_buf[index];
error_term[index] = ( int ) position_difference ;
// if position_difference not on 16 bits, satursate error_term...
// TODO check that
if (error_term[index] != position_difference)
error_term[index] = (position_difference < 0) ? -0x7fff : 0x7fff;
integration_term_buffer = integration_term[index] + error_term[index];
integration_term[index] = (short long)(integration_term_buffer);
if(integration_term[index] != integration_term_buffer){
integration_term[index] = (integration_term_buffer < 0) ? -0x7fffff:0x7fffff ;
//DEBUG
//SEND_PID_STRING(string_int_term_of);
}
// TODO The integration limit is not yet done
}
void do_pid (void)
{
if (state[index] == STATE_STOP) {
if (desired_position[index] >= mesured_position[index]) {
control_terms[index]=0;
return;
}
}
control_term_buffer = coef_prop*error_term[index] +
coef_deriv*(derivative_term[index]) + coef_integ*( (int)
(integration_term[index] >> 8) );
control_terms[index] = (int)(control_term_buffer);
if(control_terms[index] != control_term_buffer){
control_terms[index] = (control_term_buffer < 0)? -0x7fff : 0x7fff;
//DEBUG
//SEND_PID_STRING(string_control_overflow);
}
}
goto doSample;
}
status = STATUS_MOTOR_OFF | STATUS_TRAJECT_END;
stop_timer();
goto doMainLoop;
}
#endif /*__PID__H*/
.png)
2.png)
