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;
}
