src/robofsm/competition.cc

   1 /**
   2  * @file competition.cc
   3  * @author Michal Sojka
   4  * @author Filip Jares
   5  * @date 2009-2010
   6  *
   7  * @brief Robot's main control program
   8  *
   9  */
  10
  11 /*
  12  * competition.cc
  13  *
  14  * Robot's main control program (Eurobot 2010).
  15  *
  16  * Copyright: (c) 2009-2010 CTU Dragons
  17  *            CTU FEE - Department of Control Engineering
  18  * License: GNU GPL v.2
  19  */
  20
  21 #ifndef DEBUG
  22 #define DEBUG
  23 #endif
  24
  25 // FIXME: remove unused includes:
  26 #define FSM_MAIN
  27 #include <robodata.h>
  28 #include <robot.h>
  29 #include <fsm.h>
  30 #include <unistd.h>
  31 #include <math.h>
  32 #include <movehelper.h>
  33 #include <map.h>
  34 #include <sharp.h>
  35 #include <robomath.h>
  36 #include <string.h>
  37 #include <robodim.h>
  38 #include <stdbool.h>
  39 #include <error.h>
  40 #include <actuators.h>
  41
  42 #ifdef COMPETITION
  43 #define WAIT_FOR_START
  44 #define COMPETITION_TIME_DEFAULT        90
  45 #define TIME_TO_DEPOSITE_DEFAULT        60
  46 #else
  47 #undef WAIT_FOR_START
  48 #define COMPETITION_TIME_DEFAULT        900
  49 #define TIME_TO_DEPOSITE_DEFAULT        60
  50 #endif
  51
  52 /* competition time in seconds */
  53 #define COMPETITION_TIME        COMPETITION_TIME_DEFAULT
  54 #define TIME_TO_DEPOSITE        TIME_TO_DEPOSITE_DEFAULT
  55 /* competition time in seconds */
  56
  57 /************************************************************************
  58  * SUBFSM's return values ...
  59  ************************************************************************/
  60
  61 typedef enum {
  62         LOAD_SUCCESS = 0,
  63         LOAD_FAIL,
  64 } subfsm_ret_val;
  65
  66 #define FSM_EVENT_RET_VAL ((subfsm_ret_val)FSM_EVENT_INT)
  67
  68 /************************************************************************
  69  * Trajectory constraints used, are initialized in the init state
  70  ************************************************************************/
  71
  72 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
  73
  74 /************************************************************************
  75  * Variables
  76  ************************************************************************/
  77
  78 /** is set to true in separate thread when there is short time left */
  79 bool short_time_to_end;
  80
  81 /************************************************************************
  82  * Definition of particular movement sequences (FIXME)
  83  ************************************************************************/
  84
  85 /** *********************************************************************
  86  * Competition timer. Stop robot when the timer exceeds.
  87  ********************************************************************** */
  88 void *timing_thread(void *arg)
  89 {
  90         struct timespec start;
  91
  92         sem_wait(&robot.start);
  93
  94         clock_gettime(CLOCK_MONOTONIC, &start);
  95 #define WAIT(sec)                                                       \
  96         do {                                                            \
  97                 struct timespec t;                                      \
  98                 t.tv_sec = start.tv_sec+sec;                            \
  99                 t.tv_nsec = start.tv_nsec;                              \
 100                 clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); \
 101         } while(0)
 102
 103 //      WAIT(5);
 104 //      // microswitch (backside opponent detector), ignore it while at starting point
 105 //      robot.use_back_switch = true;
 106 //      printf("Back switch not ignored\n");
 107
 108         WAIT(TIME_TO_DEPOSITE);
 109         short_time_to_end = true;
 110
 111         WAIT(COMPETITION_TIME);
 112         printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
 113         robot_exit();
 114
 115         return NULL;
 116 }
 117
 118 /************************************************************************
 119  * FSM STATES DECLARATION
 120  ************************************************************************/
 121
 122 /* initial and starting states */
 123 FSM_STATE_DECL(init);
 124 FSM_STATE_DECL(wait_for_start);
 125 /* strategies related states */
 126 FSM_STATE_DECL(decide_what_next);
 127 /* movement states */
 128
 129 /************************************************************************
 130  * INITIAL AND STARTING STATES
 131  ************************************************************************/
 132
 133 FSM_STATE(init)
 134 {
 135         switch (FSM_EVENT) {
 136         case EV_ENTRY:
 137                 short_time_to_end = false;
 138                 act_camera_on();
 139                 tcFast = trajectoryConstraintsDefault;
 140                 tcSlow = trajectoryConstraintsDefault;
 141                 tcSlow.maxv = 0.2;
 142                 tcSlow.maxacc = 0.1;
 143                 tcSlow.maxomega = 1;
 144                 tcSlow.maxangacc = 1;
 145                 tcVerySlow = trajectoryConstraintsDefault;
 146                 tcVerySlow.maxv =  0.1;
 147                 tcVerySlow.maxacc = 0.05;
 148                 tcVerySlow.maxomega = 0.7;
 149                 tcVerySlow.maxangacc = 1;
 150                 FSM_TRANSITION(wait_for_start);
 151                 break;
 152         default:
 153                 break;
 154         }
 155 }
 156
 157 void set_initial_position()
 158 {
 159         robot_set_est_pos_trans(0.16,
 160                                 PLAYGROUND_HEIGHT_M - 0.16,
 161                                 DEG2RAD(-45));
 162 }
 163
 164 FSM_STATE(wait_for_start)
 165 {
 166         pthread_t thid;
 167 #ifdef COMPETITION
 168         printf("COMPETITION mode set\n");
 169 #endif
 170         switch (FSM_EVENT) {
 171                 case EV_ENTRY:
 172                         pthread_create(&thid, NULL, timing_thread, NULL);
 173 #ifdef WAIT_FOR_START
 174                         FSM_TIMER(1000);
 175                         break;
 176 #endif
 177                 case EV_START:
 178                         act_camera_off();
 179                         /* start competition timer */
 180                         sem_post(&robot.start);
 181                         /* FIXME: */
 182                         set_initial_position();
 183                         FSM_TRANSITION(decide_what_next);
 184                 case EV_TIMER:
 185                         FSM_TIMER(1000);
 186                         // We set initial position periodically in
 187                         // order for it to be updated on the display
 188                         // if the team color is changed during waiting
 189                         // for start.
 190                         set_initial_position();
 191                         break;
 192                 case EV_RETURN:
 193                 case EV_GOAL_NOT_REACHABLE: // currently not used
 194                 case EV_ACTION_DONE:
 195                 case EV_ACTION_ERROR:
 196                 case EV_MOTION_DONE:
 197                         DBG_PRINT_EVENT("unhandled event");
 198                         break;
 199                 case EV_EXIT:
 200                         break;
 201         }
 202 }
 203
 204 /************************************************************************
 205  * STRATEGIES RELATED STATES
 206  ************************************************************************/
 207
 208 FSM_STATE(decide_what_next)
 209 {
 210         switch (FSM_EVENT) {
 211                 case EV_ENTRY:
 212                         if(short_time_to_end) {
 213                         } else {
 214                         }
 215                         break;
 216                 case EV_MOTION_DONE:
 217                 case EV_ACTION_DONE:
 218                 case EV_RETURN:
 219                 case EV_TIMER:
 220                 case EV_GOAL_NOT_REACHABLE: // currently not used
 221                 case EV_ACTION_ERROR:
 222                 case EV_START:
 223                         DBG_PRINT_EVENT("unhandled event");
 224                         break;
 225                 case EV_EXIT:
 226                         break;
 227         }
 228 }
 229
 230
 231 int main()
 232 {
 233         int rv;
 234
 235         rv = robot_init();
 236         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 237
 238         robot.fsm.main.debug_states = 1;
 239         robot.fsm.motion.debug_states = 1;
 240         robot.fsm.act.debug_states = 1;
 241
 242         robot.fsm.main.state = &fsm_state_main_init;
 243
 244         rv = robot_start();
 245         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 246
 247         robot_destroy();
 248
 249         return 0;
 250 }