/*
- * 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 "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,
- //LOOK_AROUND_SUCCESS, // obsolete, no sharp sensor
- //LOOK_AROUND_FAIL // obsolete, no sharp sensor
-} SUBFSM_RET_VAL;
-
-#define FSM_EVENT_RET_VAL ((SUBFSM_RET_VAL)FSM_EVENT_PTR)
-
-/************************************************************************
- * Trajectory constraints used, are initialized in the init state
- ************************************************************************/
-
-struct TrajectoryConstraints tcFast, tcSlow;
-
-/************************************************************************
- * Variables related to puck collecting
- ************************************************************************/
-
-int free_puck_to_try_to_get_next = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
-
-/************************************************************************
- * Definition of particular "free pucks pick up sequences"
- ************************************************************************/
-
-const int free_puck_pick_up_sequence[][6][3] = {
- { /* lot 1 */
- {0, 3, 135},
- {1, 3, 180},
- {2, 3, 180},
- {2, 0, 90},
- {1, 0, 45},
- {0, 0, 0}, // FIXME: test last two pucks
- },
- {
- {0, 3, 135},
- {2, 3, 90},
- {2, 2, 90},
- {2, 1, 90},
- {2, 0, 90},
- {0, 0, 10}, // FIXME: test last two pucks
- },
- {
- {0, 3, 135},
- {2, 3, 160},
- {1, 2, 40},
- {1, 1, 90},
- {2, 0, 135},
- {0, 0, 0}, // FIXME: test last two pucks
- },
- { /* lot 4 */
- {0, 3, 135},
- {0, 2, 90},
- {0, 1, 90},
- {0, 0, 90},
- {2, 3, -90},
- {2, 0, 90}, // FIXME: test last two pucks
- },
- {
- {0, 3, 135},
- {1, 3, 180},
- {2, 2, 135},
- {2, 1, 90},
- {1, 0, 0},
- {0, 0, 0}, // FIXME: test last two pucks
- },
- { /* lot 6 */
- {0, 3, 135},
- {1, 3, 180},
- {1, 2, 135},
- {1, 1, 90},
- {1, 0, 0},
- {0, 0, 0}, // FIXME: test last two pucks
- },
- {
- {0, 3, 135},
- {0, 2, 90},
- {0, 1, 90},
- {0, 0, 90},
- {1, 3, -45},
- {1, 0, 45}, // FIXME: test last two pucks
- },
- { /* lot 8 */
- {0, 3, 135},
- {1, 2, 135},
- {1, 1, 90},
- {0, 0, 45},
- {2, 2, -90},
- {2, 1, -90}, // FIXME: test last two pucks
- },
- {
- {0, 3, 135},
- {0, 2, 90},
- {0, 1, 90},
- {0, 0, 90},
- {2, 2, -90},
- {2, 1, -90}, // FIXME: test last two pucks
- },
- { /* lot 10 */
- {0, 3, 135},
- {0, 2, 90},
- {0, 1, 90},
- {0, 0, 90},
- {1, 1, -45},
- {1, 2, -90}, // FIXME: test last two pucks
- },
-};
-
-/************************************************************************
- * 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;
-}
-
-/**
- * Get position of the point when we know the distance and angle to turn.
- *
- * FIXME (F.J.): - there used to be non-actual parameter documentation
- * - what was this function good for? (not used anywhere)
- */
-void get_relative_pos(struct est_pos_type *est, struct ref_pos_type *ref,
- double l, double phi)
-{
- ref->x = est->x + l*cos(est->phi + phi);
- ref->y = est->y + l*sin(est->phi + phi);
- ref->phi = est->phi + phi;
-}
-
-void robot_goto_point(struct ref_pos_type des_pos)
-{
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
-
- tc.maxv /= 4;
- robot_trajectory_new(&tc);
- robot_trajectory_add_final_point_trans(des_pos.x, des_pos.y, NO_TURN());
-}
-
-void robot_go_backward_to_point(struct ref_pos_type des_pos)
-{
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
-
- tc.maxv /= 1;
- robot_trajectory_new_backward(&tc);
- robot_trajectory_add_final_point_notrans(des_pos.x, des_pos.y, NO_TURN());
-}
-
-void robot_goto_puck_in_grid(int nx, int ny, int phi)
+int main()
{
- struct puck_pos pp = free_puck_pos(nx, ny); // puck position
- robot_trajectory_new(&tcSlow);
- robot_trajectory_add_point_trans(
- pp.x + (ROBOT_AXIS_TO_PUCK_M+0.10)*cos(DEG2RAD(phi)),
- pp.y + (ROBOT_AXIS_TO_PUCK_M+0.10)*sin(DEG2RAD(phi)));
- robot_trajectory_add_final_point_trans(
- pp.x + ROBOT_AXIS_TO_PUCK_M*cos(DEG2RAD(phi)),
- pp.y + ROBOT_AXIS_TO_PUCK_M*sin(DEG2RAD(phi)),
- TURN(fmod(DEG2RAD(phi+180), 2.0*M_PI)));
- /* robot_goto_trans( // does not exist
- pp.x + ROBOT_AXIS_TO_PUCK_M*cos(DEG2RAD(phi)),
- pp.y + ROBOT_AXIS_TO_PUCK_M*sin(DEG2RAD(phi)),
- TURN(fmod(DEG2RAD(phi+180), 2.0*M_PI))); */
-}
-
-/************************************************************************
- * FSM STATES DECLARATION
- ************************************************************************/
-
-/* initial and starting states */
-FSM_STATE_DECL(init);
-FSM_STATE_DECL(wait_for_start);
-/* strategies related states */
-FSM_STATE_DECL(collect_free_pucks);
-/* movement states */
-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);
-
-/************************************************************************
- * INITIAL AND STARTING STATES
- ************************************************************************/
+ int rv;
-FSM_STATE(init)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- tcFast = trajectoryConstraintsDefault;
- //tcFast.maxv = 1.5;
- tcSlow = trajectoryConstraintsDefault;
- tcSlow.maxv = 0.2;
- FSM_TRANSITION(wait_for_start);
- break;
- default:
- break;
- }
-}
+ rv = robot_init();
+ if (rv) error(1, errno, "robot_init() returned %d\n", rv);
-FSM_STATE(wait_for_start)
-{
- pthread_t thid;
-#ifdef COMPETITON
- printf("COMPETITION mode set");
-#endif
- switch (FSM_EVENT) {
-#ifdef WAIT_FOR_START
- case EV_ENTRY:
- break;
- case EV_START:
-#else
- case EV_ENTRY:
- case EV_START:
-#endif
- /* 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(collect_free_pucks);
+ robot.obstacle_avoidance_enabled = true;
- /*robot_trajectory_new(&tcSlow);
- struct puck_pos pp = free_puck_pos(0, 2, 0); // puck position
- robot_trajectory_add_final_point_trans(
- pp.x,
- pp.y,
- TURN(DEG2RAD(0))); */
- //FSM_TRANSITION(approach_our_static_dispenser);
- //robot_goto_puck_in_grid(0, 1, 2, 270);
- /* temporary:
- FSM_SIGNAL(ACT, EV_GRASP_THE_PUCK, NULL);
- FSM_TIMER(5000);
- */
- break;
- case EV_TIMER:
- /* temporary:
- FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
- break;
- */
- break;
- case EV_RETURN:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE:
- 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;
- }
-}
+ 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;
-/************************************************************************
- * STRATEGIES RELATED STATES
- ************************************************************************/
+ rv = robot_start();
+ if (rv) error(1, errno, "robot_start() returned %d\n", rv);
-static void robot_goto_next_puck_in_sequence(int lot, int puck_number)
-{
- 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]);
-}
+ robot_destroy();
-FSM_STATE(collect_free_pucks)
-{
- static const int lot = 7; // this variable location is temporary...; going to be received from the camera
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_goto_next_puck_in_sequence(lot, 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) {
- /* obsolete, no sharp sensor present
- case LOOK_AROUND_SUCCESS: // FIXME: test number of pucks loaded!!
- printf(">>>>>> Look around succeeded\n");
- SUBFSM_TRANSITION(load_the_puck, NULL);
- break; */
- case LOAD_SUCCESS:
- printf(">>>>>> Loading the puck succeeded\n");
- if(free_puck_to_try_to_get_next<4) {
- free_puck_to_try_to_get_next++;
- robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
- }
- break;
- /* obsolete, no sharp sensor present
- case LOOK_AROUND_FAIL:
- printf(">>>>>> Looking around for the puck FAILED\n");
- break; // FIXME: remove the break
- */
- case LOAD_FAIL:
- printf(">>>>>> Loading the puck FAILED\n");
- if(free_puck_to_try_to_get_next<6) { // FIXME: test number of pucks loaded!!
- free_puck_to_try_to_get_next++;
- robot_goto_next_puck_in_sequence(lot, free_puck_to_try_to_get_next);
- } else {
- // FIXME (TODO): transition to next strategy state
- }
- break;
- }
- break;
- case EV_PUCK_REACHABLE:
- robot_stop();
- printf("-----reached some free puck, hopefully no. %d\n", free_puck_to_try_to_get_next);
- SUBFSM_TRANSITION(load_the_puck, NULL);
- break;
- case EV_ACTION_DONE:
- case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE:
- case EV_SHORT_TIME_TO_END:
- 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(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_ACTION_DONE:
- case EV_RETURN:
- case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE:
- case EV_SHORT_TIME_TO_END:
- 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_RETURN:
case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE:
- case EV_SHORT_TIME_TO_END:
- //case EV_PUCK_REACHABLE: // FIXME: handle this
- 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_RETURN:
- case EV_TIMER:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE:
- case EV_SHORT_TIME_TO_END:
- //case EV_PUCK_REACHABLE: // FIXME: handle this
- 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_RETURN:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE:
- case EV_SHORT_TIME_TO_END:
- 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 ref_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:
- 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_MOTION_DONE:
- case EV_ACTION_DONE:
- case EV_RETURN:
- case EV_TIMER:
- case EV_OBSTRUCTION_AHEAD:
- case EV_LASER_POWER:
- case EV_GOAL_NOT_REACHABLE:
- case EV_SHORT_TIME_TO_END:
- case EV_ENEMY_AHEAD:
- 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