标签：
无标签
#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);
}
}
//*************************************************************
// Update pwms www.dangdangdz.com *
//*************************************************************
// Called only once
void update_pwms(void)
{
// TODO check signs
if (control_terms[0]<0) {
CCPR1L = ((-control_terms[0])>>7); // TODO is it only 7 ? signed -> unsigned...
pwm_signs.sign0 = 0;
}
else {
CCPR1L = (control_terms[0]>>7);
pwm_signs.sign0 = 1;
}
if (control_terms[1]<0) {
ECCPR1L = ((-control_terms[1])>>7); // TODO is it only 7 ? signed -> unsigned...
pwm_signs.sign1 = 0;
}
else {
ECCPR1L = (control_terms[1]>>7);
pwm_signs.sign1 = 1;
}
}
void pid_main_loop (void)
{
doMainLoop:
while (!(start_motor[0]||start_motor[1]))
Nop();
status = STATUS_BUSY;
sample_counter = 0;
start_timer();
doSample:
for (index = 0; index < 2; ++index) {
if (start_motor[index]) {
state[index] = STATE_ACCEL;
get_time();
time_offset[index] = time;
signalRegister_LSB[index].traj_complete = 0;
signalRegister_LSB[index].motor_off = 0;
}
start_motor[index] = 0;
}
get_time();
sample_time=time+8; // next sample time. prescaler = 256 -> 8*256 = 2048
get_sample();
++sample_counter;
for (index = 0; index < 2; ++index) {
calculate_desired_position();
calculate_error_term();
}
if (sample_counter == derivative_sample) {
sample_counter = 0;
get_derivative_sample();
}
for (index = 0; index < 2; ++index) {
do_pid();
}
//update pwms : try to synchronise as much as possible
update_pwms();
traj_finished = 1;
for (index = 0; index < 2; ++index) {
if (state[index] == STATE_STOP && control_terms[index] == 0) {
signalRegister_LSB[index].traj_complete = 1;
signalRegister_LSB[index].motor_off = 1;
}
else traj_finished = 0;
}
//DEBUG
if (counter == 1) {
counter = 0;
//SEND_PID_STRING(string_control);
//SEND_PID_INT(control_terms[0]);SEND_PID_BYTE('\t');
//SEND_PID_INT(control_terms[1]);SEND_PID_STRING(new_line);
SEND_PID_STRING(string_des_pos);
SEND_PID_LONG_HEXA(desired_position[0]);SEND_PID_BYTE('\t');
SEND_PID_LONG_HEXA(desired_position[1]);SEND_PID_STRING(new_line);
SEND_PID_STRING(string_des_vel);
SEND_PID_LONG_HEXA(desired_velocity[0]);SEND_PID_BYTE('\t');
SEND_PID_LONG_HEXA(desired_velocity[1]);SEND_PID_STRING(new_line);
SEND_PID_STRING(string_pwm);
SEND_PID_UCHAR_HEXA(PWM0_DUTY_CYCLE);SEND_PID_BYTE('\t');
SEND_PID_UCHAR_HEXA(PWM1_DUTY_CYCLE);SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_des_ask);
//SEND_PID_LONG_HEXA((asked_velocity[0] >> 16));SEND_PID_BYTE('\t');
//SEND_PID_LONG_HEXA((asked_velocity[1] >> 16));SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_int_term);
//SEND_PID_LONG_HEXA(integration_term[0]);SEND_PID_BYTE('\t');
//SEND_PID_LONG_HEXA(integration_term[1]);SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_control_term);
//SEND_PID_LONG_HEXA(control_terms[0]);SEND_PID_BYTE('\t');
//SEND_PID_LONG_HEXA(control_terms[1]);SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_ask_acc);
//SEND_PID_LONG_HEXA(asked_accel[0]>>16);SEND_PID_BYTE('\t');
//SEND_PID_LONG_HEXA(asked_accel[1]>>16);SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_pos_dif);
//SEND_PID_LONG_HEXA(position_difference);
//SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_error_term);
//SEND_PID_INT(error_term[0]);SEND_PID_BYTE('\t');
//SEND_PID_INT(error_term[1]);SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_int_term);
//SEND_PID_LONG_HEXA(integration_term[0]);SEND_PID_BYTE('\t');
//SEND_PID_LONG_HEXA(integration_term[1]);SEND_PID_STRING(new_line);
SEND_PID_STRING(string_state);
SEND_PID_UCHAR_HEXA(state[0]);SEND_PID_BYTE('\t');
SEND_PID_UCHAR_HEXA(state[1]);SEND_PID_STRING(new_line);SEND_PID_STRING(new_line);
}
counter++;
if (!traj_finished) {
while (time < sample_time){
get_time();
}
goto doSample;
}
status = STATUS_MOTOR_OFF | STATUS_TRAJECT_END;
stop_timer();
goto doMainLoop;
}
#endif /*__PID__H*/
系统分类:
单片机 | 用户分类:
编程技术 | 来源:
转贴 | 【推荐给朋友】