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



int main()
{
    int f_wall, b_wall;
    int old_f_wall, old_b_wall;
    int turn_check;
    LCDMenu("","","","END");
    int num;
    do
    {
        cin>>num;
        if(num == 1)
        {
            b_wall = PSDGetRaw(4) - 2;
            do
            {
                old_b_wall = b_wall;
                MOTORDriveRaw(1,19);
                MOTORDriveRaw(2,-17);
                //~ MOTORDriveRaw(1,24);
                //~ MOTORDriveRaw(2,-22);
                OSWait(850);
                //~ OSWait(1980);
                MOTORDriveRaw(1,0);
                MOTORDriveRaw(2,0);
                b_wall = (PSDGetRaw(4) - 2) * 0.7  +  old_b_wall * 0.3;   //减小突变
                //~ b_wall = PSDGetRaw(4) - 2;
                turn_check = b_wall - old_b_wall;
                /*---打印输出PSD数值---*/
                //~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
                cout << "b_wall:" << b_wall << " check:" << turn_check << endl;
                /*---打印结束---*/
                OSWait(50);
            } while ( turn_check < -3  ||  b_wall > 130);
        }
        else if (num == 3)
        {
            b_wall = PSDGetRaw(4) - 2;
            do 
            {
                old_b_wall = b_wall;
                MOTORDriveRaw(1,-19);
                MOTORDriveRaw(2,17);
                //~ MOTORDriveRaw(1,24);
                //~ MOTORDriveRaw(2,-22);
                OSWait(850);
                //~ OSWait(1980);
                MOTORDriveRaw(1,0);
                MOTORDriveRaw(2,0);
                b_wall = (PSDGetRaw(4) - 2) * 0.7  +  old_b_wall * 0.3;   //减小突变
                //~ b_wall = PSDGetRaw(4) - 2;
                turn_check = b_wall - old_b_wall;
                /*---打印输出PSD数值---*/
                //~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
                cout << "b_wall:" << b_wall << " check:" << turn_check << endl;
                /*---打印结束---*/
                OSWait(50);
            } while (turn_check < -3  ||  b_wall > 130);
        }
        else if (num == 2)
        {
            b_wall = PSDGetRaw(4) - 2;
            f_wall = PSDGetRaw(1) - 2;
            do 
            {
                old_b_wall = b_wall;
                old_f_wall = f_wall;
                MOTORDriveRaw(1,19);
                MOTORDriveRaw(2,-17);
                //~ MOTORDriveRaw(1,24);
                //~ MOTORDriveRaw(2,-22);
                OSWait(1000);
                //~ OSWait(1980);
                MOTORDriveRaw(1,0);
                MOTORDriveRaw(2,0);
                b_wall = (PSDGetRaw(4) - 2) * 0.7  +  old_b_wall * 0.3;     //减小突变
                f_wall = (PSDGetRaw(1) - 2) * 0.7  +  old_f_wall * 0.3;     //减小突变
                //~ b_wall = PSDGetRaw(4) - 2;
                //~ f_wall = PSDGetRaw(1) - 2;
                turn_check = b_wall - old_b_wall;
                /*---打印输出PSD数值---*/
                //~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
                cout << "f_wall: " << f_wall << "\t" << "b_wall: " << b_wall << "\t" << "check: " << turn_check << endl;
                /*---打印结束---*/
                OSWait(50);
            } while (f_wall < 200 || turn_check < -4  ||  b_wall > 130);
        }
        //~ if(num == 1)
        //~ {
            //~ MOTORDriveRaw(1,19);
            //~ MOTORDriveRaw(2,-17);
            //~ MOTORDriveRaw(1,24);
            //~ MOTORDriveRaw(2,-22);
            //~ OSWait(1500);
            //~ OSWait(1980);
            //~ MOTORDriveRaw(1,0);
            //~ MOTORDriveRaw(2,0);
        //~ }
        //~ else if(num == 2)
        //~ {
            //~ MOTORDriveRaw(1,24);
            //~ MOTORDriveRaw(2,-22);
            //~ MOTORDriveRaw(1,24);
            //~ MOTORDriveRaw(2,-22);
            //~ OSWait(2700);
            //~ OSWait(3250);
            //~ MOTORDriveRaw(1,0);
            //~ MOTORDriveRaw(2,0);
        //~ }
        //~ else if(num == 3)
        //~ {
            //~ MOTORDriveRaw(1,26);
            //~ MOTORDriveRaw(2,-24);
            //~ MOTORDriveRaw(1,-24);
            //~ MOTORDriveRaw(2,22);
            //~ OSWait(1815);
            //~ OSWait(2025);
            //~ MOTORDriveRaw(1,0);
            //~ MOTORDriveRaw(2,0);
        //~ }
        else
        {
        }
    } while (KEYRead() != KEY4);
    
}