欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136

標題: 搞平衡車熱身之PID試驗程序 [打印本頁]

作者: liuqq 時間: 2015-5-21 22:41
標題: 搞平衡車熱身之PID試驗程序
先上代碼
#include <string.h>
#include <stdio.h>
#include "RobotLib.h"
int flag=0;
/*====================================================================================================
PID Function

The PID  function is used in mainly
control applications. PIDCalc performs one iteration of the PID
algorithm.

While the PID function works, main is just a dummy program showing
a typical usage.
=====================================================================================================*/

typedef struct PID {

      int  SetPoint;          //  ???? Desired Value

      int  Proportion;       //  ???? Proportional Const
      int  Integral;          //  ???? Integral Const
      int  Derivative;       //  ???? Derivative Const

      int  LastError;       //  Error[-1]
      int  PrevError;       //  Error[-2]
      int  SumError;          //  Sums of Errors

} PID;

/*====================================================================================================
位置式PID

=====================================================================================================*/

int PIDCalc( PID *pp, int NextPoint )
{
int  dError,
      Error;

      Error = pp->SetPoint -  NextPoint;       // ??
      pp->SumError += Error;                   // ??
      dError = pp->LastError - pp->PrevError;    // ????
      pp->PrevError = pp->LastError;
      pp->LastError = Error;
      return (pp->Proportion * Error             // ???
         + pp->Integral * pp->SumError       // ???
         + pp->Derivative * dError          // ???
      );
}

/*====================================================================================================
增量式PID
=====================================================================================================*/
double PIDCalc_delt( PID *pp, double NextPoint )
{
      double dError, Error;
      Error = pp->SetPoint - NextPoint;    // ?????  ???????????
      dError= pp->Proportion * Error  +  pp->Integral * pp->LastError  +  pp->Derivative * pp->PrevError;
      pp->PrevError = pp->LastError;       // ?????  ????????????
      pp->LastError = Error;
      return (dError);
}
/*====================================================================================================
Initialize PID Structure
=====================================================================================================*/

void PIDInit (PID *pp)
{
memset ( pp,0,sizeof(PID));
}

/*====================================================================================================
Main Program
=====================================================================================================*/

/*#define N 12
int sensor (void)                   //  Dummy Sensor Function
{
  char count,i,j;
  int ad_data[N];
  int sum=0;
  int advalue=0;
  int temp=0;
  for (count = 0; count<=N; count++)
  {
  ad_data[count]=AI(1);
  }
  for (j=0;j<N-1;j++)
  {
  for (i=0;i<N-1;i++)
  {
  if (ad_data[i]>ad_data[i+1])
  {
temp=ad_data[i];
ad_data[i]=ad_data[i+1];
ad_data[i+1]=temp;
  }
  }
  }
  for (count=0;count<N;count++)
  {
  sum=ad_data[count];
  }
  advalue=sum/(N-2);
  return advalue;

}  */
void Run(int left_speed,int right_speed)
{
SetMoto(0,-left_speed);
SetMoto(1,-right_speed);
}

int speed=0;
void actuator(int rDelta)          //  Dummy Actuator Function
{
   speed=rDelta;
   if(flag==1)
   {
if (speed>100)
{
speed=100;
}
else if(speed<-100)
{
speed=-100;
}
}
else if(flag==2)
{
speed=rDelta/3;
if (speed>50)
{
speed=50;
}
else if(speed<-50)
{
speed=-50;
}
else if (speed<10 && speed>0)//電機死區
{
   speed=8;
}
else if (speed>-10 && speed<0) //電機死區
{
   speed=-8;
}
}
Run(speed,speed);

}
#define N 12
int sensor1(void)//均值濾波。最簡單的一種
{
  int buffer[N];
  int count=0;
  int sum=0;
  int ad=0;
  for (count=0;count<N;count++)
  {
  buffer[count]=AI(1);
  wait(0.001);
  }
  for (count=0;count<N;count++)
  {
  sum+=buffer[count];
  }
  ad=sum/N;
  return ad;
}

void main(void)
{
PID    setPID;                //  PID Control Structure
int    OutPut;                //  PID Response (Output)
int    InPut;                   //  PID Feedback (Input)

PIDInit ( &setPID );                //  Initialize Structure
setPID.Proportion = 20;             //  Set PID Coefficients
setPID.Integral = 0.5;
setPID.Derivative = 0.5;
setPID.SetPoint = 150;          //  Set PID Setpoint
while(0)
{
   printf("%d\n", sensor1());
//  wait(0.2);
}
while(1)
   {                         //  Mock Up of PID Processing
      InPut = sensor1();             //  Read Input
      if(InPut-setPID.SetPoint>80 ||InPut-setPID.SetPoint<-80)//分段式PID.距離遠，比例系數大點
      {
         flag=1;
   PIDInit ( &setPID );                //  Initialize Structure
setPID.Proportion = 20;             //  Set PID Coefficients
setPID.Integral = 0.5;
setPID.Derivative = 0.5;
setPID.SetPoint = 150;          //  Set PID Setpoint

      }
      else if (InPut-setPID.SetPoint<20 ||InPut-setPID.SetPoint<-20)//距離近，PID參數相應小一點
      {
      flag=2;
      PIDInit ( &setPID );                //  Initialize Structure
   setPID.Proportion = 2;             //  Set PID Coefficients
   setPID.Integral = 0.3;
   setPID.Derivative = 0;
   setPID.SetPoint = 150;          //  Set PID Setpoint
      }

   //  wait(0.05);
      OutPut = PIDCalc ( &setPID,InPut ); //  Perform PID Interation
      actuator ( OutPut );             //  Effect Needed Changes
      printf("InPut=%d\n,Output=%d\n",InPut,OutPut );
   // wait(0.1);
   }
}

效果還不錯（手機錄像不行，改天補上視頻）。（控制器性能強大。主頻144M RAM 1M ROM 2M）基本沒有超調，兩個震蕩周期就穩定了。設定的目標是150（測距傳感器返回值）.小車在距離150很遠時是100%的速度跑的。快接近150時，速度馬上降得很快。第一次到150時，會超前1cm左右，然后往回跑，逼近150時，剛好停下，趨于穩定。傳感器在149 150 151左右跳動。小車也在不斷微調。反應非常靈敏。最后基本上小車在+－1mm左右晃或者不動的狀態。

我后面試過不用PID來做位置控制。也是可以，效果不好就是。震蕩比較劇烈。需要調整6 7表周期才會停到目標點。沒有PID那么迅速靈敏。
簡單許多，代碼如下，非常簡單。
distance=sensor1();
NeedSpeed=distance-SetPoint;

printf("distance=%d\n,NeedSpeed=%d\n",distance,NeedSpeed );
  Run(NeedSpeed,NeedSpeed);

作者: switch 時間: 2016-4-15 23:41

作者: switch 時間: 2016-7-23 08:55
厲害

作者: degage 時間: 2019-4-19 09:30
樓主思路清晰，功能完善學習了

歡迎光臨 (http://www.raoushi.com/bbs/)