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                 case EV_START:
  88 #endif
  89                         robot_set_est_pos_trans(0.16,
  90                                                 PLAYGROUND_HEIGHT_M - 0.16,
  91                                                 DEG2RAD(-45));
  92                         FSM_TIMER(2000); // wait for the localization to settle down
  93                         break;
  94                 case EV_RETURN:
  95                 case EV_TIMER:
  96                         FSM_TRANSITION(approach_first_puck);
  97                         break;
  98                 case EV_LASER_POWER:
  99                 case EV_GOAL_NOT_REACHABLE:
 100                 case EV_SHORT_TIME_TO_END:
 101                 case EV_STACK_FULL:
 102                 case EV_ACTION_DONE:
 103                 case EV_ACTION_ERROR:
 104                 case EV_PUCK_REACHABLE:
 105                 case EV_MOTION_DONE:
 106                         DBG_PRINT_EVENT("unhandled event");
 107                         break;
 108                 case EV_EXIT:
 109                         break;
 110         }
 111 }
 112
 113
 114 /************************************************************************
 115  * MOVEMENT STATES
 116  ************************************************************************/
 117
 118 FSM_STATE(approach_first_puck)
 119 {
 120         switch (FSM_EVENT) {
 121                 case EV_ENTRY:
 122                         robot_move_by(0.42, NO_TURN(), &tcSlow);
 123                         break;
 124                 case EV_MOTION_DONE: {
 125                                 int i = 0;
 126                                 if (PUCK_REACHABLE(robot.puck_distance)) {
 127                                         printf("Puck reachable, telling act to grasp the puck\n");
 128                                         //SUBFSM_TRANSITION(grasp_the_puck, NULL); // FIXME
 129                                 } else {
 130                                         switch(i) {
 131                                         case (0):
 132                                                 printf("making transition to look_around_for_puck state\n");
 133                                                 SUBFSM_TRANSITION(look_around_for_puck, NULL);
 134                                                 break;
 135                                         case (1):
 136                                                 printf("telling the robot to move by 5 cm and try again\n");
 137                                                 robot_move_by(0.05, NO_TURN(), &tcSlow);
 138                                                 break;
 139                                         }
 140                                         i++;
 141                                 }
 142                         }
 143                         break;
 144                 case EV_PUCK_REACHABLE:
 145                 case EV_RETURN:
 146                         FSM_TRANSITION(simple_construction_zone_approach);
 147                         break;
 148                 case EV_ACTION_DONE:
 149                 case EV_TIMER:
 150                 case EV_LASER_POWER:
 151                 case EV_GOAL_NOT_REACHABLE:
 152                 case EV_SHORT_TIME_TO_END:
 153                 case EV_STACK_FULL:
 154                 case EV_START:
 155                 case EV_ACTION_ERROR:
 156                         DBG_PRINT_EVENT("unhandled event");
 157                         break;
 158                 case EV_EXIT:
 159                         break;
 160         }
 161 }
 162
 163 FSM_STATE(simple_construction_zone_approach)
 164 {
 165         switch (FSM_EVENT) {
 166                 case EV_ENTRY:
 167                         robot_trajectory_new(&tcFast);
 168                         robot_trajectory_add_point_trans(0.9, 1);
 169                         robot_trajectory_add_final_point_trans(0.9, ROBOT_AXIS_TO_FRONT_M + 0.00, NO_TURN());
 170                         break;
 171                 case EV_MOTION_DONE:
 172                         //FSM_SIGNAL(ACT, EV_UNLOAD_THE_PUCK, NULL);
 173                         break;
 174                 case EV_ACTION_DONE:
 175                         FSM_TRANSITION(get_out_of_the_construction_zone);
 176                         break;
 177                 case EV_RETURN:
 178                 case EV_TIMER:
 179                 case EV_LASER_POWER:
 180                 case EV_GOAL_NOT_REACHABLE:
 181                 case EV_SHORT_TIME_TO_END:
 182                 case EV_STACK_FULL:
 183                 case EV_ACTION_ERROR:
 184                 case EV_PUCK_REACHABLE:
 185                 case EV_START:
 186                         DBG_PRINT_EVENT("unhandled event");
 187                         break;
 188                 case EV_EXIT:
 189                         break;
 190         }
 191 }
 192
 193 FSM_STATE(get_out_of_the_construction_zone)
 194 {
 195         switch (FSM_EVENT) {
 196                 case EV_ENTRY:
 197                         robot_move_by(-0.15, NO_TURN(), &tcSlow);
 198                         break;
 199                 case EV_MOTION_DONE:
 200                         break;
 201                 case EV_ACTION_DONE:
 202                 case EV_RETURN:
 203                 case EV_TIMER:
 204                 case EV_LASER_POWER:
 205                 case EV_GOAL_NOT_REACHABLE:
 206                 case EV_SHORT_TIME_TO_END:
 207                 case EV_STACK_FULL:
 208                 case EV_ACTION_ERROR:
 209                 case EV_PUCK_REACHABLE:
 210                 case EV_START:
 211                         DBG_PRINT_EVENT("unhandled event");
 212                         break;
 213                 case EV_EXIT:
 214                         break;
 215         }
 216 }
 217
 218 /************************************************************************
 219  * STATES HANDLING ACT's ACTIONS (to be used as SUB FSMs)
 220  ************************************************************************/
 221
 222 FSM_STATE(look_around_for_puck)
 223 {
 224         static int lfp_status = 0;
 225         const static int scatter_angle = 20;
 226         static struct ref_pos_type orig_position;
 227         switch (FSM_EVENT) {
 228                 case EV_ENTRY:
 229                         ROBOT_LOCK(ref_pos);
 230                         orig_position = robot.ref_pos;
 231                         ROBOT_UNLOCK(ref_pos);
 232                         //printf("original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 233                         robot_move_by(0, TURN_CW(DEG2RAD(RAD2DEG(orig_position.phi)-scatter_angle)), &tcSlow);
 234                         //printf("lfp_status: %d\n", lfp_status);
 235                         break;
 236                 case EV_MOTION_DONE:
 237                         switch (lfp_status) {
 238                         case 0:
 239                                 //printf("2. original angle of rotation of the robot: %f degrees\n", RAD2DEG(orig_position.phi));
 240                                 //printf("robot.fer_pos angle of rotation of the robot: %f degrees\n", RAD2DEG(robot.ref_pos.phi));
 241                                 //printf("--- robot move by ... turn cw\n");
 242                                 robot_move_by(0, TURN_CCW(DEG2RAD(RAD2DEG(orig_position.phi)+scatter_angle)), &tcSlow);
 243                                 lfp_status++;
 244                                 break;
 245                         case 1:
 246                                 robot_move_by(0, TURN(orig_position.phi), &tcSlow);
 247                                 lfp_status++;
 248                                 break;
 249                         case 2: // puck not found
 250                                 SUBFSM_RET((void *)false);
 251                                 break;
 252                         }
 253                         //printf("lfp_status: %d\n", lfp_status);
 254                         break;
 255                 case EV_PUCK_REACHABLE: // puck found
 256                         robot_stop();
 257                         SUBFSM_RET((void *)true);
 258                         break;
 259                 case EV_ACTION_DONE:
 260                 case EV_ACTION_ERROR: // look for puck does not send this event
 261                 case EV_RETURN:
 262                 case EV_TIMER:
 263                 case EV_LASER_POWER:
 264                 case EV_GOAL_NOT_REACHABLE:
 265                 case EV_SHORT_TIME_TO_END:
 266                 case EV_STACK_FULL:
 267                 case EV_START:
 268                         DBG_PRINT_EVENT("unhandled event");
 269                         break;
 270                 case EV_EXIT:
 271                         break;
 272         }
 273 }
 274
 275 /************************************************************************
 276  * MAIN FUNCTION
 277  ************************************************************************/
 278
 279 int main()
 280 {
 281         int rv;
 282
 283         rv = robot_init();
 284         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 285
 286         robot.obstacle_avoidance_enabled = true;
 287
 288         robot.fsm.main.debug_states = 1;
 289         robot.fsm.motion.debug_states = 1;
 290         robot.fsm.act.debug_states = 1;
 291
 292         robot.fsm.main.state = &fsm_state_main_init;
 293         //robot.fsm.main.transition_callback = trans_callback;
 294         //robot.fsm.motion.transition_callback = move_trans_callback;
 295
 296         rv = robot_start();
 297         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 298
 299         robot_destroy();
 300
 301         return 0;
 302 }
 303
 304