第16章-超声波跟随功能
无PID跟随功能
//超声波跟随
if(HC_SR04_Read() > 25)
{
motorForward();//前进
HAL_Delay(100);
}
if(HC_SR04_Read() < 20)
{
motorBackward();//后退
HAL_Delay(100);
}
PID跟随功能
在pid.c中定义一组PID参数
tPid pidFollow; //定距离跟随PID
pidFollow.actual_val=0.0;
pidFollow.target_val=22.50;//定距离跟随 目标距离22.5cm
pidFollow.err=0.0;
pidFollow.err_last=0.0;
pidFollow.err_sum=0.0;
pidFollow.Kp=-0.5;//定距离跟随的Kp大小通过估算PID输入输出数据,确定大概大小,然后在调试
pidFollow.Ki=-0.001;//Ki小一些
pidFollow.Kd=0;
//**********PID跟随功能***********//
g_fHC_SR04_Read=HC_SR04_Read();//读取前方障碍物距离
if(g_fHC_SR04_Read < 60){ //如果前60cm 有东西就启动跟随
g_fFollow_PID_Out = PID_realize(&pidFollow,g_fHC_SR04_Read);//PID计算输出目标速度 这个速度,会和基础速度加减
if(g_fFollow_PID_Out > 6) g_fFollow_PID_Out = 6;//对输出速度限幅
if(g_fFollow_PID_Out < -6) g_fFollow_PID_Out = -6;
motorPidSetSpeed(g_fFollow_PID_Out,g_fFollow_PID_Out);//速度作用与电机上
}
else motorPidSetSpeed(0,0);//如果前面60cm 没有东西就停止
HAL_Delay(10);//读取超声波传感器不能过快
然后编译,烧录测试 。
下一章节 我们开始使用6050 讲解6050使用和如何和PID结合一起