-/**
- * @file competition.cc
- * @author Michal Sojka
- * @author Filip Jares
- * @date 2009-2010
- *
- * @brief Robot's main control program
- *
- */
-
/*
- * competition.cc
+ * competition.cc 2010/04/30
*
- * Robot's main control program (Eurobot 2010).
+ * Robot's control program intended for homologation (approval phase) on Eurobot 2009.
*
- * Copyright: (c) 2009-2010 CTU Dragons
+ * Copyright: (c) 2009 - 2010 CTU Dragons
* CTU FEE - Department of Control Engineering
* License: GNU GPL v.2
*/
#define DEBUG
#endif
-// FIXME: remove unused includes:
#define FSM_MAIN
#include <robodata.h>
#include <robot.h>
#include <robomath.h>
#include <string.h>
#include <robodim.h>
-#include <stdbool.h>
#include <error.h>
-#include <actuators.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
- ************************************************************************/
-
-/** is set to true in separate thread when there is short time left */
-bool short_time_to_end;
-
-/************************************************************************
- * Definition of particular movement sequences (FIXME)
- ************************************************************************/
+#include "corns_configs.h"
+#include "actuators.h"
+#include "match-timing.h"
+#include "eb2010misc.h"
+#include "common-states.h"
-/** *********************************************************************
- * Competition timer. Stop robot when the timer exceeds.
- ********************************************************************** */
-void *timing_thread(void *arg)
+int main()
{
- 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)
+ int rv;
- WAIT(5);
- // FIXME: any better solution?
- // microswitch (backside opponent detector), ignore it while at starting point
- robot.use_back_switch = true;
- printf("Back switch not ignored\n");
+ rv = robot_init();
+ if (rv) error(1, errno, "robot_init() returned %d\n", rv);
- WAIT(TIME_TO_DEPOSITE);
- short_time_to_end = true;
+ robot.obstacle_avoidance_enabled = true;
- WAIT(COMPETITION_TIME);
- printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
- robot_exit();
+ 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;
- return NULL;
-}
+ rv = robot_start();
+ if (rv) error(1, errno, "robot_start() returned %d\n", rv);
-/************************************************************************
- * FSM STATES DECLARATION
- ************************************************************************/
+ robot_destroy();
-/* initial and starting states */
-FSM_STATE_DECL(init);
-FSM_STATE_DECL(wait_for_start);
-/* strategies related states */
-FSM_STATE_DECL(decide_what_next);
-/* movement states */
+ return 0;
+}
/************************************************************************
- * INITIAL AND STARTING STATES
+ * STATE SKELETON
************************************************************************/
-FSM_STATE(init)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- short_time_to_end = false;
- act_camera_on();
- tcFast = trajectoryConstraintsDefault;
- 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;
- }
-}
-
-FSM_STATE(wait_for_start)
+/*
+FSM_STATE()
{
- pthread_t thid;
-#ifdef COMPETITION
- printf("COMPETITION mode set\n");
-#endif
- switch (FSM_EVENT) {
-#ifdef WAIT_FOR_START
+ switch(FSM_EVENT) {
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);
- /* FIXME: */
- 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_GOAL_NOT_REACHABLE: // currently not used
case EV_ACTION_DONE:
case EV_ACTION_ERROR:
case EV_MOTION_DONE:
+ case EV_MOTION_ERROR:
DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- }
-}
-
-/************************************************************************
- * STRATEGIES RELATED STATES
- ************************************************************************/
-
-FSM_STATE(decide_what_next)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- if(short_time_to_end) {
- } else {
- }
- break;
- case EV_MOTION_DONE:
- case EV_ACTION_DONE:
- case EV_RETURN:
- case EV_TIMER:
- case EV_GOAL_NOT_REACHABLE: // currently not used
- case EV_ACTION_ERROR:
- case EV_START:
- 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;
-}
+*/