jastintime5.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();
    byte function = -1;
    bool outCourse = true;
    int key = 0;
    
    cputs("start");
    while(!shutdown_requested())
    {
        key = getchar();
        if (key == KEY_PRGM)
        {
            function++;
            function %= 5;
            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();
                cputs("     ");
                cputw(tracker.GetCalibration().GetBlack());
            }
            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);
    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);
    gpSteeringController->Execute(&cmd);
    while(!shutdown_requested())
    {
        if (PRESSED(dbutton(), BUTTON_PROGRAM))
        {
            break;
        }
    }

    stopUserTask();
    setUserTaskAddress(0);
    gpSteeringController = (SteeringController*)NULL;

    tracker.Stop();

    return 0;
}