PID梯形运动曲线核心代码实测可用.c

/*
***************************************************************************************
*
*               （c） Copyright 2006-2008 wei wei.qiu china zj. hz 
*                            All Rights Reserved
*
*							
*                                                                                               
*
*--------------文件信息-----------------------------------------------------------------
* 文 件 名: app_gpio.c
* 创 建 人: 邱卫卫	
* 创建日期: 2014年01月08日
* 描    述: 主程序C语言入口 uC/OS-II启动
*---------- 版本信息-------------------------------------------------------------------
* 版    本: V1.0
*
*--------------------------------------------------------------------------------------
****************************************************************************************

void get_encoder_TWO（void）//******************计算当前实际速度V
{
  s32 CNT3_tempCNT3_last;
 
  cnt3 = TIM3 -> CNT;
  CNT3_last = CNT3;
  CNT3_temp = rcnt3 * ENCODE_PRD + cnt3; 
	Position_PID.ActualPoint = CNT3_temp;
	
  V3 = CNT3_temp - CNT3_last;		 
  while （V3>Vbreak）				 
  {							      
   rcnt3--;					      
   CNT3_temp = rcnt3 * ENCODE_PRD + cnt3;
   V3 = CNT3_temp - CNT3_last;		 
  }							     
  while （V3<-Vbreak）			   
  {							      
   rcnt3++;					      
   CNT3_temp = rcnt3 * ENCODE_PRD + cnt3;
   V3 = CNT3_temp - CNT3_last;		 
  }
  CNT3 = CNT3_temp;						 
  //POSITION += CNT3;
	
}

void PID_init（void）
{   
    Position_PID.SetPoint=0.0;
    Position_PID.ActualPoint=0.0;
	  Position_PID.CurPoint =0;
	  Position_PID.CurVolt =0;
	  Position_PID.DecSpeedF=0;
    Position_PID.err=0.0;
    Position_PID.err_last=0.0;
    Position_PID.voltage=0.0;
    Position_PID.integral=0.0;
    Position_PID.Kp=0.01;
    Position_PID.Ki=0.30;
    Position_PID.Kd=0.1; 
	  Position_PID.Integral_Index = 50;//（（float）MOTION_POINT/（float）7200）*60.0+2.0;
    TIM_Cmd（TIM2 ENABLE）;	    
}

float point_PID_realize（float point）
{
   float indexindex2=1.0err_temp;
   Position_PID.SetPoint = point;
   Position_PID.err=Position_PID.SetPoint-Position_PID.ActualPoint;
	 index = Position_PID.SetPoint/2; 
	 if（Position_PID.DecSpeedF == 0）
	 {
	    if（（（Position_PID.SetPoint-Position_PID.err）>=index）
		     ||（Position_PID.voltage>=PWMPeriod））
	    {
		       Position_PID.CurPoint = Position_PID.SetPoint-Position_PID.err;   //recode 
		       Position_PID.CurVolt  = Position_PID.voltage;
				   //Position_PID.integral -= 24*Position_PID.SetPoint;
		       Position_PID.DecSpeedF =0xAA;
	    }
	 }
   if（abs（Position_PID.err）>Vbreak） //
   {
      index=0.0;
   } 
	 else if（Position_PID.ActualPoint>=Position_PID.SetPoint）
	 {
		    index=0;
		    Position_PID.DecSpeedF =0;
			  Position_PID.integral =0;       
   }
   else if（abs（Position_PID.err）<=Position_PID.CurPoint）
	 {
		 
		 
		  //Position_PID.Kp = 7.0;
		  //Position_PID.voltage=Position_PID.Kp*Position_PID.err;    
      //return Position_PID.voltage;