src/robofsm/homologation.cc

   1 /*
   2  * homologation.cc       08/04/29
   3  *
   4  * Robot's control program intended for homologation (approval phase) on Eurobot 2009.
   5  *
   6  * Copyright: (c) 2009 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 #ifndef DEBUG
  12 #define DEBUG
  13 #endif
  14
  15 #define FSM_MAIN
  16 #include <robodata.h>
  17 #include <robot.h>
  18 #include <fsm.h>
  19 #include <unistd.h>
  20 #include <math.h>
  21 #include <movehelper.h>
  22 #include <map.h>
  23 #include <sharp.h>
  24 #include <robomath.h>
  25 #include <string.h>
  26 #include <robodim.h>
  27 #include <error.h>
  28
  29 /************************************************************************
  30  * Trajectory constraints used, are initialized in the init state
  31  ************************************************************************/
  32
  33 struct TrajectoryConstraints tcFast, tcSlow;
  34
  35 /************************************************************************
  36  * FSM STATES DECLARATION
  37  ************************************************************************/
  38
  39 /* initial and starting states */
  40 FSM_STATE_DECL(init);
  41 FSM_STATE_DECL(wait_for_start);
  42 /* movement states */
  43 FSM_STATE_DECL(approach_first_puck);
  44 FSM_STATE_DECL(simple_construction_zone_approach);
  45 FSM_STATE_DECL(get_out_of_the_construction_zone);
  46
  47 FSM_STATE_DECL(look_around_for_puck);
  48
  49 /************************************************************************
  50  * INITIAL AND STARTING STATES
  51  ************************************************************************/
  52
  53 FSM_STATE(init)
  54 {
  55         switch (FSM_EVENT) {
  56         case EV_ENTRY:
  57                 tcFast = trajectoryConstraintsDefault;
  58                 tcFast.maxv = 1.5;
  59                 tcSlow = trajectoryConstraintsDefault;
  60                 tcSlow.maxv = 0.2;
  61                 FSM_TRANSITION(wait_for_start);
  62                 break;
  63         default:
  64                 break;
  65         }
  66 }
  67
  68 FSM_STATE(wait_for_start)
  69 {
  70         #ifdef COMPETITON
  71                 printf("COMPETITION mode set");
  72         #endif
  73         switch (FSM_EVENT) {
  74 #ifdef WAIT_FOR_START
  75                 case EV_ENTRY:
  76                         break
  77                 case EV_START: {
  78                         pthread_t thid;
  79
  80                         /* start competition timer */
  81                         pthread_create(&thid, NULL, wait_for_end, NULL);
  82                         pthread_create(&thid, NULL, wait_to_deposition, NULL);
  83                         }
  84 #else
  85                 case EV_START:
  86                 case EV_ENTRY:
  87 #endif
  88                         robot_set_est_pos_trans(0.16,
  89                                                 PLAYGROUND_HEIGHT_M - 0.16,
  90                                                 DEG2RAD(-45));
  91                         FSM_TIMER(2000); // wait for the localization to settle down
  92                         break;
  93                 case EV_RETURN:
  94                 case EV_TIMER:
  95                         FSM_TRANSITION(approach_first_puck);
  96                         break;
  97                 case EV_LASER_POWER:
  98                 case EV_GOAL_NOT_REACHABLE:
  99                 case EV_SHORT_TIME_TO_END:
 100                 case EV_STACK_FULL:
 101                 case EV_ACTION_DONE:
 102                 case EV_ACTION_ERROR:
 103                 case EV_PUCK_REACHABLE:
 104                 case EV_MOTION_DONE:
 105                         DBG_PRINT_EVENT("unhandled event");
 106                         break;
 107                 case EV_EXIT:
 108                         break;
 109         }
 110 }
 111
 112
 113 /************************************************************************
 114  * MOVEMENT STATES
 115  ************************************************************************/
 116
 117 FSM_STATE(approach_first_puck)
 118 {
 119         switch (FSM_EVENT) {
 120                 case EV_ENTRY:
 121                         robot_move_by(0.42, NO_TURN(), &tcSlow);
 122                         break;
 123                 case EV_MOTION_DONE: {
 124                                 int i = 0;
 125                                 if (PUCK_REACHABLE(robot.puck_distance)) {
 126                                         printf("Puck reachable, telling act to grasp the puck\n");
 127                                         //SUBFSM_TRANSITION(grasp_the_puck, NULL); // FIXME
 128                                 } else {
 129                                         switch(i) {
 130                                         case (0):
 131                                                 printf("making transition to look_around_for_puck state\n");
 132                                                 SUBFSM_TRANSITION(look_around_for_puck, NULL);
 133                                                 break;
 134                                         case (1):
 135                                                 printf("telling the robot to move by 5 cm and try again\n");
 136                                                 robot_move_by(0.05, NO_TURN(), &tcSlow);
 137                                                 break;
 138                                         }
 139                                         i++;
 140                                 }
 141                         }
 142                         break;
 143                 case EV_PUCK_REACHABLE:
 144                 case EV_RETURN:
 145                         FSM_TRANSITION(simple_construction_zone_approach);
 146                         break;
 147                 case EV_ACTION_DONE:
 148                 case EV_TIMER:
 149                 case EV_LASER_POWER:
 150                 case EV_GOAL_NOT_REACHABLE:
 151                 case EV_SHORT_TIME_TO_END:
 152                 case EV_STACK_FULL:
 153                 case EV_START:
 154                 case EV_ACTION_ERROR:
 155                         DBG_PRINT_EVENT("unhandled event");
 156                         break;
 157                 case EV_EXIT:
 158                         break;
 159         }
 160 }
 161
 162 FSM_STATE(simple_construction_zone_approach)
 163 {
 164         switch (FSM_EVENT) {
 165                 case EV_ENTRY:
 166                         robot_trajectory_new(&tcFast);
 167                         robot_trajectory_add_point_trans(0.9, 1);
 168                         robot_trajectory_add_final_point_trans(0.9, ROBOT_AXIS_TO_FRONT_M + 0.00, NO_TURN());
 169                         break;
 170                 case EV_MOTION_DONE:
 171                         //FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
 172                         break;
 173                 case EV_ACTION_DONE:
 174                         FSM_TRANSITION(get_out_of_the_construction_zone);
 175                         break;
 176                 case EV_RETURN:
 177                 case EV_TIMER:
 178                 case EV_LASER_POWER:
 179                 case EV_GOAL_NOT_REACHABLE:
 180                 case EV_SHORT_TIME_TO_END:
 181                 case EV_STACK_FULL:
 182                 case EV_ACTION_ERROR:
 183                 case EV_PUCK_REACHABLE:
 184                 case EV_START:
 185                         DBG_PRINT_EVENT("unhandled event");
 186                         break;
 187                 case EV_EXIT:
 188                         break;
 189         }
 190 }
 191
 192 FSM_STATE(get_out_of_the_construction_zone)
 193 {
 194         switch (FSM_EVENT) {
 195                 case EV_ENTRY:
 196                         robot_move_by(-0.15, NO_TURN(), &tcSlow);
 197                         break;
 198                 case EV_MOTION_DONE:
 199                         break;
 200                 case EV_ACTION_DONE:
 201                 case EV_RETURN:
 202                 case EV_TIMER:
 203                 case EV_LASER_POWER:
 204                 case EV_GOAL_NOT_REACHABLE:
 205                 case EV_SHORT_TIME_TO_END:
 206                 case EV_STACK_FULL:
 207                 case EV_ACTION_ERROR:
 208                 case EV_PUCK_REACHABLE:
 209                 case EV_START:
 210                         DBG_PRINT_EVENT("unhandled event");
 211                         break;
 212                 case EV_EXIT:
 213                         break;
 214         }
 215 }
 216
 217 /************************************************************************
 218  * STATES HANDLING ACT's ACTIONS (to be used as SUB FSMs)
 219  ************************************************************************/
 220
 221 FSM_STATE(look_around_for_puck)
 222 {
 223         static int lfp_status = 0;
 224         const static int scatter_angle = 20;
 225         static struct robot_pos_type orig_position;
 226         switch (FSM_EVENT) {
 227                 case EV_ENTRY:
 228                         ROBOT_LOCK(ref_pos);
 229                         orig_position = robot.ref_pos;
 230                         ROBOT_UNLOCK(ref_pos);
 231                         //printf("original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 232                         robot_move_by(0, TURN_CW(DEG2RAD(RAD2DEG(orig_position.phi)-scatter_angle)), &tcSlow);
 233                         //printf("lfp_status: %d\n", lfp_status);
 234                         break;
 235                 case EV_MOTION_DONE:
 236                         switch (lfp_status) {
 237                         case 0:
 238                                 //printf("2. original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 239                                 //printf("robot.fer_pos angle of rotation of the robot: %f degrees\n", RAD2DEG(robot.ref_pos.phi));
 240                                 //printf("--- robot move by ... turn cw\n");
 241                                 robot_move_by(0, TURN_CCW(DEG2RAD(RAD2DEG(orig_position.phi)+scatter_angle)), &tcSlow);
 242                                 lfp_status++;
 243                                 break;
 244                         case 1:
 245                                 robot_move_by(0, TURN(orig_position.phi), &tcSlow);
 246                                 lfp_status++;
 247                                 break;
 248                         case 2: // puck not found
 249                                 SUBFSM_RET((void *)false);
 250                                 break;
 251                         }
 252                         //printf("lfp_status: %d\n", lfp_status);
 253                         break;
 254                 case EV_PUCK_REACHABLE: // puck found
 255                         robot_stop();
 256                         SUBFSM_RET((void *)true);
 257                         break;
 258                 case EV_ACTION_DONE:
 259                 case EV_ACTION_ERROR: // look for puck does not send this event
 260                 case EV_RETURN:
 261                 case EV_TIMER:
 262                 case EV_LASER_POWER:
 263                 case EV_GOAL_NOT_REACHABLE:
 264                 case EV_SHORT_TIME_TO_END:
 265                 case EV_STACK_FULL:
 266                 case EV_START:
 267                         DBG_PRINT_EVENT("unhandled event");
 268                         break;
 269                 case EV_EXIT:
 270                         break;
 271         }
 272 }
 273
 274 /************************************************************************
 275  * MAIN FUNCTION
 276  ************************************************************************/
 277
 278 int main()
 279 {
 280         int rv;
 281
 282         rv = robot_init();
 283         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 284
 285         robot.obstacle_avoidance_enabled = true;
 286
 287         robot.fsm.main.debug_states = 1;
 288         robot.fsm.motion.debug_states = 1;
 289         robot.fsm.act.debug_states = 1;
 290
 291         robot.fsm.main.state = &fsm_state_main_init;
 292         //robot.fsm.main.transition_callback = trans_callback;
 293         //robot.fsm.motion.transition_callback = move_trans_callback;
 294
 295         rv = robot_start();
 296         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 297
 298         robot_destroy();
 299
 300         return 0;
 301 }
 302
 303