main.c

柴油发动机的电控PID代码。平台freescale的MC9S12DZ60单片机。核心算法

      button1_flag = 1;  //set
    }
  } 
  else
  {
    button1_flag = 0;  //clear
    PORTB_BIT6 = 0; //enable no rotation protection
  }    
  
  if(PORTA_BIT1 == 0)   //按钮1：停车
  {
    delay(); //delay 5ms,prevent twitter
    if(PORTA_BIT1 == 0)
      button2_flag = 1;  //set
  } 
  else
    button2_flag = 0;  //clear      
  
  if(PORTA_BIT2 == 0)   //按钮2：待定义
  {
    delay(); //delay 5ms,prevent twitter
    if(PORTA_BIT2 == 0)
      button3_flag = 1;  //set
  } 
  else
    button3_flag = 0;  //clear  
    
   if(PORTA_BIT3 == 0)   //按钮4：待定义
  {
    delay(); //delay 5ms,prevent twitter
    if(PORTA_BIT3 == 0)
      button4_flag = 1;  //set
  } 
  else
    button4_flag = 0;  //clear    
    
  /*if(first_ovrot_flag == 1)  //first over rotation
  {
    set_rot = 650;  //拉至怠速
    //open_ovrot();   //打开超速保护
  } */

 
  /*if(no_mrot_flag == 1)  //no main rotation
  {
    PORTA_BIT5 = 1;
  }
  else
  {
    PORTA_BIT5 = 0;
  }  

  if(no_srot_flag == 1)  //no sub rotation
  {
    PORTA_BIT6 = 1;
  }
  else
  {
    PORTA_BIT6 = 0;
  } */ 
}

/****************************************************************************
* 函数名称:get_rotation()																												
* 函数功能:采集反馈转速																													
* 入口参数:无																															
* 出口参数:无																																
*****************************************************************************/
void get_rotation(void) 
{
  uint t1,t2;
  //long int rotation;
  //uint rota;
  //rotation = pre_fb_rot;
  t1 = pre_tc0;
  t2 = TC0;
  pre_tc0 = t2;
  if(t1 != t2) //当前捕捉寄存器不等于0或者捕捉值变化，计算转速
  {
    if(t2 > t1) 
    {
      ltemp = 15000000/(t2-t1);
    }
    else 
    {
      ltemp = 15000000/(65535-t1+t2);
    }
  }
  fb_rot = (uint)(ltemp);
  //return(rota);
}

/****************************************************************************
* 函数名称:insert(uchar f)																											
* 函数功能:插值程序																								        
* 入口参数:f:1，主程序调用。0，中断调用																															*
* 出口参数:运算结果 uint result																														
****************************************************************************/     
int insert(uchar f) 
{
  //int resu;
  long int temppp;
  switch(f)
  {
    case 0:
    {
      temppp = x - x1;
      temppp *= (y2 - y1);
      temppp /= (x2 - x1);
      temppp += y1;// + temppp;
      //resu = (int)temppp;
      //result = y1 + (x-x1) * (y2-y1) / (x2-x1);
      //return((int)temppp);
      break; 
    }
    case 1:
    {
     //long int temppp;
      temppp = x12 - x11;
      temppp *= (y22 - y11);
      temppp /= (x22 - x11);
      temppp += y11;// + temppp;
      //resu = (int)temppp;
      //result = y1 + (x-x1) * (y2-y1) / (x2-x1);
      //return((int)tempp); 
      break;
    }
    default:
      break; 
  }
  return((int)temppp);
}
/****************************************************************************
* 函数名称:comp()																											
* 函数功能:限制数值程序																								        
* 入口参数:2个比较数																											
* 出口参数:返回结果																														
****************************************************************************/
int comp(int a,int b)
{
  if(a > b)
  {
    return(b);
  }
  else if(a < -b)
  {
    return(-b);
  }
  return(a);   
}

