mirror of
https://gitee.com/yyz_o/bk_bishe_pi.git
synced 2025-09-07 23:21:26 +00:00
53 lines
1.1 KiB
C++
53 lines
1.1 KiB
C++
#include<iostream>
|
|
#include"PID.h"
|
|
#include"eyebot++.h"
|
|
using namespace std;
|
|
|
|
|
|
/*---定义全局常量---*/
|
|
// const int DIST_cell = 300 ;
|
|
// const int DIST_wall = 100 ;
|
|
const int DIST_cell = 300;
|
|
const int DIST_wall_f = 80;
|
|
const int DIST_wall_rl = 87;
|
|
/*---全局各种变量与常量定义完成---*/
|
|
|
|
|
|
/*---下面是主函数---*/
|
|
int main()
|
|
{
|
|
int Fwall,Lwall,Rwall;
|
|
LCDMenu("","","","END");
|
|
|
|
do
|
|
{ //检测墙面
|
|
Fwall = PSDGetRaw(1) > DIST_cell; //检测前面侧是否有墙
|
|
Lwall = PSDGetRaw(2) > DIST_cell; //检测左面
|
|
Rwall = PSDGetRaw(3) > DIST_cell; //检测右面
|
|
|
|
//判断旋转方向
|
|
if (Lwall)
|
|
{
|
|
VWTurn(+90,15); //向左旋转
|
|
VWWait();
|
|
}
|
|
else if (Fwall)
|
|
{
|
|
//不旋转
|
|
}
|
|
else if (Rwall)
|
|
{
|
|
VWTurn(-90,15); //向右旋转
|
|
VWWait();
|
|
}
|
|
else
|
|
{
|
|
VWTurn(180,15);
|
|
VWWait();
|
|
}
|
|
//直走
|
|
PIDStraight();
|
|
VWWait();
|
|
} while (KEYRead() != KEY4);
|
|
}
|