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 /************************************************************
   9  *  Actuator's constants
  10  ************************************************************/
  11
  12 #define MAX_PUCKS_NUM 3 // pucks in holder
  13
  14 /************************************************************
  15  *  Actuator's variables
  16  ************************************************************/
  17
  18 typedef enum {
  19         HIGH = LIFT_TRAVEL_POS,
  20         LOW  = LIFT_GROUND_POS,
  21 } BASIC_LIFT_POSITION;
  22
  23 BASIC_LIFT_POSITION basic_lift_position = HIGH;
  24 int pucks_loaded = 0;
  25 bool lintel_present = 1;
  26 int floor_to_unload;
  27
  28 /************************************************************
  29  *  STATE DECLARATIONS
  30  ************************************************************/
  31
  32 FSM_STATE_DECL(wait_for_command);
  33
  34 FSM_STATE_DECL(prepare_for_load);
  35 FSM_STATE_DECL(scrabble); // hrabej
  36 FSM_STATE_DECL(load_the_puck);
  37 FSM_STATE_DECL(suck_the_puck);
  38 FSM_STATE_DECL(store_pucks_in_holder);
  39
  40 FSM_STATE_DECL(unload_pucks);
  41 FSM_STATE_DECL(drive_lift_into_floor);
  42
  43 /* ***********************************************************
  44  *  STATE DEFINITIONS
  45  * ***********************************************************/
  46
  47 FSM_STATE(wait_for_command)
  48 {
  49         switch (FSM_EVENT) {
  50         case EV_ENTRY:
  51                 printf("wait_for_command_state_entered, number of pucks stored: %d\n", pucks_loaded);
  52                 // put all actuators to defined initial positions
  53                 act_belts(BELTS_OFF, BELTS_OFF);
  54                 if (pucks_loaded<4) {
  55                         act_chelae(CHELA_OPEN, CHELA_OPEN);
  56                 } else {
  57                         act_chelae(CHELA_TIGHT, CHELA_TIGHT);
  58                 }
  59                 act_holder(HOLDER_LOOSE);
  60                 act_lift(basic_lift_position);
  61                 switch(basic_lift_position) {
  62                 case (LOW):
  63                         printf("basic lift position set to LOW\n");
  64                         break;
  65                 case (HIGH):
  66                         printf("basic lift position set to HIGH\n");
  67                         break;
  68                 }
  69                 act_pusher(PUSHER_INSIDE);
  70                 act_hokuyo(HOKUYO_PITCH_HORIZONTAL);
  71                 break;
  72         case EV_PREPARE_FOR_LOAD:
  73                 //basic_lift_position = LOW;
  74                 //FSM_TRANSITION(wait_for_command);
  75                 break;
  76         case EV_SCRABBLE:
  77                 FSM_TRANSITION(scrabble);
  78                 break;
  79         case EV_LOAD_THE_PUCK:
  80                 FSM_TRANSITION(load_the_puck);
  81                 break;
  82         case EV_PREPARE_THE_UNLOAD:
  83                 if (pucks_loaded == 0) {
  84                         FSM_SIGNAL(MAIN, EV_ACTION_ERROR, NULL);
  85                 } else {
  86                         floor_to_unload = FSM_EVENT_INT;
  87                         FSM_TRANSITION(unload_pucks);
  88                 }
  89                 break;
  90         case EV_LIFT_IN_POS:
  91         case EV_PUSHER_IN_POS:
  92         case EV_PUCK_INSIDE:     // we do not want react unless MAIN FSM tells us to
  93                 break;
  94         case EV_UNLOAD_PUCKS:
  95         case EV_RETURN:
  96         case EV_FREE_SPACE:
  97         case EV_TIMER:
  98                 DBG_PRINT_EVENT("unhandled event");
  99                 break;
 100         case EV_EXIT:
 101                 break;
 102         }
 103 }
 104
 105 /************************************************************
 106  * LOAD THE PUCK ACTION
 107  ************************************************************/
 108
 109 /* FSM_STATE(prepare_for_load)
 110 {
 111         switch (FSM_EVENT) {
 112         case EV_ENTRY:
 113                 act_lift(LIFT_GROUND_POS);
 114                 break;
 115         case EV_LIFT_IN_POS:
 116                 FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
 117                 break;
 118         case EV_SCRABBLE:
 119                 FSM_TRANSITION(scrabble);
 120                 break;
 121         case EV_LOAD_THE_PUCK:
 122                 FSM_TRANSITION(load_the_puck);
 123                 break;
 124         case EV_PUCK_INSIDE:
 125         case EV_PREPARE_FOR_LOAD:
 126         case EV_PREPARE_THE_UNLOAD:
 127         case EV_UNLOAD_PUCKS:
 128         case EV_TIMER:
 129         case EV_RETURN:
 130         case EV_PUSHER_IN_POS:
 131         case EV_FREE_SPACE:
 132                 DBG_PRINT_EVENT("unhandled event");
 133                 break;
 134         case EV_EXIT:
 135                 break;
 136         }
 137 } */
 138
 139 FSM_STATE(scrabble)
 140 {
 141         switch (FSM_EVENT) {
 142         case EV_ENTRY:
 143                 act_chelae(CHELA_LOOSE, CHELA_LOOSE);
 144                 break;
 145         case EV_LOAD_THE_PUCK:
 146                 FSM_TRANSITION(load_the_puck);
 147                 break;
 148         case EV_SCRABBLE:
 149         case EV_PUCK_INSIDE:
 150         case EV_PREPARE_FOR_LOAD:
 151         case EV_PREPARE_THE_UNLOAD:
 152         case EV_UNLOAD_PUCKS:
 153         case EV_TIMER:
 154         case EV_RETURN:
 155         case EV_LIFT_IN_POS:
 156         case EV_PUSHER_IN_POS:
 157         case EV_FREE_SPACE:
 158                 DBG_PRINT_EVENT("unhandled event");
 159                 break;
 160         case EV_EXIT:
 161                 break;
 162         }
 163 }
 164
 165 FSM_STATE(load_the_puck)
 166 {
 167         switch (FSM_EVENT) {
 168         case EV_ENTRY:
 169                 printf("load_the_puck_state entered\n");
 170                 act_lift(LIFT_GROUND_POS);
 171                 act_pusher(PUSHER_INSIDE);
 172                 act_chelae(CHELA_OPEN, CHELA_OPEN);
 173                 act_belts(BELTS_OFF, BELTS_OFF);
 174                 // FIXME: act_holder ?
 175                 break;
 176         case EV_LIFT_IN_POS:
 177                 printf("lift in position\n");
 178                 FSM_TRANSITION(suck_the_puck);
 179                 break;
 180         case EV_PUCK_INSIDE:
 181         case EV_PREPARE_THE_UNLOAD:
 182         case EV_UNLOAD_PUCKS:
 183         case EV_TIMER:
 184         case EV_RETURN:
 185         case EV_PUSHER_IN_POS:
 186         case EV_FREE_SPACE:
 187         case EV_PREPARE_FOR_LOAD:
 188         case EV_LOAD_THE_PUCK:
 189         case EV_SCRABBLE:
 190                 DBG_PRINT_EVENT("unhandled event");
 191                 break;
 192         case EV_EXIT:
 193                 break;
 194         }
 195 }
 196
 197 FSM_STATE(suck_the_puck)
 198 {
 199         static int timer_arrival_count;
 200         switch (FSM_EVENT) {
 201         case EV_ENTRY:
 202                 act_lift(LIFT_MIN);
 203         case EV_LIFT_IN_POS:
 204                 timer_arrival_count = 0;
 205                 printf("suck_the_puck state entered\n");
 206                 FSM_TIMER(800);
 207                 //FSM_TIMER(2000);
 208                 act_chelae(CHELA_TIGHT, CHELA_TIGHT);
 209                 act_belts(BELTS_IN, BELTS_IN);
 210                 break;
 211         case EV_PUCK_INSIDE:
 212         case EV_TIMER:
 213                 printf("puck inside\n");
 214                 FSM_TRANSITION(store_pucks_in_holder);
 215                 break;
 216         /*case EV_TIMER:
 217                 switch (timer_arrival_count) {
 218                 case 0:
 219                         act_belts(BELTS_OUT, BELTS_OUT);
 220                         FSM_TIMER(1000);
 221                         break;
 222                 case 1:
 223                         act_belts(BELTS_IN, BELTS_IN);
 224                         FSM_TIMER(2000);
 225                         break;
 226                 case 2:
 227                         FSM_SIGNAL(MAIN, EV_ACTION_ERROR, NULL);
 228                         FSM_TRANSITION(wait_for_command);
 229                         break;
 230                 }
 231                 timer_arrival_count++; */
 232                 break;
 233         case EV_RETURN:
 234         case EV_LOAD_THE_PUCK:
 235         case EV_PUSHER_IN_POS:
 236         case EV_PREPARE_THE_UNLOAD:
 237         case EV_UNLOAD_PUCKS:
 238         case EV_FREE_SPACE:
 239         case EV_PREPARE_FOR_LOAD:
 240         case EV_SCRABBLE:
 241                 DBG_PRINT_EVENT("unhandled event");
 242                 break;
 243         case EV_EXIT:
 244                 act_belts(BELTS_OFF, BELTS_OFF);
 245                 act_chelae(CHELA_OPEN, CHELA_OPEN);
 246                 break;
 247         }
 248 }
 249
 250 FSM_STATE(store_pucks_in_holder)
 251 {
 252         static int i;
 253         switch (FSM_EVENT) {
 254         case EV_ENTRY:
 255                 act_holder(HOLDER_OPENED);
 256                 if (pucks_loaded > 2) {
 257                         pucks_loaded++; // FIXME: (?) change this variable only here in this state?
 258                         basic_lift_position = HIGH;
 259                         FSM_TRANSITION(wait_for_command);
 260                         FSM_SIGNAL(MAIN, EV_ACTION_DONE, (void*)pucks_loaded);
 261                 } else {
 262                         i = 0;
 263                         act_holder(HOLDER_OPENED);
 264                         act_lift(LIFT_IN_HOLDER_POS);
 265                 }
 266                 break;
 267         case EV_LIFT_IN_POS:
 268                 switch(i) {
 269                 case 0:
 270                         act_holder(HOLDER_TIGHT);
 271                         act_lift(LIFT_TRAVEL_POS);
 272                         break;
 273                 case 1:
 274                         pucks_loaded++; // FIXME: (?) change this variable in EV_ENTRY and only in one place?
 275                         basic_lift_position = HIGH;
 276                         FSM_TRANSITION(wait_for_command);
 277                         FSM_SIGNAL(MAIN, EV_ACTION_DONE, (void*)pucks_loaded);
 278                 }
 279                 i++;
 280                 break;
 281         case EV_PUCK_INSIDE: // ignore
 282                 break;
 283         case EV_PREPARE_THE_UNLOAD:
 284         case EV_UNLOAD_PUCKS:
 285         case EV_TIMER:
 286         case EV_RETURN:
 287         case EV_PUSHER_IN_POS:
 288         case EV_LOAD_THE_PUCK:
 289         case EV_FREE_SPACE:
 290         case EV_PREPARE_FOR_LOAD:
 291         case EV_SCRABBLE:
 292                 DBG_PRINT_EVENT("unhandled event");
 293                 break;
 294         case EV_EXIT:
 295                 break;
 296         }
 297 }
 298
 299 /************************************************************
 300  * UNLOAD THE PUCK ACTION
 301  ************************************************************/
 302
 303 FSM_STATE(unload_pucks)
 304 {
 305         switch (FSM_EVENT) {
 306         case EV_ENTRY:
 307                 act_chelae(CHELA_OPEN, CHELA_OPEN);
 308                 printf("unloading pucks\n");
 309                 if (pucks_loaded < 4) {
 310                         act_lift(LIFT_BELOW_HOLDER_POS);
 311                 } else {
 312                         // if there are 4 pucks loaded, we do not need to manipulate the lift
 313                         FSM_TRANSITION(drive_lift_into_floor);
 314                 }
 315                 break;
 316         case EV_LIFT_IN_POS:
 317                 printf("lift in position\n");
 318                 FSM_TRANSITION(drive_lift_into_floor);
 319                 break;
 320         case EV_PUCK_INSIDE: // ignore
 321                 break;
 322         case EV_PREPARE_THE_UNLOAD:
 323         case EV_UNLOAD_PUCKS:
 324         case EV_TIMER:
 325         case EV_RETURN:
 326         case EV_PUSHER_IN_POS:
 327         case EV_LOAD_THE_PUCK:
 328         case EV_FREE_SPACE:
 329         case EV_PREPARE_FOR_LOAD:
 330         case EV_SCRABBLE:
 331                 DBG_PRINT_EVENT("unhandled event");
 332                 break;
 333         case EV_EXIT:
 334                 act_holder(HOLDER_OPENED);
 335                 break;
 336         }
 337 }
 338
 339 FSM_STATE(drive_lift_into_floor)
 340 {
 341         switch (FSM_EVENT) {
 342         case EV_ENTRY: {
 343                 printf("driving lift to floor %d\n", floor_to_unload);
 344                 int lift_destination;
 345                 switch(floor_to_unload) {
 346                         case 0: lift_destination = LIFT_FLOOR0;
 347                                 break;
 348                         case 1: lift_destination = LIFT_FLOOR1 + LIFT_SHIFT;
 349                                 break;
 350                         case 2: lift_destination = LIFT_FLOOR2 + LIFT_SHIFT;
 351                                 break;
 352                         case 3: lift_destination = LIFT_FLOOR3 + LIFT_SHIFT;
 353                                 break;
 354                         case 4: lift_destination = LIFT_FLOOR4 + LIFT_SHIFT;
 355                                 break;
 356                 }
 357                 act_lift(lift_destination);
 358                 }
 359                 break;
 360         case EV_LIFT_IN_POS:
 361                 printf("lift in position\n");
 362                 FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
 363                 break;
 364         case EV_UNLOAD_PUCKS:
 365                 printf("using pusher\n");
 366                 pucks_loaded = 0;
 367                 act_pusher(PUSHER_FULLY_OUTSIDE);
 368                 break;
 369         case EV_PUSHER_IN_POS:
 370                 printf("pusher in position\n");
 371                 FSM_SIGNAL(MAIN, EV_ACTION_DONE, NULL);
 372                 break;
 373         case EV_FREE_SPACE:
 374                 printf("returning to wait_for_command state\n");
 375                 FSM_TRANSITION(wait_for_command);
 376                 break;
 377         case EV_PUCK_INSIDE: // ignore
 378                 break;
 379         case EV_PREPARE_THE_UNLOAD:
 380         case EV_TIMER:
 381         case EV_RETURN:
 382         case EV_LOAD_THE_PUCK:
 383         case EV_PREPARE_FOR_LOAD:
 384         case EV_SCRABBLE:
 385                 DBG_PRINT_EVENT("unhandled event");
 386                 break;
 387         case EV_EXIT:
 388                 break;
 389         }
 390 }
 391
 392 /*
 393 FSM_STATE()
 394 {
 395         switch (FSM_EVENT) {
 396         case EV_ENTRY:
 397                 break;
 398         case EV_PUCK_INSIDE:
 399         case EV_PREPARE_THE_UNLOAD:
 400         case EV_UNLOAD_PUCKS:
 401         case EV_TIMER:
 402         case EV_RETURN:
 403         case EV_LIFT_IN_POS:
 404         case EV_PUSHER_IN_POS:
 405         case EV_LOAD_THE_PUCK:
 406         case EV_FREE_SPACE:
 407         case EV_PREPARE_FOR_LOAD:
 408         case EV_SCRABBLE:
 409                 DBG_PRINT_EVENT("unhandled event");
 410                 break;
 411         case EV_EXIT:
 412                 break;
 413         }
 414 }
 415 */