/*
- * fsmmain.cc 09/04/..
+ * competition.cc 2010/04/30
*
- * Robot's main control program (Eurobot 2009).
+ * Robot's control program intended for homologation (approval phase) on Eurobot 2009.
*
- * Copyright: (c) 2009 CTU Dragons
+ * Copyright: (c) 2009 - 2010 CTU Dragons
* CTU FEE - Department of Control Engineering
* License: GNU GPL v.2
*/
#include <robomath.h>
#include <string.h>
#include <robodim.h>
-#include <stdbool.h>
#include <error.h>
-#include <actuators.h>
+#include "corns_configs.h"
+#include "actuators.h"
+#include "match-timing.h"
+#include "eb2010misc.h"
+#include "common-states.h"
-#ifdef COMPETITION
-#define WAIT_FOR_START
-#define COMPETITION_TIME_DEFAULT 90
-#define TIME_TO_DEPOSITE_DEFAULT 60
-#else
-#undef WAIT_FOR_START
-#define COMPETITION_TIME_DEFAULT 900
-#define TIME_TO_DEPOSITE_DEFAULT 60
-#endif
-
-/* competition time in seconds */
-#define COMPETITION_TIME COMPETITION_TIME_DEFAULT
-#define TIME_TO_DEPOSITE TIME_TO_DEPOSITE_DEFAULT
-/* competition time in seconds */
-
-/************************************************************************
- * SUBFSM's return values ...
- ************************************************************************/
-
-typedef enum {
- LOAD_SUCCESS = 0,
- LOAD_FAIL,
-} subfsm_ret_val;
-
-#define FSM_EVENT_RET_VAL ((subfsm_ret_val)FSM_EVENT_INT)
-
-/************************************************************************
- * Trajectory constraints used, are initialized in the init state
- ************************************************************************/
-
-struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
-
-/************************************************************************
- * Variables related to puck collecting
- ************************************************************************/
-
-int free_puck_to_try_to_get_next; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
-int pucks_at_once; // number of pucks to load at once (maximum number of pucks we want to have in robot)
-
-/************************************************************************
- * Definition of particular "free pucks pick up sequences" and related stuff
- ************************************************************************/
-
-// FIXME: has to be redefined and tested
-const int free_puck_pick_up_sequence[][6][3] = {
-/* nx, ny, arrival angle */
- { /* lot 1 */
- {0, 3, 135},
- {1, 3, 180},
- {2, 3, 180},
- {2, 0, 90},
- {1, 0, 45},
- {0, 0, 0},
- },
- {
- {0, 3, 135},
- {2, 3, 90},
- {2, 2, 90},
- {2, 1, 90},
- {2, 0, 90},
- {0, 0, 10},
- },
- {
- {0, 3, 125},
- {2, 3, 160},
- {1, 2, 40},
- {1, 1, 90},
- {2, 0, 135},
- {0, 0, 0},
- },
- { /* lot 4 */
- {0, 3, 135},
- {0, 2, 90},
- {0, 1, 90},
- {0, 0, 90},
- {2, 3, -90},
- {2, 0, 90},
- },
- {
- {0, 3, 135},
- {1, 3, 180},
- {2, 2, 135},
- {2, 1, 90},
- {1, 0, 0},
- {0, 0, 0},
- },
- { /* lot 6 */
- {0, 3, 135},
- {1, 3, 180},
- {1, 2, 135},
- {1, 1, 90},
- {1, 0, 0},
- {0, 0, 0},
- },
- {
- {0, 3, 135},
- {0, 2, 90},
- {0, 1, 90},
- {0, 0, 90},
- {1, 3, -45},
- {1, 0, 45},
- },
- { /* lot 8 */
- {0, 3, 135},
- {1, 2, 135},
- {1, 1, 90},
- {0, 0, 45},
- {2, 2, -90},
- {2, 1, -90},
- },
- {
- {0, 3, 135},
- {0, 2, 90},
- {0, 1, 90},
- {0, 0, 90},
- {2, 2, -90},
- {2, 1, -90},
- },
- { /* lot 10 */
- {0, 3, 135},
- {0, 2, 90},
- {0, 1, 90},
- {0, 0, 90},
- {1, 1, -45},
- {1, 2, -90},
- },
-};
-
-const int prefered_acropolis_approach_angles[][3] = {
- {180, 180, 220},
- {130, 160, 200},
- {130, 160, 200},
- {130, 160, 200}, // FIXME... fix these
- {130, 160, 200}, // FIXME... fix these
- {130, 160, 200}, // FIXME... fix these
- {130, 160, 200}, // FIXME... fix these
- {130, 160, 200}, // FIXME... fix these
- {130, 160, 200}, // FIXME... fix these
- {130, 160, 200}, // FIXME... fix these
-};
-
-/************************************************************************
- * NOTES ON DATA WE NEED TO STORE
- ************************************************************************/
-
-/*
-PLAYGROUND:
- - puck (column element) dispensers status (number of pucks)
- - their max. capacity is 5
- - lintel dispensers status
- - free column elements configuration and their positions
- - free pucks configuration (lot number)
-ROBOT:
- - puck stack status (puck count)
- - lintel holder status
- */
-
-/************************************************************************
- * MISC FUNCTIONS
- ************************************************************************/
-
-/**
- * Competition timer. Stop robot when the timer exceeds.
- *
- */
-void *timing_thread(void *arg)
-{
- struct timespec start;
-
- clock_gettime(CLOCK_MONOTONIC, &start);
-#define WAIT(sec) \
- do { \
- struct timespec t; \
- t.tv_sec = start.tv_sec+sec; \
- t.tv_nsec = start.tv_nsec; \
- clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); \
- } while(0)
-
- WAIT(5);
- robot.use_back_switch = true;
- printf("Back switch not ignored\n");
-
- WAIT(TIME_TO_DEPOSITE);
- FSM_SIGNAL(MAIN, EV_SHORT_TIME_TO_END, NULL);
-
- WAIT(COMPETITION_TIME);
- printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
- robot_exit();
-
- return NULL;
-}
-
-void robot_goto_acropolis(int phi)
-{
- robot_moveto_trans(
- ACROPOLIS_CENTER_X + (ACROPOLIS_RADIUS + ROBOT_AXIS_TO_PUCK_M + 0.07)*cos(DEG2RAD(phi)),
- ACROPOLIS_CENTER_Y + (ACROPOLIS_RADIUS + ROBOT_AXIS_TO_PUCK_M + 0.07)*sin(DEG2RAD(phi)),
- ARRIVE_FROM(DEG2RAD(phi+180), 0.07), &tcSlow);
-}
-
-void robot_goto_puck_in_grid(int nx, int ny, int phi)
-{
- struct puck_pos pp = free_puck_pos(nx, ny); // puck position
- robot_moveto_trans(
- pp.x + ROBOT_AXIS_TO_PUCK_M*cos(DEG2RAD(phi)),
- pp.y + ROBOT_AXIS_TO_PUCK_M*sin(DEG2RAD(phi)),
- ARRIVE_FROM(DEG2RAD(phi+180), 0.1), &tcSlow);
-}
-
-void robot_goto_next_puck_in_sequence(int lot, int puck_number)
+int main()
{
- robot_goto_puck_in_grid(
- free_puck_pick_up_sequence[lot][puck_number][0],
- free_puck_pick_up_sequence[lot][puck_number][1],
- free_puck_pick_up_sequence[lot][puck_number][2]);
-}
-
-/************************************************************************
- * FSM STATES DECLARATION
- ************************************************************************/
-
-/* initial and starting states */
-FSM_STATE_DECL(init);
-FSM_STATE_DECL(wait_for_start);
-/* strategies related states */
-FSM_STATE_DECL(decide_what_next);
-FSM_STATE_DECL(collect_free_pucks);
-FSM_STATE_DECL(deposit_at_acropolis);
-/* movement states */
-FSM_STATE_DECL(approach_acropolis);
-FSM_STATE_DECL(simple_construction_zone_approach);
-FSM_STATE_DECL(approach_our_static_dispenser);
-FSM_STATE_DECL(approach_opponents_static_dispenser);
-/* States handling ACT's actions (SUBFSMs) */
-FSM_STATE_DECL(load_the_puck);
-FSM_STATE_DECL(grasp_the_puck);
-FSM_STATE_DECL(look_for_puck_ahead);
-FSM_STATE_DECL(look_around_for_puck);
+ int rv;
-/************************************************************************
- * INITIAL AND STARTING STATES
- ************************************************************************/
+ rv = robot_init();
+ if (rv) error(1, errno, "robot_init() returned %d\n", rv);
-FSM_STATE(init)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- act_camera_on();
- free_puck_to_try_to_get_next = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
- pucks_at_once = 4; // number of pucks to load at once (maximum number of pucks we want to have in robot)
- tcFast = trajectoryConstraintsDefault;
- //tcFast.maxv = 1.5;
- tcSlow = trajectoryConstraintsDefault;
- tcSlow.maxv = 0.2;
- tcSlow.maxacc = 0.1;
- tcSlow.maxomega = 1;
- tcSlow.maxangacc = 1;
- tcVerySlow = trajectoryConstraintsDefault;
- tcVerySlow.maxv = 0.1;
- tcVerySlow.maxacc = 0.05;
- tcVerySlow.maxomega = 0.7;
- tcVerySlow.maxangacc = 1;
- FSM_TRANSITION(wait_for_start);
- break;
- default:
- break;
- }
-}
+ robot.obstacle_avoidance_enabled = true;
-FSM_STATE(wait_for_start)
-{
- pthread_t thid;
-#ifdef COMPETITION
- printf("COMPETITION mode set\n");
-#endif
- switch (FSM_EVENT) {
-#ifdef WAIT_FOR_START
- case EV_ENTRY:
- break;
- case EV_START:
-#else
- case EV_ENTRY:
- case EV_START:
-#endif
- act_camera_off();
- /* start competition timer */
- pthread_create(&thid, NULL, timing_thread, NULL);
- robot_set_est_pos_trans(0.16,
- PLAYGROUND_HEIGHT_M - 0.16,
- DEG2RAD(-45));
- FSM_TRANSITION(decide_what_next);
- break;
- case EV_TIMER:
- case EV_RETURN:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE: // currently not used
- case EV_SHORT_TIME_TO_END:
- case EV_STACK_FULL:
- case EV_ACTION_DONE:
- case EV_ACTION_ERROR:
- case EV_PUCK_REACHABLE:
- case EV_MOTION_DONE:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
-
-/************************************************************************
- * STRATEGIES RELATED STATES
- ************************************************************************/
+ robot.fsm.main.debug_states = 1;
+ robot.fsm.motion.debug_states = 1;
+ //robot.fsm.act.debug_states = 1;
+
+ //robot.fsm.main.state = &fsm_state_main_start_opp_corn;
+ //robot.fsm.main.state = &fsm_state_main_start_opp_oranges;
+ robot.fsm.main.state = &fsm_state_main_start_six_oranges;
+
+ //robot.fsm.main.transition_callback = trans_callback;
+ //robot.fsm.motion.transition_callback = move_trans_callback;
+
+ tcFast = trajectoryConstraintsDefault;
+ tcFast.maxv = 1;
+ tcFast.maxacc = 0.3;
+ tcFast.maxomega = 2;
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.3;
+ tcSlow.maxacc = 0.1;
+ tcVerySlow = trajectoryConstraintsDefault;
+ tcVerySlow.maxv = 0.05;
+ tcVerySlow.maxacc = 0.05;
-FSM_STATE(decide_what_next)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- if(robot.pucks_inside == pucks_at_once) {
- FSM_TRANSITION(deposit_at_acropolis);
- } else {
- if(robot.pucks_inside < pucks_at_once && free_puck_to_try_to_get_next < 6) {
- FSM_TRANSITION(collect_free_pucks);
- }
- }
- break;
- case EV_MOTION_DONE:
- case EV_ACTION_DONE:
- case EV_RETURN:
- case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE: // currently not used
- case EV_SHORT_TIME_TO_END:
- FSM_TRANSITION(deposit_at_acropolis); //FIXME fsm transition to decide_what_next
- break; // everywhere and ...
- case EV_STACK_FULL:
- case EV_ACTION_ERROR:
- case EV_PUCK_REACHABLE:
- case EV_START:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
+ rv = robot_start();
+ if (rv) error(1, errno, "robot_start() returned %d\n", rv);
-FSM_STATE(deposit_at_acropolis)
-{
- static int deposit_status;
- switch (FSM_EVENT) {
- case EV_ENTRY:
- deposit_status = 0;
- SUBFSM_TRANSITION(approach_acropolis, NULL);
- break;
- case EV_RETURN: {
- //if ((bool)FSM_EVENT_INT == true) {
- int floor = 2;
- FSM_SIGNAL(ACT, EV_PREPARE_THE_UNLOAD, (void*)floor);
- //}
- }
- break;
- case EV_ACTION_DONE:
- switch(deposit_status) {
- case 0:
- robot_move_by(0.11, NO_TURN(), &tcSlow);
- break;
- case 1:
- robot_move_by(-0.07, NO_TURN(), &tcSlow);
- break;
- }
- deposit_status++;
- break;
- case EV_MOTION_DONE:
- switch(deposit_status) {
- case 1:
- FSM_SIGNAL(ACT, EV_UNLOAD_PUCKS, NULL);
- break;
- case 2:
- robot_move_by(0.08, NO_TURN(), &tcVerySlow);
- deposit_status++;
- break;
- case 3:
- robot_move_by(-0.15, NO_TURN(), &tcSlow);
- deposit_status++;
- break;
- case 4:
- FSM_TRANSITION(decide_what_next);
- FSM_SIGNAL(ACT, EV_FREE_SPACE, NULL);
- deposit_status++;
- }
- break;
- case EV_SHORT_TIME_TO_END:
- //FSM_TRANSITION(deposit_at_acropolis);
- break;
- case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE: // currently not used
- case EV_STACK_FULL:
- case EV_ACTION_ERROR:
- case EV_PUCK_REACHABLE:
- case EV_START:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
+ robot_destroy();
-FSM_STATE(collect_free_pucks)
-{
- // game configuration... detected lot number is stored in robot.game_conf
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_goto_next_puck_in_sequence(robot.game_conf, free_puck_to_try_to_get_next);
- break;
- case EV_MOTION_DONE: {
- printf("-----arrived where the free puck no. %d should be\n", free_puck_to_try_to_get_next);
- SUBFSM_TRANSITION(load_the_puck, NULL);
- }
- break;
- case EV_RETURN:
- switch(FSM_EVENT_RET_VAL) {
- case LOAD_SUCCESS:
- printf(">>>>>> Loading the puck succeeded\n");
- break;
- case LOAD_FAIL:
- printf(">>>>>> Loading the puck FAILED\n");
- break;
- }
- free_puck_to_try_to_get_next++;
- FSM_TRANSITION(decide_what_next);
- break;
- case EV_SHORT_TIME_TO_END:
- FSM_TRANSITION(deposit_at_acropolis);
- break;
- case EV_PUCK_REACHABLE:
- case EV_ACTION_DONE:
- case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE: // currently not used
- case EV_STACK_FULL:
- case EV_ACTION_ERROR:
- case EV_START:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
+ return 0;
}
/************************************************************************
- * MOVEMENT STATES
+ * STATE SKELETON
************************************************************************/
-FSM_STATE(approach_acropolis)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_goto_acropolis(prefered_acropolis_approach_angles[(int)robot.game_conf][(free_puck_to_try_to_get_next-1)/2]);
- break;
- case EV_MOTION_DONE:
- SUBFSM_RET(NULL);
- // FIXME SUBFSM_RET((void*)true);
- break;
- case EV_GOAL_NOT_REACHABLE: // currently not used
- break;
- case EV_SHORT_TIME_TO_END:
- break;
- case EV_ACTION_DONE:
- case EV_RETURN:
- case EV_TIMER:
- case EV_LASER_POWER:
- case EV_STACK_FULL:
- case EV_ACTION_ERROR:
- case EV_PUCK_REACHABLE:
- case EV_START:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(simple_construction_zone_approach)
+/*
+FSM_STATE()
{
- switch (FSM_EVENT) {
+ switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcFast);
- robot_trajectory_add_point_trans(0.9, 1);
- robot_trajectory_add_final_point_trans(0.9, ROBOT_AXIS_TO_FRONT_M + 0.05, NO_TURN());
break;
- case EV_MOTION_DONE:
- //FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
- break;
- case EV_SHORT_TIME_TO_END:
- FSM_TRANSITION(deposit_at_acropolis);
- break;
- case EV_ACTION_DONE:
- case EV_RETURN:
- case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE: // currently not used
- case EV_STACK_FULL:
- case EV_ACTION_ERROR:
- case EV_PUCK_REACHABLE:
case EV_START:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(approach_our_static_dispenser)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: {
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
- tc.maxv /= 1.0;
- robot_trajectory_new(&tc);
-
- robot_trajectory_add_point_trans(STATIC_DISPENSER_X,
- STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
- robot_trajectory_add_final_point_trans(STATIC_DISPENSER_X,
- STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
- NO_TURN());
- }
- break;
- case EV_MOTION_DONE:
- FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
- printf("Arrived to the static dispenser\n");
- break;
- case EV_ACTION_DONE:
- printf("A Puck picked up\n");
- break;
- case EV_SHORT_TIME_TO_END:
- FSM_TRANSITION(deposit_at_acropolis);
- break;
- case EV_RETURN:
case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE: // currently not used
- case EV_STACK_FULL:
- case EV_ACTION_ERROR:
- case EV_PUCK_REACHABLE:
- case EV_START:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(approach_opponents_static_dispenser)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY: {
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
- tc.maxv /= 1.0;
- robot_trajectory_new(&tc);
-
- robot_trajectory_add_point_trans(OPPONENTS_STATIC_DISPENSER_X,
- OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
- robot_trajectory_add_final_point_trans(OPPONENTS_STATIC_DISPENSER_X,
- OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
- NO_TURN());
- }
- break;
- case EV_MOTION_DONE:
- FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
- printf("Arrived to the static dispenser\n");
- break;
- case EV_ACTION_DONE:
- printf("A Puck picked up\n");
- break;
- case EV_SHORT_TIME_TO_END:
- FSM_TRANSITION(deposit_at_acropolis);
- break;
case EV_RETURN:
- case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE: // currently not used
- case EV_STACK_FULL:
- case EV_ACTION_ERROR:
- case EV_PUCK_REACHABLE:
- case EV_START:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
-
-/************************************************************************
- * STATES HANDLING ACT's ACTIONS (to be used as SUB FSMs)
- ************************************************************************/
-
-FSM_STATE(load_the_puck)
-{
- static int puck_load_attempt_count;
- switch (FSM_EVENT) {
- case EV_ENTRY:
- puck_load_attempt_count = 0;
- FSM_SIGNAL(ACT, EV_SCRABBLE, NULL);
- FSM_TIMER(200);
- break;
- case EV_TIMER:
- robot_move_by(0.02, NO_TURN(), &tcSlow);
- break;
- case EV_MOTION_DONE:
- FSM_SIGNAL(ACT, EV_LOAD_THE_PUCK, NULL);
- break;
case EV_ACTION_DONE:
- SUBFSM_RET((void *)LOAD_SUCCESS);
- break;
case EV_ACTION_ERROR:
- puck_load_attempt_count++;
- if (puck_load_attempt_count > 2) {
- SUBFSM_RET((void *)LOAD_FAIL);
- } else {
- robot_move_by(0.02, NO_TURN(), &tcSlow);
- }
- break;
- case EV_SHORT_TIME_TO_END:
- FSM_TRANSITION(deposit_at_acropolis);
- break;
- case EV_RETURN:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE: // currently not used
- case EV_STACK_FULL:
- case EV_PUCK_REACHABLE:
- case EV_START:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
-
-/* of no use without the sharp sensor measuring "puck distance"
-FSM_STATE(look_around_for_puck)
-{
- static int lfp_status = 0;
- const static int scatter_angle = 20;
- static struct robot_pos_type orig_position;
- switch (FSM_EVENT) {
- case EV_ENTRY:
- ROBOT_LOCK(ref_pos);
- orig_position = robot.ref_pos;
- ROBOT_UNLOCK(ref_pos);
- //printf("original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
- robot_move_by(0, TURN_CW(DEG2RAD(RAD2DEG(orig_position.phi)-scatter_angle)), &tcSlow);
- //printf("lfp_status: %d\n", lfp_status);
- break;
- case EV_MOTION_DONE:
- switch (lfp_status) {
- case 0:
- //printf("2. original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
- //printf("robot.fer_pos angle of rotation of the robot: %f degrees\n", RAD2DEG(robot.ref_pos.phi));
- //printf("--- robot move by ... turn cw\n");
- robot_move_by(0, TURN_CCW(DEG2RAD(RAD2DEG(orig_position.phi)+scatter_angle)), &tcSlow);
- lfp_status++;
- break;
- case 1:
- robot_move_by(0, TURN(orig_position.phi), &tcSlow);
- lfp_status++;
- break;
- case 2: // puck not found
- SUBFSM_RET((void *)LOOK_AROUND_FAIL);
- break;
- }
- //printf("lfp_status: %d\n", lfp_status);
- break;
- case EV_PUCK_REACHABLE: // puck found
- robot_stop();
- SUBFSM_RET((void *)LOOK_AROUND_SUCCESS);
- break;
- case EV_ACTION_DONE:
- case EV_ACTION_ERROR: // look for puck does not send this event
- case EV_RETURN:
- case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE: // currently not used
- case EV_SHORT_TIME_TO_END:
- case EV_STACK_FULL:
- case EV_START:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
-*/
-
-
-/*
-FSM_STATE()
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- break;
- case EV_SHORT_TIME_TO_END:
case EV_MOTION_DONE:
- case EV_ACTION_DONE:
- case EV_RETURN:
- case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE: // currently not used
- case EV_STACK_FULL:
- case EV_ACTION_ERROR:
- case EV_PUCK_REACHABLE:
- case EV_START:
+ case EV_MOTION_ERROR:
DBG_PRINT_EVENT("unhandled event");
- break;
case EV_EXIT:
break;
}
}
*/
-
-
-int main()
-{
- int rv;
-
- rv = robot_init();
- if (rv) error(1, errno, "robot_init() returned %d\n", rv);
-
- robot.fsm.main.debug_states = 1;
- robot.fsm.motion.debug_states = 1;
- robot.fsm.act.debug_states = 1;
-
- robot.fsm.main.state = &fsm_state_main_init;
-
- rv = robot_start();
- if (rv) error(1, errno, "robot_start() returned %d\n", rv);
-
- robot_destroy();
-
- return 0;
-}
projects / eurobot / public.git / blobdiff