bk_bishe_pi/robot_test_code/PID/PIDmain.cpp

#include<iostream>
#include"PID.h"
#include"eyebot++.h"
using namespace std;


/*---定义全局常量---*/
const int DIST_cell = 300 ;
const int DIST_wall = 100 ;
/*---全局各种变量与常量定义完成---*/


/*---下面是主函数---*/
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,45);     //向左旋转
            VWWait();
        }
        else if (Fwall)     
        {
        //不旋转
        }
        else if (Rwall)
        {
            VWTurn(-90,45);     //向右旋转
            VWWait();
        }
        else 
        {
            VWTurn(180,45);
            VWWait();
        }
        //直走
        PIDStraight();
    } while (KEYRead() != KEY4);
}