先上代码
#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);
|