/****************************************************************************
* 函数名称:pid_rot()																										
* 函数功能:转速环pid子程序																							
* 入口参数:无																													
* 出口参数:无																													
*****************************************************************************/
void pid_rot(void) 
{
  /* 变量定义 */  
  //uint *p,*q;
  //uchar average_times;
  //int x1,x2,y1,y2,x;
  //uint cak_set_rot;
  //uint cak_fb_rot = fb_rot;
  
  //设定转速平滑滤波  
  pre_set_rot *= 7;
  pre_set_rot += set_rot;
  pre_set_rot /= 8;
  
  /* 计算偏差 */
  //if(state_flag != DYNAF) 
  //{
  /*cak_fb_rot[7] = cak_fb_rot[6];
  cak_fb_rot[6] = cak_fb_rot[5];
  cak_fb_rot[5] = cak_fb_rot[4];
  cak_fb_rot[4] = cak_fb_rot[3];
  cak_fb_rot[3] = cak_fb_rot[2];
  cak_fb_rot[2] = cak_fb_rot[1];
  cak_fb_rot[1] = cak_fb_rot[0];
  cak_fb_rot[0] = fb_rot;
  filtered_fb_rot = cak_fb_rot[0] + cak_fb_rot[1] + cak_fb_rot[2] 
  + cak_fb_rot[3] + cak_fb_rot[4] + cak_fb_rot[5] + cak_fb_rot[6] 
  + cak_fb_rot[7];
  filtered_fb_rot /= 8;*/              
  
  /*if((fb_rot <= nrm.rt540 - (int)SCHMITT) 
  || ((fb_rot <= nrm.rt540 + SCHMITT) && (average_times == 2))) 
  {
    average_times = 2;
  }
  
  if(((fb_rot <= nrm.rt540 + SCHMITT) && (average_times == 4)) ||
     ((fb_rot >= nrm.rt540 + SCHMITT) && (fb_rot <= nrm.rt541 
     - (int)SCHMITT)) ||
     ((fb_rot <= nrm.rt541 + SCHMITT) && (average_times == 4))) 
  {
    average_times = 4;
  }

  if(((fb_rot <= nrm.rt541 + SCHMITT) && (average_times == 8)) ||
     ((fb_rot >= nrm.rt541 + SCHMITT) && (fb_rot <= nrm.rt542 
     - (int)SCHMITT)) ||
     ((fb_rot <= nrm.rt542 + SCHMITT) && (average_times == 8))) 
  {
    average_times = 8;
  }
  
  if((fb_rot > nrm.rt542 + SCHMITT) || ((fb_rot <= nrm.rt542 + SCHMITT) 
  && (average_times == 16))) 
  {
    average_times = 16;
  } */
  if(fb_rot < nrm.rt540)
  {
    average_times = 2;
  }
  if((fb_rot > nrm.rt540 + SCHMITT) && fb_rot < nrm.rt541)
  {
    average_times = 4;
  }
  if((fb_rot > nrm.rt541 + SCHMITT) && fb_rot < nrm.rt542)
  {
    average_times = 8;
  }
  if(fb_rot > nrm.rt542 + SCHMITT) 
  {
    average_times = 16;
  }
  pre_fb_rot *= (average_times - 1);
  pre_fb_rot += fb_rot;
  pre_fb_rot /= average_times; 
  if( (nrm.rdrop == 255) || (stksi > MAXKI) ) 
  {
    drop_rot = 0; //调速率在16-255之外或者stksi大于MAXKI,调速率不起作用
  }
  else 
  {
    drop_rot = pre_fb_exe / nrm.rdrop;
    drop_rot *= 4;
  } /* end if */
  delta_rot = (pre_set_rot - (int)drop_rot) - pre_fb_rot;//get delta rotation
  
  //pid运算
  if((fb_rot > (rot.lmt2 + SCHMITT)) //判断进入哪个环 
     || ((fb_rot > rot.lmt2) && (rp_flag == 0)))
  {
    rp_flag = 0; //设置转速环运算标志 
    // KP 
    temp = delta_rot;
    temp = comp(temp,704);
    //参数动静态分离 
    /*if(state_flag == DYNAF) 
    {
      if(temp > nrm.rbandw) 
      {
        temp = 2*temp - (int)nrm.rbandw;
      }
      else if(temp < -(int)nrm.rbandw)
      {
        temp = 2*temp + (int)nrm.rbandw;
      } 
    } */
    //参数高低速分离
    if(fb_rot > nrm.rsphlsp + SCHMITT) //高速 
      stksp = nrm.rksp;
    else if(fb_rot < nrm.rsphlsp)  //低速
      stksp = nrm.rksp1;
    
    x1 = RPSEPA;
    y1 = LKSP;
    x2 = 1023;
    y2 = stksp;
    x = pre_fb_exe;
    if(x < x1)
    {
      caksp = LKSP;
    }
    else
    {
      caksp = insert(0);
    }
    temp *= (int)stksp; //here no insertion
    temp /= 10;
    temp1 = temp;
    //KD 
    temp = delta_rot - pre_delta_rot; //soft
    pre_delta_rot = delta_rot;
    temp = comp(temp,70);    
    // 参数高低速分离
    if(fb_rot > nrm.rsphlsp + SCHMITT) //高速 
      stksd = nrm.rksd;
    else if(fb_rot < nrm.rsphlsp)  //低速
      stksd = nrm.rksd1; 
    x1 = RPSEPA;
    y1 = LKSD;
    x2 = 1023;
    y2 = stksd;
    x = pre_fb_exe;
    if(x < x1)
    {
      caksd = LKSD; 
    }
    else 
    {
      caksd = insert(0);
    }    
    //参数动静态分离 
    /*if(state_flag == DYNAF) 
    {
      stksd *= 2;
    }  */
    
    temp *= (int)stksd; 
    temp /= 8;
    temp += kd_output;
    temp /= 2;
    temp = (temp + kd_output) / 2;
    temp2 = temp;
    kd_output = temp2;
    //KA 
    temp = kd_output - pre_kd_output;//soft
    pre_kd_output = kd_output;
    temp = comp(temp,80);                          
    temp *= (int)nrm.rdynaw;
    temp += ka_output;
    temp /= 2;
    temp = (temp + ka_output)/2;
    temp3 = temp;
    ka_output = temp3;  
    //KI 
    ltemp = comp(delta_rot,512);
    //temp = (long int)temp;
    sum_delta_rot += ltemp;
    if(sum_delta_rot > max_sum_rot)
      sum_delta_rot = max_sum_rot;
    else if(sum_delta_rot < min_sum_rot)
      sum_delta_rot = min_sum_rot;
    //参数高低速分离 
    if(fb_rot > nrm.rsphlsp + SCHMITT) //高速 
      stksi = nrm.rksi;
    else if(fb_rot < nrm.rsphlsp)  //低速
      stksi = nrm.rksi1;
    if(stksi >= MAXKI)                    
      temp4 = 0;  //积分超过最大，则取消积分
    ltemp = sum_delta_rot / (int)stksi;
    ltemp /= 2;
    temp4 = (int)ltemp;
    //rtemp4 = 0; //取消积分
    
    // 计算设定位置set_exe 
    //temp = temp1 + temp2 + temp3 + temp4; //PID
    temp = temp1 + temp2 + temp3;    //PD
    //rtempx = temp4;  //for 积分测试
    temp = comp(temp,limt1/2);
    temp += (limt1 / 2);
    set_exe = (uint)temp;
  }
  else   
  {
    rp_flag = 1; //设置位置环运算标志
    set_exe = limt1;
  }/* end if */
  //计算油量限制limt1
  if(pre_fb_rot >= rot.lmt12)
  {
    limt1 = 0;  //超速，油量限制归零
  }
  else if(pre_fb_rot <= rot.lmt1)
  {
    limt1 = oil.lmt1;
  }
  else
  {
    p = &rot.lmt1;
    q = &oil.lmt1;
    while(pre_fb_rot > *p)
    {
      p++;
      q++;
    } 
    //插值求油量限制limt1
    x2 = *p;
    y2 = *q;
    p--;
    q--;
    x1 = *p;
    y1 = *q;
    x = pre_fb_rot;
    limt1 = insert(0);
  }
  /*p = &rot.lmt1;
  q = &oil.lmt1;
  do //确定转速范围
  {
    p++;
    q++;
  }
  while(pre_fb_rot > *p);*/
  
  //计算max_sum_rot,min_sum_rot 
  //max_sum_rot = 2 * stksi * limt1 - OFFSET;
  //max_sum_rot = 9 * stksi * limt1 / 10;
  max_sum_rot = 9 * stksi;
  max_sum_rot *= limt1;
  max_sum_rot /= 10;
  min_sum_rot = limt1 * stksi; 
  min_sum_rot = -(long int)min_sum_rot;
  //dynamic-static separate
  if((set_rot >= SEPSPD) && ((delta_rot > (int)nrm.rbandw) 
     || (delta_rot < -(int)nrm.rbandw)))
  {
    state_flag = DYNAF;
    PORTB_BIT2 = 0;
  }  
  else 
  {
    state_flag = STATF;
    PORTB_BIT2 = 1;
  }
}

