jastintime2.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;
SteeringMotor motor;
volatile int start = 0;
int count = 0;
int count1 = 0;
#define MotorDelay   2
void int_1ms()
{
    if (start == 1)
    {
        if (count1 < 360)
        {
            //start = 0;
            motor.Left();
            count++;
            if (count >= (4 + MotorDelay))
            {
                //motor.Right();
                count1++;
                //if (count1 >= 12)
                //{
                    start = 0;
                    //count1 = 0;
                    motor.Brake();
                //}
                //cputw(count1);
            }
        }
    }
    else
    {
        //count1 = 0;
        count = 0;
    }
    //gpSteeringController.Control();
}

int main(int argc, char **argv)
{
    Command cmd;
    setUserTaskAddress(int_1ms);
    //runUserTask();
    
    motor.SetSpeed(255);
    //motor.Left();
    
    int targetAngle = 5000;
    int counter = 0;
    time_t prevTime = get_system_up_time();
    cmd.SetSteeringMode(TargetAngle);
    cmd.SetSteeringSpeed(1000);
    cmd.SetSteeringAngle(targetAngle);
    //gpSteeringController->SetTargetSteeringAngle(targetAngle);
    //while(!PRESSED(dbutton(), BUTTON_VIEW));
    runUserTask();
    while(!shutdown_requested())
    {
        time_t curTime = get_system_up_time();
        if ((curTime - prevTime) >= 100)
        {
            start = 1;
            //targetAngle = -targetAngle;
           targetAngle += 1000;
           if (targetAngle > 30000)
           {
               targetAngle = -30000;
           }
            //cmd.SetSteeringAngle(targetAngle);
            //gpSteeringController.Execute(&cmd);
            prevTime = curTime;
        }
        
        if (PRESSED(dbutton(), BUTTON_VIEW))
        {
            break;
        }
    }
    
    stopUserTask();
    setUserTaskAddress(0);
    gpSteeringController.Stop();

    return 0;
}