C++ TIMER::restart方法代码示例

MDA_TASK_RETURN_CODE MDA_TASK_GATE:: run_task() {
    puts("Press q to quit");

    MDA_VISION_MODULE_GATE gate_vision;
    TASK_STATE state = STARTING;

    bool done_gate = false;
    MDA_TASK_RETURN_CODE ret_code = TASK_MISSING;

    // read the starting orientation
    int starting_yaw = attitude_input->yaw();
    printf("Starting yaw: %d\n", starting_yaw);
    
    MDA_TASK_BASE::starting_depth = attitude_input->depth();
    // gate depth
    set (DEPTH, HARDCODED_DEPTH-50/*MDA_TASK_BASE::starting_depth+GATE_DELTA_DEPTH*/);
    set (DEPTH, HARDCODED_DEPTH/*MDA_TASK_BASE::starting_depth+GATE_DELTA_DEPTH*/);

    // go to the starting orientation in case sinking changed it
    set (YAW, starting_yaw);

    static TIMER timer; // keeps track of time spent in each state
    static TIMER master_timer; // keeps track of time spent not having found the target
    static TIMER full_detect_timer; // keeps track of time since the last full detect
    timer.restart();
    master_timer.restart();
    full_detect_timer.restart();

    while (1) {
        IplImage* frame = image_input->get_image();
        if (!frame) {
            ret_code = TASK_ERROR;
            break;
        }
        MDA_VISION_RETURN_CODE vision_code = gate_vision.filter(frame);

        // clear dwn image. RZ - do we need this?
        // This ensures the other camera is properly logged
        // and that the webcam cache is cleared so it stays in sync - VZ
        image_input->ready_image(DWN_IMG);

        // static
        //static int prev_t = -1;

        /**
        * Basic Algorithm: (repeat)
        *  - Go straight foward in STARTING state until we see anything
        *  
        *    - If we saw a ONE_SEGMENT, calculate the course we should take to allow the segment to remain in view.
        *    - If we saw a FULL_DETECT, change course to face it
        *  - Always go forwards in increments and stop for 1 seconds to stare each time.
        */
        if (!done_gate) {
            if (state == STARTING) {
                printf ("Starting: Moving Foward at High Speed\n");
                set (SPEED, 9);

                if (master_timer.get_time() > MASTER_TIMEOUT) {
                    printf ("Master Timer Timeout!!\n");
                    return TASK_MISSING;
                }
                else if (vision_code == FULL_DETECT) {
                    printf ("FAST Foward: Full Detect\n");
                    //int ang_x = gate_vision.get_angular_x();
                    //set_yaw_change(ang_x);

                    if (gate_vision.get_range() < 420) {
                        done_gate = true;
                        printf ("Range = %d, Approaching Gate\n", gate_vision.get_range());
                    }

                    timer.restart();
                    full_detect_timer.restart();
                    master_timer.restart();
                }
                /*if (gate_vision.latest_frame_is_valid()) {
                    set (SPEED, 0);
                    master_timer.restart();
                    timer.restart();
                    gate_vision.clear_frames();
                    state = SLOW_FOWARD;
                }*/
            }
            else if (state == SLOW_FOWARD) {
                printf ("Slow Foward: Moving foward a little\n");
                set (SPEED, 4);

                if (timer.get_time() > 3) {
                    timer.restart();
                    gate_vision.clear_frames();
                    state = PANNING;
                }
                else if (vision_code == FULL_DETECT) {
                    printf ("Slow Foward: Full Detect\n");
                    int ang_x = gate_vision.get_angular_x();
                    set_yaw_change(ang_x);

                    if (gate_vision.get_range() < 420) {
                        done_gate = true;
                        printf ("Range = %d, Approaching Gate\n", gate_vision.get_range());
//.........这里部分代码省略.........

MDA_TASK_RETURN_CODE MDA_TASK_PATH_SKIP:: run_task() {
    puts("Press q to quit");

    MDA_VISION_MODULE_PATH path_vision;
    MDA_TASK_RETURN_CODE ret_code = TASK_MISSING;

    bool done_skip = false;

    TIMER t;

    // sink to starting depth
    //set (DEPTH, MDA_TASK_BASE::starting_depth+PATH_DELTA_DEPTH);
    set (DEPTH, 500);
    set (DEPTH, 600);
    set (DEPTH, 750);

    t.restart();
    while (t.get_time() < 6) {
        set (SPEED, 10);
    }
    set (SPEED, 0);
/*
    while (1) {
        IplImage* frame = image_input->get_image(DWN_IMG);
        if (!frame) {
            ret_code = TASK_ERROR;
            break;
        }
        MDA_VISION_RETURN_CODE vision_code = path_vision.filter(frame);

        // clear fwd image
        image_input->ready_image(FWD_IMG);

        if (vision_code == FULL_DETECT) {
            // Get path out of vision
            move(FORWARD, 1);
            // Reset back to 0
            images_checked = 0;
        } else {
            images_checked++;
            if (images_checked >= num_images_to_check) {
                done_skip = true;
                break;
            }
        }

        // Ensure debug messages are printed
        fflush(stdout);
        // Exit if instructed to
        char c = cvWaitKey(TASK_WK);
        if (c != -1) {
            CharacterStreamSingleton::get_instance().write_char(c);
        }
        if (CharacterStreamSingleton::get_instance().wait_key(1) == 'q'){
            ret_code = TASK_QUIT;
            break;
        }
    }
*/
    stop();

    if(done_skip){
        ret_code = TASK_DONE;
    }

    return ret_code;
}

MDA_TASK_RETURN_CODE MDA_TASK_GOALPOST:: run_task() {
    puts("Press q to quit");

    MDA_VISION_MODULE_GOALPOST goalpost_vision;
    MDA_TASK_RETURN_CODE ret_code = TASK_MISSING;
    
    int starting_yaw = attitude_input->yaw();
    printf("Starting yaw: %d\n", starting_yaw);

    bool done_goalpost = false;

    int GOALPOST_DEPTH;
    read_mv_setting ("hacks.csv", "GOALPOST_DEPTH", GOALPOST_DEPTH);
    
    if (GOALPOST_DEPTH > 500)
        set(DEPTH, 500);
    if (GOALPOST_DEPTH > 600)
        set(DEPTH, 600);
    set(DEPTH, GOALPOST_DEPTH);
    set(YAW, starting_yaw);

    TIMER t;
    t.restart();

    while (1) {
        IplImage* frame = image_input->get_image();
        if (!frame) {
            ret_code = TASK_ERROR;
            break;
        }
        MDA_VISION_RETURN_CODE vision_code = goalpost_vision.filter(frame);

        // clear dwn image
        int down_frame_ready = image_input->ready_image(DWN_IMG);
        (void) down_frame_ready;

        if(!done_goalpost){
            if (vision_code == FATAL_ERROR) {
                ret_code = TASK_ERROR;
                break;
            }
            else if (vision_code == NO_TARGET) {
                set(SPEED, 5);

                if (t.get_time() > MASTER_TIMEOUT) {
                    stop();
                    return TASK_MISSING;
                }
            }
            else if (vision_code == FULL_DETECT) {
                int ang_x = goalpost_vision.get_angular_x();
                int ang_y = goalpost_vision.get_angular_y();
                int range = goalpost_vision.get_range();
                int depth_change = tan(ang_y*0.017453) * range; 

                // if we can see full goalpost and range is less than 400 we are done the frame part
                if (goalpost_vision.get_range() < 350) {
                    t.restart();
                    while (t.get_time() < 5) {
                        set (SPEED, 8);
                    }
                    stop();
                    done_goalpost = true;
                    ret_code = TASK_DONE;
                    break;
                }

                if(fabs(ang_y) > 30.0) {
                    stop();
                    move(SINK, depth_change);
                }
                else if(abs(ang_x) > 10.0) {
                    stop();
                    move(RIGHT, ang_x);
                }
                else {
                    set(SPEED, 5);
                }
            }
            else {
                printf ("Error: %s: line %d\ntask module recieved an unhandled vision code.\n", __FILE__, __LINE__);
                exit(1);
            }
        }

        // Ensure debug messages are printed
        fflush(stdout);
        // Exit if instructed to
        char c = cvWaitKey(TASK_WK);
        if (c != -1) {
            CharacterStreamSingleton::get_instance().write_char(c);
        }
        if (CharacterStreamSingleton::get_instance().wait_key(1) == 'q'){
            stop();
            ret_code = TASK_QUIT;
            break;
        }
    }

    return ret_code;
//.........这里部分代码省略.........

MDA_TASK_RETURN_CODE MDA_TASK_PATH:: run_task() {
    puts("Press q to quit");

    MDA_VISION_MODULE_PATH path_vision;
    MDA_VISION_MODULE_GATE gate_vision;
    MDA_TASK_RETURN_CODE ret_code = TASK_MISSING;
    
    TASK_STATE state = STARTING_GATE;
    bool done_gate = false;
    bool done_path = false;

    // read the starting orientation
    int starting_yaw = attitude_input->yaw();
    printf("Starting yaw: %d\n", starting_yaw);
    
    int GATE_DEPTH;
    read_mv_setting ("hacks.csv", "GATE_DEPTH", GATE_DEPTH);

    // gate depth
    if (HARDCODED_DEPTH > 350)
        set (DEPTH, 350);
    if (HARDCODED_DEPTH > 400)
        set (DEPTH, 400);
    set (DEPTH, GATE_DEPTH);
    //set(DEPTH, 100);

    // go to the starting orientation in case sinking changed it
    set (YAW, starting_yaw);

    //TIMER master_timer;
    TIMER timer;
    timer.restart();

    while (1) {

        IplImage* frame = NULL;
        MDA_VISION_RETURN_CODE vision_code = NO_TARGET;
        MDA_VISION_RETURN_CODE gate_vision_code = NO_TARGET;
        (void) gate_vision_code;
        /*if (!done_gate) {
            frame = image_input->get_image(FWD_IMG);
            if (!frame) {
                ret_code = TASK_ERROR;
                break;
            }
            gate_vision_code = gate_vision.filter(frame);
        }*/

        frame = image_input->get_image(DWN_IMG);
        if (!frame) {
            ret_code = TASK_ERROR;
            break;
        }
        vision_code = path_vision.filter(frame);
        
        // clear fwd image. RZ - do we need this?
        // This ensures the other camera is properly logged
        // and that the webcam cache is cleared so it stays in sync - VZ
        //image_input->ready_image(FWD_IMG);

        /**
        * Basic Algorithm:
        *  - Go to path
        *  - Align with path
        */

        if (!done_gate) {
            if (state == STARTING_GATE) {
                printf ("Starting Gate: Moving Foward at High Speed\n");
                set (SPEED, 5);

                if (timer.get_time() > MASTER_TIMEOUT) {
                    printf ("Starting Gate: Master Timer Timeout!!\n");
                    return TASK_MISSING;
                }
                /*else if (gate_vision_code == FULL_DETECT) {
                    printf ("Starting Gate: Full Detect\n");
                    int ang_x = gate_vision.get_angular_x();
                    set_yaw_change(ang_x);

                    if (gate_vision.get_range() < 420) { // finished the gate
                        printf ("Range = %d, Approaching Gate\n", gate_vision.get_range());
                        timer.restart();
                        while (timer.get_time() < 3) {
                            set(SPEED, 6);
                        }
                        set(SPEED, 0);
                        printf ("Gate Task Done!!\n");

                        // get ready for path task
                        done_gate = true;
                        set(YAW, starting_yaw);
                        state = STARTING_PATH;
                    }

                    timer.restart();
                }*/

                // if path vision saw something, go do the path task
                if (vision_code != NO_TARGET) {
//.........这里部分代码省略.........

MDA_TASK_RETURN_CODE MDA_TASK_BUOY:: run_single_buoy(int buoy_index, BUOY_COLOR color) {
    puts("Press q to quit");

    assert (buoy_index >= 0 && buoy_index <= 1);
    
    MDA_VISION_MODULE_BUOY buoy_vision;
    MDA_TASK_RETURN_CODE ret_code = TASK_MISSING;

    /// Here we store the starting attitude vector, so we can return to this attitude later
    int starting_yaw = attitude_input->yaw();
    printf("Starting yaw: %d\n", starting_yaw);

    //set (DEPTH, 400);

    TASK_STATE state = STARTING;
    bool done_buoy = false;
    static TIMER timer;
    static TIMER master_timer;
    timer.restart();
    master_timer.restart();


//###### hack code for competition
    set (SPEED, 0);
    
    int hack_depth, hack_time;
    read_mv_setting ("hacks.csv", "BUOY_DEPTH", hack_depth);
    read_mv_setting ("hacks.csv", "BUOY_TIME", hack_time);
    printf ("Buoy: going to depth %d\n", hack_depth);
        fflush(stdout);
    if (hack_depth > 500)
	set (DEPTH, 500);
    if (hack_depth > 600)
        set (DEPTH, 600);
    set (DEPTH, hack_depth);
    set (YAW, starting_yaw);

    printf ("Buoy: moving forward for %d seconds\n", hack_time);
        fflush(stdout);
    timer.restart();
    while (timer.get_time() < hack_time) {
        set (SPEED, 8);
    }
    set(SPEED, 0);

    if (hack_depth > 600)
         set (DEPTH, 600);
    if (hack_depth > 500)
        set (DEPTH, 500);
    set (YAW, starting_yaw);
    return TASK_DONE;
//###### end hack code for competition
    /**
    * Basic Algorithm
    *  - Assume for now that we just want to hit the cylindrical buoys when they're red
    *  - We want to search for 1 or 2 buoys. If we find both:
    *    - Are both non-red and/or cycling? Go for the one indicated by buoy_index
    *    - Is one non-red and/or cycling? Go for that one
    *    - Do we only see one buoy? Go there if non-red and/or cycling
    *
    */
    
    while (1) {
        IplImage* frame = image_input->get_image();
        if (!frame) {
            ret_code = TASK_ERROR;
            break;
        }

        MDA_VISION_RETURN_CODE vision_code = buoy_vision.filter(frame);
        (void) vision_code;
        // clear dwn image - RZ: do we need this?
        //int down_frame_ready = image_input->ready_image(DWN_IMG);
        //(void) down_frame_ready;

        bool valid[2] = {false, false};
        int ang_x[2], ang_y[2];
        int range[2];
        int color[2];
        static bool color_cycling[2] = {false, false};
        static int curr_index = -1;
        static int prev_time = -1;
        static int prev_color = -1;
        static int n_valid_color_find_frames = 0;

        if (!done_buoy) {
            // state machine
            if (state == STARTING) {
                // here we just move forward until we find something, and pan if we havent found anything for a while
                printf ("Starting: Moving Foward for 1 meter\n");
                move (FORWARD, 1);

                if (timer.get_time() > 1) {
                    set (SPEED, 0);
                    timer.restart();
                    buoy_vision.clear_frames();
                    state = STOPPED;
                }
            }
            else if (state == STOPPED) {
//.........这里部分代码省略.........