/****************************************************************************
* 函数名称:pid_exe()																												
* 函数功能:位置环pid子程序																							
* 入口参数:无																														
* 出口参数:无																														
****************************************************************************/
void pid_exe(void) 
{
  //变量定义  
  //int x1,x2,y1,y2,x;
  //计算偏差
  pre_set_exe += set_exe;
  pre_set_exe /= 2;
  pre_fb_exe += fb_exe;
  pre_fb_exe /= 2;
  delta_exe = pre_set_exe - pre_fb_exe;
  //插值求stpkp 
  x1 = PSEPA;
  y1 = LPKP;
  x2 = 1023;
  y2 = nrm.rpkp;
  x = pre_fb_exe;
  if(x > x1)
  {
    stpkp = insert(0);
  }
  else
  {
    stpkp = LPKP;
  } 
    
  //插值求stpkd
  y1 = LPKD;
  y2 = nrm.rpkd;
  if(x > x1)
  {
    stpkd = insert(0);
  }
  else 
  {
    stpkd = LPKD;
  }
  //PKP 
  temp = delta_exe;
  temp = comp(temp,255);// 偏差限制 
  temp *= (int)nrm.rpkp;//计算, 没用到插值
  temp /= 2; 
  temp1 = temp;  
  //PKD 
  temp = delta_exe - pre_delta_exe;  // 偏差计算 soft
  pre_delta_exe = delta_exe;
  //temp = hard_pkd - V0CNV;    //hard
  temp = comp(temp,255);//偏差限制
  temp *= (int)nrm.rpkd;//没用插值
  temp /= 2; 
  temp = (temp + pkd_exe) / 2;
  temp2 = temp;
  pkd_exe = temp2;
  //PKA 
  temp = pkd_exe - pre_pkd_exe; //偏差计算,soft
  pre_pkd_exe = pkd_exe;
  //temp = V0CNV - hard_pka  ;  //hard
  temp = comp(temp,127);//偏差限制
  temp *= (int)nrm.rpdynaw;
  temp /= 2;
  //temp = temp * (int)nrm.rpdynaw / 2;
  temp = (temp + pka_exe) / 2;
  temp3 = temp;