src/robofsm/common-states.cc

   1 #define FSM_MAIN
   2 #include "robodata.h"
   3 #include <robot.h>
   4 #include <fsm.h>
   5 #include <unistd.h>
   6 #include <math.h>
   7 #include <movehelper.h>
   8 #include <map.h>
   9 #include <sharp.h>
  10 #include <robomath.h>
  11 #include <string.h>
  12 #include <robodim.h>
  13 #include <error.h>
  14 #include "actuators.h"
  15 #include <trgen.h>
  16 #include "match-timing.h"
  17 #include <stdbool.h>
  18 #include <ul_log.h>
  19
  20 UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
  21
  22 #include "common-states.h"
  23
  24 /************************************************************************
  25  * Functions used in and called from all the (almost identical)
  26  * "wait for start" states in particular strategies.
  27  ************************************************************************/
  28
  29 #undef DBG_FSM_STATE
  30 #define DBG_FSM_STATE(name)     do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
  31                                                                    fsm->debug_name, robot_current_time(), \
  32                                                                    name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
  33
  34
  35 void set_initial_position()
  36 {
  37         robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
  38                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
  39                                 0);
  40 }
  41
  42 void actuators_home()
  43 {
  44         act_jaws(CLOSE);
  45         // drive lift to home position
  46         //act_lift(0, 0, 1);
  47         // unset the homing request
  48         //act_lift(0, 0, 0);
  49 }
  50
  51 void start_entry()
  52 {
  53         pthread_t thid;
  54         robot.check_turn_safety = false;
  55         pthread_create(&thid, NULL, timing_thread, NULL);
  56         start_timer();
  57 }
  58
  59 // We set initial position periodically in order for it to be updated
  60 // on the display if the team color is changed during waiting for
  61 // start.
  62 void start_timer()
  63 {
  64         set_initial_position();
  65         if (robot.start_state == START_PLUGGED_IN)
  66                 actuators_home();
  67 }
  68
  69 void start_go()
  70 {
  71         sem_post(&robot.start);
  72         actuators_home();
  73         set_initial_position();
  74 }
  75
  76 void start_exit()
  77 {
  78
  79 }
  80
  81 void inline enable_bumpers(bool enabled)
  82 {
  83         robot.use_left_bumper = enabled;
  84         robot.use_right_bumper = enabled;
  85         robot.use_back_bumpers = enabled;
  86 }
  87
  88 /************************************************************************
  89  * Trajectory constraints used; They are  initialized in the main() function in competition.cc
  90  ************************************************************************/
  91
  92 struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow;
  93
  94 /* assures that the robot is near the slope rim; if outside our starting area, approach the slope first */
  95 FSM_STATE(bypass_figure_in_front_of_start)
  96 {
  97         switch(FSM_EVENT) {
  98                 case EV_ENTRY:
  99                         robot_trajectory_new(&tcSlow);
 100                         robot_trajectory_add_point_trans(
 101                                 0.45 + 0.2,
 102                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
 103                         robot_trajectory_add_point_trans(
 104                                 0.45 + 0.35 + 0.175,
 105                                 PLAYGROUND_HEIGHT_M - 0.35);
 106                         robot_trajectory_add_final_point_trans(
 107                                 0.45 + 0.35 + 0.175,
 108                                 0.7 + 0.175,
 109                                 NO_TURN());
 110                         break;
 111                 case EV_MOTION_DONE:
 112                         SUBFSM_RET(NULL);
 113                         break;
 114                 case EV_START:
 115                 case EV_TIMER:
 116                 case EV_RETURN:
 117                 case EV_MOTION_ERROR:
 118                 case EV_SWITCH_STRATEGY:
 119                         DBG_PRINT_EVENT("unhandled event");
 120                 case EV_EXIT:
 121                         break;
 122         }
 123 }
 124
 125 FSM_STATE(approach_second_green_figure)
 126 {
 127         switch(FSM_EVENT) {
 128                 case EV_ENTRY:
 129                         robot.use_left_bumper = true;
 130                         robot.use_right_bumper = true;
 131                         robot.use_back_bumpers = true;
 132
 133                         robot_trajectory_new(&tcSlow);
 134                         robot_trajectory_add_final_point_trans(
 135                                 0.45 + 0.35,
 136                                 0.29 + 0.28,
 137                                 ARRIVE_FROM(DEG2RAD(180), 0.10));
 138                         break;
 139                 case EV_MOTION_DONE:
 140                         act_jaws(OPEN);
 141                         FSM_TIMER(2000);
 142                         break;
 143                 case EV_TIMER:
 144                         FSM_TRANSITION(load_second_green_figure);
 145                         break;
 146                 case EV_RETURN:
 147                 case EV_MOTION_ERROR:
 148                 case EV_EXIT:
 149                         break;
 150         }
 151 }
 152
 153 FSM_STATE(load_second_green_figure)
 154 {
 155         switch(FSM_EVENT) {
 156                 case EV_ENTRY:
 157                         robot_trajectory_new(&tcSlow);
 158                         robot_trajectory_add_final_point_trans(
 159                                 ROBOT_AXIS_TO_FIGURE_CENTER_M + 0.05,
 160                                 0.29 + 0.28,
 161                                 NO_TURN());
 162                         break;
 163                 case EV_MOTION_DONE:
 164                         FSM_TIMER(2000);
 165                         act_jaws(CATCH);
 166                         break;
 167                 case EV_TIMER:
 168                         FSM_TRANSITION(go_out_second_green_figure);
 169                         break;
 170                 case EV_RETURN:
 171                 case EV_MOTION_ERROR:
 172                 case EV_EXIT:
 173                         // enables using side switches on bumpers
 174                         //robot.use_left_switch = true;
 175                         //robot.use_right_switch = true;
 176                         //robot.ignore_hokuyo = false;
 177                         break;
 178         }
 179 }
 180
 181 FSM_STATE(go_out_second_green_figure)
 182 {
 183         switch(FSM_EVENT) {
 184                 case EV_ENTRY:
 185                         robot_trajectory_new_backward(&tcSlow);
 186                         robot_trajectory_add_final_point_trans(0.45 + 0.35, 0.7, NO_TURN());
 187                         break;
 188                 case EV_MOTION_DONE:
 189                 case EV_TIMER:
 190                         FSM_TRANSITION(place_figure_to_protected_block);
 191                         break;
 192                 case EV_START:
 193                 case EV_RETURN:
 194                 case EV_MOTION_ERROR:
 195                 case EV_EXIT:
 196                         break;
 197         }
 198 }
 199
 200 FSM_STATE(place_figure_to_protected_block)
 201 {
 202         switch(FSM_EVENT) {
 203                 case EV_ENTRY:
 204                         robot_trajectory_new(&tcSlow);
 205                         robot_trajectory_add_final_point_trans(
 206                                 0.45 + 0.2,
 207                                 0.12 + 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M,
 208                                 ARRIVE_FROM(DEG2RAD(-90), 0.20));
 209                         break;
 210                 case EV_START:
 211                 case EV_TIMER:
 212                         FSM_TRANSITION(leave_protected_figure);
 213                         break;
 214                 case EV_RETURN:
 215                 case EV_MOTION_DONE:
 216                         act_jaws(OPEN);
 217                         FSM_TIMER(2000);
 218                         break;
 219                 case EV_MOTION_ERROR:
 220                 case EV_SWITCH_STRATEGY:
 221                         DBG_PRINT_EVENT("unhandled event");
 222                 case EV_EXIT:
 223                         break;
 224         }
 225 }
 226
 227 FSM_STATE(leave_protected_figure)
 228 {
 229         switch(FSM_EVENT) {
 230                 case EV_ENTRY:
 231                         //FSM_TIMER(1000);
 232                         robot_trajectory_new_backward(&tcSlow);
 233                         robot_trajectory_add_point_trans(
 234                                 0.45 + 0.175,
 235                                 0.7);
 236                         robot_trajectory_add_final_point_trans(
 237                                 0.45 + 0.35,
 238                                 0.35 + 0.35 + 0.175,
 239                                 TURN_CW(DEG2RAD(0)));
 240                         break;
 241                 case EV_START:
 242                 case EV_TIMER:
 243                         break;
 244                 case EV_RETURN:
 245                 case EV_MOTION_DONE:
 246                         //FSM_TRANSITION(load_second_figure);
 247                         SUBFSM_RET(NULL);
 248                         break;
 249                 case EV_MOTION_ERROR:
 250                 case EV_EXIT:
 251                         break;
 252         }
 253 }