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*/