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()); } timer.restart(); full_detect_timer.restart(); master_timer.restart(); } if (master_timer.get_time() > MASTER_TIMEOUT) { printf ("Master Timer Timeout!!\n"); return TASK_MISSING; } } else if (state == STOPPED) { // if havent spent 1 second in this state, keep staring /*if (timer.get_time() < 1) { printf ("Stopped: Collecting Frames\n"); } else {*/ if (vision_code == NO_TARGET) { printf ("Stopped: No target\n"); if (master_timer.get_time() > 60) { // we've seen nothing for 60 seconds printf ("Master Timer Timeout!!\n"); return TASK_MISSING; } if (timer.get_time() > 3) { printf ("Stopped: Timeout\n"); timer.restart(); state = SLOW_FOWARD; } } else if (vision_code == ONE_SEGMENT) { printf ("Stopped: One Segment\n"); //int ang_x = gate_vision.get_angular_x(); // if segment too close just finish if (gate_vision.get_range() < 240) { printf ("One Segment with range too low. Ending task.\n"); done_gate = true; } else if (gate_vision.get_range() < 470 && full_detect_timer.get_time() > 10) { timer.restart(); master_timer.restart(); //prev_t = -1; //state = PANNING; } // only execute turn if the segment is close to out of view and no other options /*else if (ang_x >= 40) { ang_x -= 20; printf ("Moving Left on One Segment %d Degrees\n", ang_x); move (RIGHT, ang_x); gate_vision.clear_frames(); } else if (ang_x <= -40) { ang_x += 20; printf ("Moving Left on One Segment %d Degrees\n", ang_x); move (RIGHT, ang_x); gate_vision.clear_frames(); }*/ if (timer.get_time() > 2) { timer.restart(); master_timer.restart(); state = SLOW_FOWARD; } } else if (vision_code == FULL_DETECT) { printf ("Stopped: Full Detect\n"); int ang_x = gate_vision.get_angular_x(); move (RIGHT, 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(); state = SLOW_FOWARD; } else { printf ("Error: %s: line %d\ntask module recieved an unhandled vision code.\n", __FILE__, __LINE__); exit(1); } //} // collecting frames } // state else if (state == PANNING) { // pan and look for a frame with 2 segments /*printf ("Panning\n"); int t = timer.get_time(); if (t < PAN_TIME_HALF && t != prev_t) { // pan left for first 6 secs set_yaw_change (-20); prev_t = timer.get_time(); } else if (t < 3*PAN_TIME_HALF && t != prev_t) { // pan right for next 10 secs set_yaw_change (20); prev_t = timer.get_time(); } else if (t >= 3*PAN_TIME_HALF) { // stop pan and reset printf ("Pan completed\n"); set (YAW, starting_yaw); gate_vision.clear_frames(); master_timer.restart(); timer.restart(); state = STOPPED; } else if (gate_vision.latest_frame_is_two_segment()) { printf ("Two segment frame found - stopping pan\n"); set_yaw_change (0); gate_vision.clear_frames(); master_timer.restart(); full_detect_timer.restart(); timer.restart(); state = STOPPED; }*/ } else if (state == APPROACH) { } } // done_gate else { // charge foward for 2 secs //set(YAW, starting_yaw); timer.restart(); while (timer.get_time() < 2) { set(SPEED, 6); } set(SPEED, 0); printf ("Gate Task Done!!\n"); return TASK_DONE; } // 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_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) { printf ("\nSaw Path! Going to Path vision!"); done_gate = true; set(SPEED, 0); set(YAW, starting_yaw); timer.restart(); while (timer.get_time() < 2); state = STARTING_PATH; } } } else if (!done_path) { // calculate some values that we will need float xy_ang = path_vision.get_angular_x(); // angle equal to atan(x/y) float pos_angle = path_vision.get_angle(); // PA, equal to orientation of the thing int pix_x = path_vision.get_pixel_x(); int pix_y = path_vision.get_pixel_y(); int pix_distance = sqrt(pow(pix_y,2) + pow(pix_x,2)); printf("xy_distance = %d xy_angle = %5.2f\n==============================\n", pix_distance, pos_angle); if (state == STARTING_PATH) { if (vision_code == NO_TARGET) { printf ("Starting: No target\n"); set(SPEED,3); if (timer.get_time() > MASTER_TIMEOUT) { // timeout printf ("Master Timeout\n"); return TASK_MISSING; } } else if (vision_code == UNKNOWN_TARGET) { printf ("Starting: Unknown target\n"); timer.restart(); } else { printf ("Starting: Good\n"); set(SPEED, 0); timer.restart(); state = AT_SEARCH_DEPTH; } } else if (state == AT_SEARCH_DEPTH){ if (vision_code == NO_TARGET) { if (timer.get_time() < 1) { continue; } printf ("Searching: No target\n"); if (timer.get_time() > 11) { // timeout, go back to starting state printf ("Timeout\n"); //set (YAW, starting_yaw); timer.restart(); path_vision.clear_frames(); state = STARTING_PATH; } else if (timer.get_time() % 2 == 0) { // spin around a bit to try to re-aquire? //move (RIGHT, 45); } else { // just wait } } else if (vision_code == UNKNOWN_TARGET) { printf ("Searching: Unknown target\n"); timer.restart(); } else { timer.restart(); printf ("Searching: Good\n"); if(pix_distance > frame->height/5){ // move over the path if (abs(xy_ang) < 10) { // go fowards or backwards depending on the pix_y value if (pix_y >= 0) { printf ("Set speed foward\n"); set(SPEED, 3); } else { printf ("Set speed reverse\n"); set(SPEED, -3); } } else { if (abs(xy_ang) > 90 ) { // turn different direction based on pix_y value xy_ang = (xy_ang > 0) ? xy_ang - 180 : xy_ang + 180; } printf("Turning %s %d degrees (xy_ang)\n", (abs(xy_ang) > 0) ? "Right" : "Left", static_cast<int>(abs(xy_ang))); set(SPEED, 0); move(RIGHT, xy_ang); path_vision.clear_frames(); } } else { // we are over the path, sink and try align state set(SPEED, 0); move(SINK, ALIGN_DELTA_DEPTH); timer.restart(); path_vision.clear_frames(); state = AT_ALIGN_DEPTH; } } } else if (state == AT_ALIGN_DEPTH) { if (vision_code == NO_TARGET) { // wait for timeout printf ("Aligning: No target\n"); if (timer.get_time() > 6) { // timeout printf ("Timeout\n"); move(RISE, ALIGN_DELTA_DEPTH); timer.restart(); path_vision.clear_frames(); state = AT_SEARCH_DEPTH; } } else if (vision_code == UNKNOWN_TARGET) { printf ("Aligning: Unknown target\n"); timer.restart(); } else { if (abs(pos_angle) >= 10) { move(RIGHT, pos_angle); path_vision.clear_frames(); timer.restart(); } else { done_path = true; } } } } // done_path else { // wait foward 2 secs, then charge foward for 2 secs timer.restart(); while (timer.get_time() < 2) { set(SPEED, 0); } while (timer.get_time() < 2) { set(SPEED, 4); } set(SPEED, 0); printf ("Path Task Done!!\n"); return TASK_DONE; } // 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; } } if(done_path){ ret_code = TASK_DONE; } return ret_code; }
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) { if (timer.get_time() < 0) { printf ("Stopped: Collecting Frames\n"); } else { get_data_from_frame (&buoy_vision, valid, ang_x, ang_y, range, color); if (!valid[0] && !valid[1]) { // no buoys printf ("Stopped: No target\n"); if (master_timer.get_time() > 60) { // we've seen nothing for 60 seconds printf ("Master Timer Timeout!!\n"); return TASK_MISSING; } if (timer.get_time() > 2) { printf ("Stopped: Timeout\n"); timer.restart(); state = STARTING; } } else { // turn towards the buoy we want // chose buoy if (valid[0] && valid[1]) curr_index = buoy_index; else curr_index = valid[0] ? 0 : 1; printf ("Stopped: Identified buoy %d (%s) as target\n", curr_index, color_int_to_string(color[curr_index]).c_str()); move (RIGHT, ang_x[curr_index]); timer.restart(); master_timer.restart(); prev_color = color[curr_index]; prev_time = -1; buoy_vision.clear_frames(); n_valid_color_find_frames = 0; state = FIND_COLOR; } } } else if (state == FIND_COLOR) { // stare for 6 seconds, check if the buoy color changes int t = timer.get_time(); if (t >= 4) { if (n_valid_color_find_frames <= 2) { printf ("Find Color: Not enough good frames (%d).\n", n_valid_color_find_frames); timer.restart(); master_timer.restart(); state = STARTING; } else if (color_cycling[curr_index] || prev_color != MV_RED) { printf ("Find Color: Finished. Must approach buoy %d.\n", curr_index); timer.restart(); master_timer.restart(); state = APPROACH; } else { printf ("Find Color: Finished. No need to do buoy %d.\n", curr_index); done_buoy = true; return TASK_QUIT; } } else if (t != prev_time) { printf ("Find_Color: examined buoy %d for %d seconds.\n", curr_index, t); get_data_from_frame (&buoy_vision, valid, ang_x, ang_y, range, color, curr_index); if (valid[curr_index]) { if (color[curr_index] != prev_color) { printf ("\tFound buoy %d as color cycling.\n", curr_index); color_cycling[curr_index] = true; } prev_time = t; n_valid_color_find_frames++; } } } else if (state == APPROACH) { get_data_from_frame (&buoy_vision, valid, ang_x, ang_y, range, color, curr_index); if (valid[curr_index]) { printf ("Approach[%d]: range=%d, ang_x=%d\n", curr_index, range[curr_index], ang_x[curr_index]); if (range[curr_index] > 100) { // long range = more freedom to turn/sink if (abs(ang_x[curr_index]) > 5) { set(SPEED, 0); move(RIGHT, ang_x[curr_index]); buoy_vision.clear_frames(); } /*else if (tan(ang_y[curr_index])*range[curr_index] > 50) { // depth in centimeters set(SPEED, 0); move (SINK, 25); // arbitrary rise for now }*/ else { set(SPEED, 1); } } else { if (abs(ang_x[curr_index]) > 10) { set(SPEED, 0); move(RIGHT, ang_x[curr_index]); buoy_vision.clear_frames(); } else { done_buoy = true; } } timer.restart(); master_timer.restart(); } else { set(SPEED, 0); if (timer.get_time() > 4) { // 4 secs without valid input printf ("Approach: Timeout"); timer.restart(); state = STOPPED; } } } } // done_buoy else { // done_buoy // charge forwards, then retreat back some number of meters, then realign sub to starting attitude printf("Ramming buoy\n"); timer.restart(); while (timer.get_time() < 2) move(FORWARD, 2); stop(); // retreat backwards printf("Reseting Position\n"); timer.restart(); while (timer.get_time() < 3) move(REVERSE, 2); stop(); ret_code = TASK_DONE; 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'){ stop(); ret_code = TASK_QUIT; break; } } return ret_code; }