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 #include "match-timing.h"
  42
  43 #ifdef COMPETITION
  44 #define WAIT_FOR_START
  45 #else
  46 #undef WAIT_FOR_START
  47 #endif
  48
  49 /************************************************************************
  50  * SUBFSM's return values ...
  51  ************************************************************************/
  52
  53 typedef enum {
  54         LOAD_SUCCESS = 0,
  55         LOAD_FAIL,
  56 } subfsm_ret_val;
  57
  58 #define FSM_EVENT_RET_VAL ((subfsm_ret_val)FSM_EVENT_INT)
  59
  60 /************************************************************************
  61  * Trajectory constraints used, are initialized in the init state
  62  ************************************************************************/
  63
  64 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
  65
  66 /************************************************************************
  67  * Definition of particular movement sequences (FIXME)
  68  ************************************************************************/
  69
  70 /************************************************************************
  71  * FSM STATES DECLARATION
  72  ************************************************************************/
  73
  74 /* initial and starting states */
  75 FSM_STATE_DECL(init);
  76 FSM_STATE_DECL(wait_for_start);
  77 /* strategies related states */
  78 FSM_STATE_DECL(decide_what_next);
  79 /* movement states */
  80
  81 /************************************************************************
  82  * INITIAL AND STARTING STATES
  83  ************************************************************************/
  84
  85 FSM_STATE(init)
  86 {
  87         switch (FSM_EVENT) {
  88         case EV_ENTRY:
  89                 robot.short_time_to_end = false;
  90                 act_camera_on();
  91                 tcFast = trajectoryConstraintsDefault;
  92                 tcSlow = trajectoryConstraintsDefault;
  93                 tcSlow.maxv = 0.2;
  94                 tcSlow.maxacc = 0.1;
  95                 tcSlow.maxomega = 1;
  96                 tcSlow.maxangacc = 1;
  97                 tcVerySlow = trajectoryConstraintsDefault;
  98                 tcVerySlow.maxv =  0.1;
  99                 tcVerySlow.maxacc = 0.05;
 100                 tcVerySlow.maxomega = 0.7;
 101                 tcVerySlow.maxangacc = 1;
 102                 FSM_TRANSITION(wait_for_start);
 103                 break;
 104         default:
 105                 break;
 106         }
 107 }
 108
 109 void set_initial_position()
 110 {
 111         robot_set_est_pos_trans(0.16,
 112                                 PLAYGROUND_HEIGHT_M - 0.16,
 113                                 DEG2RAD(-45));
 114 }
 115
 116 FSM_STATE(wait_for_start)
 117 {
 118         pthread_t thid;
 119 #ifdef COMPETITION
 120         printf("COMPETITION mode set\n");
 121 #endif
 122         switch (FSM_EVENT) {
 123                 case EV_ENTRY:
 124                         pthread_create(&thid, NULL, timing_thread, NULL);
 125 #ifdef WAIT_FOR_START
 126                         FSM_TIMER(1000);
 127                         break;
 128 #endif
 129                 case EV_START:
 130                         act_camera_off();
 131                         /* start competition timer */
 132                         sem_post(&robot.start);
 133                         /* FIXME: */
 134                         set_initial_position();
 135                         FSM_TRANSITION(decide_what_next);
 136                 case EV_TIMER:
 137                         FSM_TIMER(1000);
 138                         // We set initial position periodically in
 139                         // order for it to be updated on the display
 140                         // if the team color is changed during waiting
 141                         // for start.
 142                         set_initial_position();
 143                         break;
 144                 case EV_RETURN:
 145                 case EV_GOAL_NOT_REACHABLE: // currently not used
 146                 case EV_ACTION_DONE:
 147                 case EV_ACTION_ERROR:
 148                 case EV_MOTION_DONE:
 149                         DBG_PRINT_EVENT("unhandled event");
 150                         break;
 151                 case EV_EXIT:
 152                         break;
 153         }
 154 }
 155
 156 /************************************************************************
 157  * STRATEGIES RELATED STATES
 158  ************************************************************************/
 159
 160 FSM_STATE(decide_what_next)
 161 {
 162         switch (FSM_EVENT) {
 163                 case EV_ENTRY:
 164                         if(robot.short_time_to_end) {
 165                         } else {
 166                         }
 167                         break;
 168                 case EV_MOTION_DONE:
 169                 case EV_ACTION_DONE:
 170                 case EV_RETURN:
 171                 case EV_TIMER:
 172                 case EV_GOAL_NOT_REACHABLE: // currently not used
 173                 case EV_ACTION_ERROR:
 174                 case EV_START:
 175                         DBG_PRINT_EVENT("unhandled event");
 176                         break;
 177                 case EV_EXIT:
 178                         break;
 179         }
 180 }
 181
 182
 183 int main()
 184 {
 185         int rv;
 186
 187         rv = robot_init();
 188         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 189
 190         robot.fsm.main.debug_states = 1;
 191         robot.fsm.motion.debug_states = 1;
 192         robot.fsm.act.debug_states = 1;
 193
 194         robot.fsm.main.state = &fsm_state_main_init;
 195
 196         rv = robot_start();
 197         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 198
 199         robot_destroy();
 200
 201         return 0;
 202 }