请教一下关于速度闭环

薇笑

shock 发表于 2015-3-30 18:52
你的电磁怎么样了呢，双车追逐诶，难度很大啊

还行吧

王七七

会不会加入速度闭环没处理好

最美的时光

*void Speed_PID(void)
{
   s16 P_Value,I_Value,D_Value;
   
   s16 Speed_Out;
     
   Speed_Old=Speed_New;
   Error=SPEED-Now_Speed;
   if(Error)
   P_Value=P_DATA*(Error-Last_Error);
   I_Value=I_DATA*Error/100;
   D_Value=D_DATA*(Error-2*Last_Error+Pre_Error)/10;
   
  if(I_Value>=100)
   {
      I_Value=100;
   }
   else if(I_Value<=-100)
   {
      I_Value=-100;
   }
   
if(P_Value>=600)
   {
      P_Value=600;
   }
   else if(P_Value<=-600)
   {
      P_Value=-600;
   }
  if(Error>=-10&&Error<=10)
   {
      Speed_New=0;
   }
   Speed_Out=P_Value+I_Value+D_Value;
   Speed_New=Speed_Old+Speed_Out;
   if(Speed_New>=500)
   {
      Speed_New=500;
   }
   else if(Speed_New<=-500)
   {
      Speed_New=-500;
   }
   if(Speed_New>=0)
   {
      Speed_New=350;
   }
   
   else if(Speed_New<=-350)
   {
      Speed_New=-350;
   }
   Pre_Error=Last_Error;
   Last_Error=Error;
   
}*/





void Speed_Count(void)//5ms采集计算一次 50ms计算一次
{


  Speed_Flag++;

  Left_Speed_Temp=FTM1_CNT; //左边电机  编码器
  Right_Speed_Temp=FTM2_CNT;//右边电机  编码器
  FTM1_CNT=0;//清零
  FTM2_CNT=0;//清零
      
if( FTM_QDCTRL_REG(FTMx[FTM1])&0x04)//判断左边电机的正传和反转
  {
      Left_Speed+= Left_Speed_Temp;
  }
  
  else
  {
      Left_Speed+=Left_Speed_Temp;
   }
if( FTM_QDCTRL_REG(FTMx[FTM2])&0x04)//判断右边电机的正传反转
{
     Right_Speed+= Right_Speed_Temp;
  }
  else
  {
     Right_Speed+= Right_Speed_Temp;
  }
  if(Speed_Flag==16)
  {
      Speed_Flag=0;
     
        
        Now_Speed=-(Left_Speed+Right_Speed)/20;
       // var[0]=Now_Speed;
   
      // vcan_sendware((uint8_t *)var, sizeof(var));
        Left_Speed=0;
        Right_Speed=0;
        Speed_Control();

      
  }
SpeedControlOutPut();
   
}

/*****************************************************************
功能：位置式PID计算，用于控制车速
*****************************************************************/

void Speed_Control(void)
{
     
    s16 P_Value,I_Value,D_Value;

      //---当前误差的计算，目标速度减去当前速度---//
    Current_Error=SPEED-Now_Speed;
  
    P_Value=P_DATA*Current_Error;
    I_Value=I_DATA*Current_Error;
    D_Value=D_DATA*(Current_Error-LastError);
    //---速度积分---//
    nSpeedControlKeep+=I_Value;
   
    //---把上次经PI运算后的值存起来---//
    Speed_Old=Speed_New;
   
    Speed_New=(P_Value+I_Value+D_Value)/100 ;
   
    if(nSpeedControlKeep>50)    nSpeedControlKeep=50;
   
    else if(nSpeedControlKeep<-50)   nSpeedControlKeep=-50;   
     
     LastError=Current_Error;    //将本次误差存为上次误差
   

}  


/*****************************************************************
功能：将车速均匀分配在角度控制的20个周期里，可以保证车模的稳定性
*****************************************************************/
void SpeedControlOutPut(void)
{
    if(OK==1)
    {
        s16 nValue;
        nValue=Speed_New-Speed_Old;
        
        /*--速度值分级输出--*/
        nValue=nValue*(Speed_Flag+1)/ 16+Speed_Old;
        
        Speed_OutPut=nValue;
        
        if(Speed_OutPut>100)      Speed_OutPut=100;
        
       else if (Speed_OutPut<-100)      Speed_OutPut=-100;   

        ANGLE_ZERO=Remember_Angle_Zero+Speed_OutPut;
    }
}

喜欢桃子

囩と唂囩飄飄 发表于 2015-3-30 19:37
你是在调静止不动的速度环么？

麻烦问一下，如果先p后i，当p调到什么现场的时候才开始i呢