先上代碼
#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)//電機死區(qū)
{
speed=8;
}
else if (speed>-10 && speed<0) //電機死區(qū)
{
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.距離遠，比例系數(shù)大點
{
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參數(shù)相應小一點
{
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）基本沒有超調，兩個震蕩周期就穩(wěn)定了。設定的目標是150（測距傳感器返回值）.小車在距離150很遠時是100%的速度跑的�？旖咏�150時，速度馬上降得很快。第一次到150時，會超前1cm左右，然后往回跑，逼近150時，剛好停下，趨于穩(wěn)定。傳感器在149 150 151左右跳動。小車也在不斷微調。反應非常靈敏。最后基本上小車在+－1mm左右晃或者不動的狀態(tài)。
我后面試過不用PID來做位置控制。也是可以，效果不好就是。震蕩比較劇烈。需要調整6 7表周期才會停到目標點。沒有PID那么迅速 靈敏。
簡單許多，代碼如下，非常簡單。
distance=sensor1();
NeedSpeed=distance-SetPoint;
printf("distance=%d\n,NeedSpeed=%d\n",distance,NeedSpeed );
Run(NeedSpeed,NeedSpeed);
|
