#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 = 68;
// const int DIST_wall_rl = 88;
/*---全局各种变量与常量定义完成---*/


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