|
|
硬件: 主控板： arduino2560
超声波模块：HC-SR04
电机控制模块：L298N
实现功能：小车距离障碍物到设定距离后停下
程序如下：
#define uint unsigned int //定义无符号整型
#define uchar unsigned char //定义无符号字符型
#define ulint unsigned long //定义无符号长整型
#define Echo 12 //定义接收引脚
#define Trig 13 //定义触发引脚
#define Enb 5 //电机驱动使能
#define IN4 6 //转向驱动1
#define IN3 7 //转向驱动2
#define Disa_val 350
/********************************
***********变量定义区************
********************************/
uchar Pid_Flag; //电机转向控制
uint PID_val=0; //输出PID值
uchar PWM_val=0; //PWM值
uchar Count1=0; //采样滤波计数器
uint Ul_Tab[4]; //定义超声波采样数组
//*******PID所用的变量************
uint SetPoint; //目标设置值
float Proportion; //比例系数
float Integral; //积分系数
float Derivative; //微分系数
int LastError; //Error[-1]
int PrevError; //Error[-2]
//*********************************
void IncPIDInit(void)
{
LastError=0; //Error[-1]=0
PrevError=0; //Error[-2]=0
SetPoint=200; //设置目标值
Proportion=0.5; //设置比例值
Integral=1.0; //设置积分值
Derivative=0.0; //设置微分值
}
//PID=Uk+KP*E(k)+KI*E(k-1)+KD*E(k-2)
//输出类型是无符号字符型为配合PWM
//入口值是测量值
uchar PID_Operation(int NextPoint)
{
int iError,iIncpid,Tamp0,Tamp1; //偏差值和PID值
uchar PID_ch; //返回值为无符号字符型
iError=NextPoint; //偏差值等于设置值减去测量值
Tamp1=LastError<<1;
Tamp0=iError+PrevError+Tamp1;
Tamp1=iError-LastError;
iIncpid=Proportion*Tamp1+Integral*LastError+Derivative*Tamp0; //完成计算
//Serial.print("********");
//Serial.println(Proportion);
PrevError=LastError;
LastError=iError; //数值存储完成用于下次计算
PID_val+=iIncpid; //PID=U(k)+（PID测量值）
if(PID_val>100) //如果计算的累加PID值大于100
{
PID_val=100; //输出PID值为100
}
else if(PID_val<0) //如果计算的累加值小于0
{
PID_val=0; //输出PID值为0
}
else
{
//PID_ch=PID_val; //转换为字符型0-255
;
}
PID_ch=PID_val;
return(PID_ch); //返回PID值
}
//距离计算
//形参值为测量后的距离值并且输出PWM值
void Distance(uint Dist)
{
int iPe;
if(Dist>200) //如果测量距离大于PID距离
{
Pid_Flag=0; //电机正转
if(Dist<500) //测量距离小于500
{
if((Dist-200)>200) //如果测量值减去设置值大于200
{
PWM_val=150;
}
else
{
iPe=Dist-200;
PWM_val=PID_Operation(iPe);
}
}
else
{
PWM_val=200;
}
}
else
{
Pid_Flag=1; //电机反转
iPe=Dist-200;
PWM_val=PID_Operation(iPe);
}
}
uint Ultra_JS(void)
{
uint Ul_val; //测量数值
digitalWrite(Trig,HIGH);
delayMicroseconds(10); //延时10微秒
digitalWrite(Trig,LOW);
Ul_val=pulseIn(Echo,HIGH);
delayMicroseconds(80); //延时为下次测量准确
return(Ul_val); //返回距离值
}
void Ul_First(void) //超声波测距第一次采样
{
uchar i;
//unsigned long Temp1=0;
for(i=0;i<4;i++)
{
Ul_Tab=Ultra_JS;
//Temp1+=Ul_Tab; //求和
}
//Temp1=Temp1>>2; //采样和除以4
//return (uint)Temp1;
}
uint Ultrasonic(void) //超声波计算
{
uchar Ui;
long int Ui_Num=0;
Ul_Tab[Count1]=Ultra_JS();
for(Ui=0;Ui<4;Ui++)
{
Ui_Num+=Ul_Tab[Ui];
}
Ui_Num>>=2;
Count1++;
if(Count1==4)
{
Count1=0;
}
return(Ui_Num);
}
void setup() {
IncPIDInit();
Serial.begin(9600); //串行通信波特率为9600
pinMode(Echo,INPUT);
pinMode(Trig,OUTPUT);
digitalWrite(Echo,LOW);
digitalWrite(Trig,LOW);
pinMode(Enb,OUTPUT); //PWM驱动引脚
pinMode(IN4,OUTPUT); //转向控制引脚
pinMode(IN3,OUTPUT); //转向控制引脚
Ul_First();
}
void loop() {
float Ul_val=0; //保存距离值用
uint PID_icurval;
Ul_val=Ultrasonic(); //计算距离值
PID_icurval=Ul_val/5.8; //保留一位小数位到此计算距离完成
Serial.print("distance is");
Serial.println(PID_icurval);
//***********************************
Distance(PID_icurval);
if(Pid_Flag==0)
{
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
}
else
{
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
}
analogWrite(Enb,PWM_val); //PWM输出
Serial.println("PIDvalue is");
Serial.println(PWM_val);
}
|
|
发表于 2016-11-9 10:03
