From: Tran Duy Khanh Date: Fri, 2 May 2008 08:58:19 +0000 (+0200) Subject: robofsm: ball pick up and deposition updated, some part of the homologation program X-Git-Tag: eb2008~49^2 X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/9d6124cdb08b8dbf372b9fe36f7ad22d26c9a534 robofsm: ball pick up and deposition updated, some part of the homologation program have been rewritten. Homologation program almost done, need to be tested. --- diff --git a/src/robofsm/eb2008/fsmmain.cc b/src/robofsm/eb2008/fsmmain.cc index f716f514..c7fe5d77 100644 --- a/src/robofsm/eb2008/fsmmain.cc +++ b/src/robofsm/eb2008/fsmmain.cc @@ -31,16 +31,16 @@ #ifdef COMPETITION #define WAIT_FOR_START #define COMPETITION_TIME_DEFAULT 90 -#define WAIT_FOR_DEPOSITION_DEFAULT 60 +#define TIME_TO_DEPOSITE_DEFAULT 60 #else #undef WAIT_FOR_START #define COMPETITION_TIME_DEFAULT 900 -#define WAIT_FOR_DEPOSITION_DEFAULT 60 +#define TIME_TO_DEPOSITE_DEFAULT 60 #endif /* competition time in seconds */ #define COMPETITION_TIME COMPETITION_TIME_DEFAULT -#define WAIT_FOR_DEPOSITION WAIT_FOR_DEPOSITION_DEFAULT +#define TIME_TO_DEPOSITE TIME_TO_DEPOSITE_DEFAULT /* competition time in seconds */ enum { @@ -49,6 +49,10 @@ enum { CENTER }; +/************************************************************************ + * MISC FUNCTIONS + ************************************************************************/ + /** * Convert back sharps' measured values to mm. * @@ -193,7 +197,7 @@ void *wait_for_end(void *arg) */ void *wait_to_deposition(void *arg) { - sleep(WAIT_FOR_DEPOSITION); + sleep(TIME_TO_DEPOSITE); FSM_SIGNAL(MAIN, EV_SHORT_TIME_TO_END, NULL); return NULL; } @@ -230,22 +234,28 @@ void robot_go_backward_to_point(struct ref_pos_type des_pos) robot_trajectory_add_final_point_notrans(des_pos.x, des_pos.y, NO_TURN()); } +/************************************************************************ + * FSM STATES DECLARATION + ************************************************************************/ + +/* initial and starting states */ FSM_STATE_DECL(init); +FSM_STATE_DECL(wait_for_start); +/* movement states */ FSM_STATE_DECL(go_to_our_white_dispenser); FSM_STATE_DECL(go_to_our_white_dispenser2); +FSM_STATE_DECL(go_far_from_dispenser); +FSM_STATE_DECL(go_from_our_dispenser); +FSM_STATE_DECL(go_to_container); +/* ball and carousel states */ FSM_STATE_DECL(get_balls); FSM_STATE_DECL(next_carousel_position); -FSM_STATE_DECL(wait_for_start); -FSM_STATE_DECL(go_to_container); FSM_STATE_DECL(deposite_balls); -FSM_STATE_DECL(go_far_from_dispenser); -FSM_STATE_DECL(go_from_our_dispenser); -/** - * Set starting position, playground`s safety zone and other obstacles. - * - * @ingroup fsmmain - */ +/************************************************************************ + * INITIAL AND STARTING STATES + ************************************************************************/ + FSM_STATE(init) { /* start event ocurred */ @@ -262,7 +272,10 @@ FSM_STATE(init) close_back_door(); // FSM_TRANSITION(wait_for_start); // FSM_TRANSITION(go_to_container); - FSM_TRANSITION(get_balls); +// FSM_TRANSITION(get_balls); + robot.carousel_cnt = 5; + robot.carousel_pos = 0; + FSM_TRANSITION(deposite_balls); break; default: break; } @@ -290,6 +303,10 @@ FSM_STATE(wait_for_start) } } +/************************************************************************ + * MOVEMENT STATES + ************************************************************************/ + FSM_STATE(go_to_our_white_dispenser) { switch (FSM_EVENT) { @@ -356,6 +373,43 @@ FSM_STATE(go_from_our_dispenser) } } +#define GO_TO_CONTAINER_TIMER 10000 + +FSM_STATE(go_to_container) +{ + switch (FSM_EVENT) { + case EV_ENTRY: + off_brush_left(); + off_brush_right(); + off_bagr(); + robot_trajectory_new_backward(NULL); + robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.6, 1.0); + robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M - 0.4, + 0, NO_TURN()); + FSM_TIMER(GO_TO_CONTAINER_TIMER); + break; + case EV_TIMER: + case EV_MOTION_DONE: + if (closed_to_container()) { + robot.carousel_pos = 0; + FSM_TRANSITION(deposite_balls); + } else { + DBG("FIXME: go_closer_to_container\n"); + } + break; + default: break; + } +} + +/************************************************************************ + * BALLS AND CAROUSEL MANIPULATION STATES + ************************************************************************/ + +#define MAX_GET_BALL_ATTEMPS 5 +#define GET_BALL_TIMER 1500 +#define WAIT_BALL_INSIDE 5000 +#define GET_BALL_BAGR_SPEED 120 + FSM_STATE(get_balls) { static int get_ball_attemps = 0; @@ -363,8 +417,9 @@ FSM_STATE(get_balls) switch (FSM_EVENT) { case EV_ENTRY: + case EV_RETURN: open_bottom_door(); - set_bagr(120); + set_bagr(GET_BALL_BAGR_SPEED); brushes_drives_in(); FSM_TIMER(500); break; @@ -373,146 +428,104 @@ FSM_STATE(get_balls) ball_inside = 0; close_bottom_door(); off_bagr(); - if (robot.carousel_cnt >= CAROUSEL_SIZE) { + if (robot.carousel_cnt >= BALL_TO_COLLECT) { + /* FIXME: just to test deposition */ + robot.carousel_pos = 0; + FSM_TRANSITION(deposite_balls); // printf("go_to_container\n"); // FSM_TRANSITION(go_to_container); - FSM_TRANSITION(deposite_balls); } else { - FSM_TRANSITION(next_carousel_position); + robot.carousel_pos = + (robot.carousel_pos+2) % CAROUSEL_SIZE; + SUBFSM_TRANSITION( + next_carousel_position, NULL); } - } else if (get_ball_attemps++ < 5) { - FSM_TIMER(1500); + } else if (get_ball_attemps++ < MAX_GET_BALL_ATTEMPS) { + FSM_TIMER(GET_BALL_TIMER); } else { - printf("go_to_container\n"); + /* FIXME: */ + DBG("go_to_container\n"); // FSM_TRANSITION(go_to_container); } break; case EV_BALL_INSIDE: + /* ball is already inside */ if (ball_inside) break; ball_inside = 1; - robot.carousel[robot.carousel_pos % CAROUSEL_SIZE] = (enum ball_color)robot.cmu.color; + robot.carousel[robot.carousel_pos % CAROUSEL_SIZE] = + (enum ball_color)robot.cmu.color; robot.carousel_cnt++; - printf("cnt=%d\n", robot.carousel_cnt); - FSM_TIMER(5000); + DBG("ball cnt=%d\n", robot.carousel_cnt); + FSM_TIMER(WAIT_BALL_INSIDE); break; default: break; } } +#define MAX_CAROUSEL_ATTEMPTS 3 +#define CAROUSEL_ATTEMPT_TIMER 1000 + FSM_STATE(next_carousel_position) { - static int carousel_attempts = 0; + static int carousel_attempts; int rv; switch (FSM_EVENT) { case EV_ENTRY: - robot.carousel_pos = - (robot.carousel_pos+2) % CAROUSEL_SIZE; + carousel_attempts = 0; set_carousel(robot.carousel_pos); - FSM_TIMER(1000); + FSM_TIMER(CAROUSEL_ATTEMPT_TIMER); break; case EV_TIMER: carousel_attempts++; + /* check out, if the carousel reached position */ ROBOT_LOCK(carousel); rv = (robot.drives.carousel_pos == robot.carousel_pos); ROBOT_UNLOCK(carousel); - if (rv) { - FSM_TRANSITION(get_balls); - } else if (carousel_attempts < 3) { - FSM_TIMER(1000); + if (!rv && carousel_attempts++ < MAX_CAROUSEL_ATTEMPTS) { + FSM_TIMER(CAROUSEL_ATTEMPT_TIMER); + } else if(rv) { + DBG("carousel reached the position.\n"); + SUBFSM_RET(NULL); } else { - printf("go_to_container\n"); -// FSM_TRANSITION(go_to_container); + DBG("FIXME: carousel lost.\n"); + SUBFSM_RET(NULL); } break; default: break; } } -FSM_STATE(next_carousel_position2) -{ - static int carousel_attempts = 0; - int rv; - - switch (FSM_EVENT) { - case EV_ENTRY: - set_carousel(robot.carousel_pos); - break; - case EV_TIMER: - carousel_attempts++; - ROBOT_LOCK(carousel); - rv = (robot.drives.carousel_pos == robot.carousel_pos); - ROBOT_UNLOCK(carousel); - if (rv) { - FSM_TRANSITION(deposite_balls); - } else if (carousel_attempts < 3) { - FSM_TIMER(500); - } else { - FSM_TRANSITION(deposite_balls); - } - break; - default: break; - } -} +#define WAIT_FOR_DEPOSITION_TIMER 1500 -FSM_STATE(go_to_container) +FSM_STATE(deposite_balls) { switch (FSM_EVENT) { case EV_ENTRY: - off_brush_left(); - off_brush_right(); - off_bagr(); - robot_trajectory_new_backward(NULL); - robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.6, 1.0); - robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M - 0.4, - 0, NO_TURN()); - FSM_TIMER(10000); - break; - case EV_TIMER: - case EV_MOTION_DONE: - if (closed_to_container()) { - FSM_TRANSITION(deposite_balls); + if (robot.carousel_pos < CAROUSEL_SIZE) { + close_back_door(); + robot.carousel_pos++; + robot.carousel_cnt--; + SUBFSM_TRANSITION(next_carousel_position, NULL); } else { - printf("FIXME:\n"); + robot.carousel_cnt = 0; + /* FIXME: */ + DBG("go_to_our_white_dispenser\n"); +// FSM_TRANSITION(go_to_our_white_dispenser); } break; - default: break; - } -} - -FSM_STATE(deposite_balls) -{ - int carousel_pos = 0; - int wait_for_deposition = 0; - - switch (FSM_EVENT) { - case EV_ENTRY: - set_carousel(carousel_pos++); -// robot.carousel_cnt--; -// if (robot.carousel_cnt <= 0) { -// robot.carousel_cnt = 0; -// FSM_TRANSITION(go_to_our_white_dispenser); -// } -// ROBOT_LOCK(drives); -// robot.carousel_pos = robot.drives.carousel_pos; -// ROBOT_UNLOCK(drives); - FSM_TIMER(500); + case EV_RETURN: + DBG("open_back_door\n"); + open_back_door(); + FSM_TIMER(WAIT_FOR_DEPOSITION_TIMER); break; case EV_TIMER: - if (!wait_for_deposition) { - open_back_door(); - wait_for_deposition = 1; - } - - FSM_TIMER(500); - if (carousel_pos > 4) - printf("ok\n"); -// FSM_TRANSITION(go_to_our_white_dispenser); -// FSM_TRANSITION(next_carousel_position2); close_back_door(); - set_carousel(carousel_pos++); + robot.carousel_pos++; + DBG("carousel_cnt = %d\n", robot.carousel_cnt); + FSM_TRANSITION(deposite_balls); break; default: break; } diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c index 5e3ad041..7d8b5092 100644 --- a/src/robofsm/eb2008/robot_orte.c +++ b/src/robofsm/eb2008/robot_orte.c @@ -188,9 +188,9 @@ void rcv_drives_cb(const ORTERecvInfo *info, void *vinstance, switch (info->status) { case NEW_DATA: - ROBOT_LOCK(carousel); + ROBOT_LOCK(drives); robot.drives = *instance; - ROBOT_UNLOCK(carousel); + ROBOT_UNLOCK(drives); break; case DEADLINE: DBG("ORTE deadline occurred - drives receive\n");