jastintime.c

为参加日本的ET机器人比赛写的LEGO机器人控制程序

#include "Calibration.C"
//#include "ChainloopStrategy.C"
#include "ColorDetector.C"
#include "Command.C"
//#include "CurveStrategy.C"
#include "DriveMotor.C"
//#include "GoalStrategy.C"
#include "LaneTracker.C"
#include "LightSensor.C"
//#include "ShortcutStrategy.C"
//#include "SlopeStrategy.C"
//#include "SteeringController.C"
#include "SteeringMotor.C"
//#include "Strategy.C"
#include "TouchSensor.C"
    
//SteeringController* gpSteeringController = (SteeringController*)NULL;
void int_1ms()
{
//    if (gpSteeringController != NULL)
//    {
//        gpSteeringController->Control();
//    }
}

int main(int argc, char **argv)
{
    LaneTracker tracker;
    //gpSteeringController = tracker.GetSteeringController();
    int function = -1;
    bool outCourse = true;
    int key = 0;

    cputs("start");
    while(!shutdown_requested())
    {
        key = getchar();
        if (key == KEY_PRGM)
        {
            function++;
            if (function >= 5)
            {
                function = 0;
            }
            switch(function)
            {
                case 0:
                {
                    cputs("black");
                    break;
                }
                case 1:
                {
                    cputs("white");
                    break;
                }
                case 2:
                {
                    cputs("gray");
                    break;
                }
                case 3:
                {
                    cputs("lane");
                    break;
                }
                case 4:
                {
                    cputs("run");
                    break;
                }
            }
        }
        
        if (key == KEY_VIEW)
        {
            if (function == 0)
            {
                tracker.CalibrationBlack();
            }
            else if (function == 1)
            {
                tracker.CalibrationWhite();
            }
            else if (function == 2)
            {
                tracker.CalibrationGray();
            }
            else if (function == 3)
            {
                outCourse = !outCourse;
                if (outCourse)
                {
                    cputs("out");
                }
                else
                {
                    cputs("in");
                }
            }
            else if (function == 4)
            {
                break;
            }
        }
    }
    
    setUserTaskAddress(int_1ms);

    time_t prevTime = get_system_up_time();
    tracker.Initialize(outCourse);
    
    getchar();
    tracker.Start();
    //runUserTask();
    while(!shutdown_requested())
    {
        time_t curTime = get_system_up_time();
        if ((curTime - prevTime) >= 1)
        {
            prevTime = curTime;
            if (tracker.Track() != 0)
            {
                break;
            }
        }
        
        if (PRESSED(dbutton(), BUTTON_VIEW))
        {
            break;
        }
    }
    
    //cputs("quit");
    /*Command cmd;
    cmd.SetSteeringMode(TargetAngle);
    cmd.SetSteeringAngle(0);
    cmd.SetSpeed(0);
    cmd.SetFlag(0x03);
    tracker.Execute(&cmd);
    while(!shutdown_requested())
    {
        if (PRESSED(dbutton(), BUTTON_PROGRAM))
        {
            break;
        }
    }
    */
    //stopUserTask();
    //setUserTaskAddress(0);
    //gpSteeringController = (SteeringController*)NULL;

    tracker.Stop();

    return 0;
}