src/robofsm/fsmact.c

   1 #define FSM_ACT
   2
   3 #include <robot.h>
   4 #include <fsm.h>
   5 #include "actuators.h"
   6 #include <stdbool.h>
   7
   8 #define USE_DUMMY_ACTIONS // if defined, simulate actuators' actions by timeouts only
   9 //#define GIVE_UP_ON_PUCK_PRESENCE_SENSOR
  10
  11 /************************************************************
  12  *  Actuator's constants
  13  ************************************************************/
  14
  15 #define MAX_PUCKS_NUM 3 // pucks in holder
  16
  17 /************************************************************
  18  *  Actuator's variables
  19  ************************************************************/
  20
  21 typedef enum {
  22         HIGH = LIFT_TRAVEL_POS,
  23         LOW  = LIFT_GROUND_POS,
  24 } BASIC_LIFT_POSITION;
  25
  26 BASIC_LIFT_POSITION basic_lift_position = HIGH;
  27
  28 // number of pucks loaded is stored in robot.pucks_inside variable
  29 bool lintel_present = 1;
  30 int floor_to_unload;
  31
  32 /************************************************************
  33  *  STATE DECLARATIONS
  34  ************************************************************/
  35
  36 FSM_STATE_DECL(init);
  37
  38 FSM_STATE_DECL(wait_for_command);
  39
  40 FSM_STATE_DECL(prepare_for_load);
  41 FSM_STATE_DECL(scrabble); // hrabej
  42 FSM_STATE_DECL(load_the_puck);
  43 FSM_STATE_DECL(suck_the_puck);
  44 FSM_STATE_DECL(store_pucks_in_holder);
  45
  46 FSM_STATE_DECL(unload_pucks);
  47 FSM_STATE_DECL(drive_lift_into_floor);
  48
  49 FSM_STATE_DECL(dummy_action);
  50 FSM_STATE_DECL(dummy_pucks_unload);
  51
  52 /* ***********************************************************
  53  *  STATE DEFINITIONS
  54  * ***********************************************************/
  55
  56 FSM_STATE(init)
  57 {
  58         switch (FSM_EVENT) {
  59         case EV_ENTRY:
  60                 robot.pucks_inside = 0;
  61                 FSM_TRANSITION(wait_for_command);
  62                 break;
  63         case EV_PUCK_INSIDE:
  64         case EV_PREPARE_THE_UNLOAD:
  65         case EV_UNLOAD_PUCKS:
  66         case EV_TIMER:
  67         case EV_RETURN:
  68         case EV_LIFT_IN_POS:
  69         case EV_PUSHER_IN_POS:
  70         case EV_LOAD_THE_PUCK:
  71         case EV_FREE_SPACE:
  72         case EV_SCRABBLE:
  73                 DBG_PRINT_EVENT("unhandled event");
  74                 break;
  75         case EV_EXIT:
  76                 break;
  77         }
  78 }
  79
  80 FSM_STATE(wait_for_command)
  81 {
  82         switch (FSM_EVENT) {
  83         case EV_ENTRY:
  84                 printf("wait_for_command_state_entered, number of pucks stored: %d\n", robot.pucks_inside);
  85                 // put all actuators to defined initial positions
  86                 act_belts(BELTS_OFF, BELTS_OFF);
  87                 if (robot.pucks_inside<4) {
  88                         act_chelae(CHELA_OPEN, CHELA_OPEN);
  89                 } else {
  90                         act_chelae(CHELA_TIGHT, CHELA_TIGHT);
  91                 }
  92                 act_holder(HOLDER_TIGHT); //act_holder(HOLDER_LOOSE);
  93                 act_lift(basic_lift_position);
  94                 switch(basic_lift_position) {
  95                 case (LOW):
  96                         printf("basic lift position set to LOW\n");
  97                         break;
  98                 case (HIGH):
  99                         printf("basic lift position set to HIGH\n");
 100                         break;
 101                 }
 102                 act_pusher(PUSHER_INSIDE);
 103                 act_hokuyo(HOKUYO_PITCH_HORIZONTAL);
 104                 break;
 105         case EV_SCRABBLE:
 106 #ifdef USE_DUMMY_ACTIONS
 107                 FSM_TRANSITION(dummy_action);
 108 #else
 109                 FSM_TRANSITION(scrabble);
 110 #endif
 111                 break;
 112         case EV_LOAD_THE_PUCK:
 113 #ifdef USE_DUMMY_ACTIONS
 114                 FSM_TRANSITION(dummy_action);
 115 #else
 116                 FSM_TRANSITION(load_the_puck);
 117 #endif
 118                 break;
 119         case EV_PREPARE_THE_UNLOAD:
 120                 if (robot.pucks_inside == 0) {
 121                         FSM_SIGNAL(MAIN, EV_ACTION_ERROR, NULL);
 122                 } else {
 123                         floor_to_unload = FSM_EVENT_INT;
 124 #ifdef USE_DUMMY_ACTIONS
 125                         FSM_TRANSITION(dummy_pucks_unload);
 126 #else
 127                         FSM_TRANSITION(unload_pucks);
 128 #endif
 129                 }
 130                 break;
 131         case EV_LIFT_IN_POS:
 132         case EV_PUSHER_IN_POS:
 133         case EV_PUCK_INSIDE:     // we do not want react unless MAIN FSM tells us to
 134                 break;
 135         case EV_UNLOAD_PUCKS:
 136         case EV_TIMER:
 137         case EV_RETURN:
 138         case EV_FREE_SPACE:
 139                 DBG_PRINT_EVENT("unhandled event");
 140                 break;
 141         case EV_EXIT:
 142                 break;
 143         }
 144 }
 145
 146 /************************************************************
 147  * LOAD THE PUCK ACTION
 148  ************************************************************/
 149
 150 /* FSM_STATE(prepare_for_load)
 151 {
 152         switch (FSM_EVENT) {
 153         case EV_ENTRY:
 154                 act_lift(LIFT_GROUND_POS);
 155                 break;
 156         case EV_LIFT_IN_POS:
 157                 FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
 158                 break;
 159         case EV_SCRABBLE:
 160                 FSM_TRANSITION(scrabble);
 161                 break;
 162         case EV_LOAD_THE_PUCK:
 163                 FSM_TRANSITION(load_the_puck);
 164                 break;
 165         case EV_PUCK_INSIDE:
 166         case EV_PREPARE_THE_UNLOAD:
 167         case EV_UNLOAD_PUCKS:
 168         case EV_TIMER:
 169         case EV_RETURN:
 170         case EV_PUSHER_IN_POS:
 171         case EV_FREE_SPACE:
 172                 DBG_PRINT_EVENT("unhandled event");
 173                 break;
 174         case EV_EXIT:
 175                 break;
 176         }
 177 } */
 178
 179 FSM_STATE(scrabble)
 180 {
 181         switch (FSM_EVENT) {
 182         case EV_ENTRY:
 183                 act_chelae(CHELA_LOOSE, CHELA_LOOSE);
 184                 break;
 185         case EV_LOAD_THE_PUCK:
 186                 FSM_TRANSITION(load_the_puck);
 187                 break;
 188         case EV_SCRABBLE:
 189         case EV_PUCK_INSIDE:
 190         case EV_PREPARE_THE_UNLOAD:
 191         case EV_UNLOAD_PUCKS:
 192         case EV_TIMER:
 193         case EV_RETURN:
 194         case EV_LIFT_IN_POS:
 195         case EV_PUSHER_IN_POS:
 196         case EV_FREE_SPACE:
 197                 DBG_PRINT_EVENT("unhandled event");
 198                 break;
 199         case EV_EXIT:
 200                 break;
 201         }
 202 }
 203
 204 FSM_STATE(load_the_puck)
 205 {
 206         switch (FSM_EVENT) {
 207         case EV_ENTRY:
 208                 printf("load_the_puck_state entered\n");
 209                 act_lift(LIFT_MIN);
 210                 act_pusher(PUSHER_INSIDE);
 211                 act_chelae(CHELA_OPEN, CHELA_OPEN);
 212                 act_belts(BELTS_OFF, BELTS_OFF);
 213                 if (robot.pucks_inside > 0) {
 214                         act_holder(HOLDER_TIGHT);
 215                 }
 216                 break;
 217         case EV_LIFT_IN_POS:
 218                 printf("lift in position\n");
 219                 FSM_TRANSITION(suck_the_puck);
 220                 break;
 221         case EV_PUCK_INSIDE:
 222         case EV_PREPARE_THE_UNLOAD:
 223         case EV_UNLOAD_PUCKS:
 224         case EV_TIMER:
 225         case EV_RETURN:
 226         case EV_PUSHER_IN_POS:
 227         case EV_FREE_SPACE:
 228         case EV_LOAD_THE_PUCK:
 229         case EV_SCRABBLE:
 230                 DBG_PRINT_EVENT("unhandled event");
 231                 break;
 232         case EV_EXIT:
 233                 break;
 234         }
 235 }
 236
 237 FSM_STATE(suck_the_puck)
 238 {
 239         static int timer_arrival_count;
 240         switch (FSM_EVENT) {
 241         case EV_ENTRY:
 242                 // we expect lift is on the ground
 243                 timer_arrival_count = 0;
 244                 printf("suck_the_puck state entered\n");
 245                 FSM_TIMER(2000);
 246                 act_chelae(CHELA_TIGHT, CHELA_TIGHT);
 247                 act_belts(BELTS_IN, BELTS_IN);
 248                 break;
 249         case EV_PUCK_INSIDE:
 250 #ifdef GIVE_UP_ON_PUCK_PRESENCE_SENSOR
 251         case EV_TIMER:
 252                 FSM_TRANSITION(store_pucks_in_holder);
 253 #else
 254                 FSM_TRANSITION(store_pucks_in_holder);
 255                 break;
 256         case EV_TIMER:
 257                 timer_arrival_count++;
 258                 switch (timer_arrival_count) {
 259                 case 1:
 260                         act_belts(BELTS_OUT, BELTS_OUT);
 261                         FSM_TIMER(1000);
 262                         break;
 263                 case 2:
 264                         act_belts(BELTS_IN, BELTS_IN);
 265                         FSM_TIMER(2000);
 266                         break;
 267                 case 3:
 268                         FSM_SIGNAL(MAIN, EV_ACTION_ERROR, NULL);
 269                         FSM_TRANSITION(wait_for_command);
 270                         break;
 271                 }
 272 #endif
 273                 break;
 274         case EV_RETURN:
 275         case EV_LOAD_THE_PUCK:
 276         case EV_LIFT_IN_POS:
 277         case EV_PUSHER_IN_POS:
 278         case EV_PREPARE_THE_UNLOAD:
 279         case EV_UNLOAD_PUCKS:
 280         case EV_FREE_SPACE:
 281         case EV_SCRABBLE:
 282                 DBG_PRINT_EVENT("unhandled event");
 283                 break;
 284         case EV_EXIT:
 285                 act_belts(BELTS_OFF, BELTS_OFF);
 286                 act_chelae(CHELA_OPEN, CHELA_OPEN);
 287                 break;
 288         }
 289 }
 290
 291 FSM_STATE(store_pucks_in_holder)
 292 {
 293         static int i;
 294         switch (FSM_EVENT) {
 295         case EV_ENTRY:
 296                 act_holder(HOLDER_OPENED);
 297                 if (robot.pucks_inside > 2) {
 298                         robot.pucks_inside++; // FIXME: (?) change this variable only here in this state?
 299                         basic_lift_position = HIGH;
 300                         FSM_TRANSITION(wait_for_command);
 301                         FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
 302                 } else {
 303                         i = 0;
 304                         act_holder(HOLDER_OPENED);
 305                         act_lift(LIFT_IN_HOLDER_POS);
 306                 }
 307                 break;
 308         case EV_LIFT_IN_POS:
 309                 switch(i) {
 310                 case 0:
 311                         act_holder(HOLDER_TIGHT);
 312                         act_lift(LIFT_TRAVEL_POS);
 313                         break;
 314                 case 1:
 315                         robot.pucks_inside++; // FIXME: (?) change this variable in EV_ENTRY and only in one place?
 316                         basic_lift_position = HIGH;
 317                         FSM_TRANSITION(wait_for_command);
 318                         FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
 319                 }
 320                 i++;
 321                 break;
 322         case EV_PUCK_INSIDE: // ignore
 323                 break;
 324         case EV_PREPARE_THE_UNLOAD:
 325         case EV_UNLOAD_PUCKS:
 326         case EV_TIMER:
 327         case EV_RETURN:
 328         case EV_PUSHER_IN_POS:
 329         case EV_LOAD_THE_PUCK:
 330         case EV_FREE_SPACE:
 331         case EV_SCRABBLE:
 332                 DBG_PRINT_EVENT("unhandled event");
 333                 break;
 334         case EV_EXIT:
 335                 break;
 336         }
 337 }
 338
 339 /************************************************************
 340  * UNLOAD THE PUCK ACTION
 341  ************************************************************/
 342
 343 FSM_STATE(unload_pucks)
 344 {
 345         switch (FSM_EVENT) {
 346         case EV_ENTRY:
 347                 act_chelae(CHELA_OPEN, CHELA_OPEN);
 348                 printf("unloading pucks\n");
 349                 if (robot.pucks_inside < 4) {
 350                         act_lift(LIFT_BELOW_HOLDER_POS);
 351                 } else {
 352                         // if there are 4 pucks loaded, we do not need to manipulate the lift
 353                         FSM_TRANSITION(drive_lift_into_floor);
 354                 }
 355                 break;
 356         case EV_LIFT_IN_POS:
 357                 printf("lift in position\n");
 358                 FSM_TRANSITION(drive_lift_into_floor);
 359                 break;
 360         case EV_PUCK_INSIDE: // ignore
 361                 break;
 362         case EV_PREPARE_THE_UNLOAD:
 363         case EV_UNLOAD_PUCKS:
 364         case EV_TIMER:
 365         case EV_RETURN:
 366         case EV_PUSHER_IN_POS:
 367         case EV_LOAD_THE_PUCK:
 368         case EV_FREE_SPACE:
 369         case EV_SCRABBLE:
 370                 DBG_PRINT_EVENT("unhandled event");
 371                 break;
 372         case EV_EXIT:
 373                 act_holder(HOLDER_OPENED);
 374                 break;
 375         }
 376 }
 377
 378 FSM_STATE(drive_lift_into_floor)
 379 {
 380         switch (FSM_EVENT) {
 381         case EV_ENTRY: {
 382                 printf("driving lift to floor %d\n", floor_to_unload);
 383                 int lift_destination;
 384                 switch(floor_to_unload) {
 385                         case 0: lift_destination = LIFT_FLOOR0;
 386                                 break;
 387                         case 1: lift_destination = LIFT_FLOOR1 + LIFT_SHIFT;
 388                                 break;
 389                         case 2: lift_destination = LIFT_FLOOR2 + LIFT_SHIFT;
 390                                 break;
 391                         case 3: lift_destination = LIFT_FLOOR3 + LIFT_SHIFT;
 392                                 break;
 393                         case 4: lift_destination = LIFT_FLOOR4 + LIFT_SHIFT;
 394                                 break;
 395                 }
 396                 act_lift(lift_destination);
 397                 }
 398                 break;
 399         case EV_LIFT_IN_POS:
 400                 printf("lift in position\n");
 401                 FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
 402                 break;
 403         case EV_UNLOAD_PUCKS:
 404                 printf("using pusher\n");
 405                 robot.pucks_inside = 0;
 406                 act_pusher(PUSHER_FULLY_OUTSIDE);
 407                 break;
 408         case EV_PUSHER_IN_POS:
 409                 printf("pusher in position\n");
 410                 FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
 411                 break;
 412         case EV_FREE_SPACE:
 413                 printf("returning to wait_for_command state\n");
 414                 FSM_TRANSITION(wait_for_command);
 415                 break;
 416         case EV_PUCK_INSIDE: // ignore
 417                 break;
 418         case EV_PREPARE_THE_UNLOAD:
 419         case EV_TIMER:
 420         case EV_RETURN:
 421         case EV_LOAD_THE_PUCK:
 422         case EV_SCRABBLE:
 423                 DBG_PRINT_EVENT("unhandled event");
 424                 break;
 425         case EV_EXIT:
 426                 break;
 427         }
 428 }
 429
 430 /************************************************************
 431  * GENERIC DUMMY ACTION
 432  ************************************************************/
 433
 434 FSM_STATE(dummy_action)
 435 {
 436         switch (FSM_EVENT) {
 437         case EV_ENTRY:
 438                 FSM_TIMER(1000);
 439                 break;
 440         case EV_TIMER:
 441                 FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
 442                 FSM_TRANSITION(wait_for_command);
 443                 break;
 444         case EV_PUCK_INSIDE:
 445         case EV_PREPARE_THE_UNLOAD:
 446         case EV_UNLOAD_PUCKS:
 447         case EV_RETURN:
 448         case EV_LIFT_IN_POS:
 449         case EV_PUSHER_IN_POS:
 450         case EV_LOAD_THE_PUCK:
 451         case EV_FREE_SPACE:
 452         case EV_SCRABBLE:
 453                 DBG_PRINT_EVENT("unhandled event");
 454                 break;
 455         case EV_EXIT:
 456                 break;
 457         }
 458 }
 459
 460 /************************************************************
 461  * DUMMY UNLOAD ACTION
 462  ************************************************************/
 463
 464 FSM_STATE(dummy_pucks_unload)
 465 {
 466         static int dummy_unload_status;
 467         switch (FSM_EVENT) {
 468         case EV_ENTRY:
 469                 dummy_unload_status = 0;
 470                 printf("dummy: driving lift to floor %d\n", floor_to_unload);
 471                 FSM_TIMER(3000);
 472                 break;
 473         case EV_TIMER:
 474                 FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
 475                 dummy_unload_status++;
 476                 break;
 477         case EV_UNLOAD_PUCKS:
 478                 if (dummy_unload_status == 1)
 479                         FSM_TIMER(2000);
 480                 break;
 481         case EV_FREE_SPACE:
 482                 if (dummy_unload_status == 2)
 483                         FSM_TIMER(1000);
 484                 break;
 485         case EV_LIFT_IN_POS:
 486         case EV_PUSHER_IN_POS:
 487         case EV_PUCK_INSIDE:
 488         case EV_PREPARE_THE_UNLOAD:
 489         case EV_RETURN:
 490         case EV_LOAD_THE_PUCK:
 491         case EV_SCRABBLE:
 492                 DBG_PRINT_EVENT("unhandled event");
 493                 break;
 494         case EV_EXIT:
 495                 break;
 496         }
 497 }
 498
 499 /*
 500 FSM_STATE()
 501 {
 502         switch (FSM_EVENT) {
 503         case EV_ENTRY:
 504                 break;
 505         case EV_PUCK_INSIDE:
 506         case EV_PREPARE_THE_UNLOAD:
 507         case EV_UNLOAD_PUCKS:
 508         case EV_TIMER:
 509         case EV_RETURN:
 510         case EV_LIFT_IN_POS:
 511         case EV_PUSHER_IN_POS:
 512         case EV_LOAD_THE_PUCK:
 513         case EV_FREE_SPACE:
 514         case EV_SCRABBLE:
 515                 DBG_PRINT_EVENT("unhandled event");
 516                 break;
 517         case EV_EXIT:
 518                 break;
 519         }
 520 }
 521 */