src/robofsm/eb2008/fsmdisplay.cc

   1 /**
   2  * @file        fsmdisplay.cc
   3  * @brief       FSM for uOLED display
   4  * @author      Martin Zidek
   5  *
   6  */
   7 #define FSM_DISP
   8
   9 #include <robodata.h>
  10 #include <robot_eb2008.h>
  11 #include <signal.h>
  12 #include <pthread.h>
  13 #include <unistd.h>
  14 #include <uoled.h>
  15 #include <string.h>
  16 #include <oledlib.h>
  17
  18 /**
  19  * A thread for serial line communication.
  20  *
  21  * This thread is waititng for a character to be intercepted
  22  * on the serial line, then the character is evaluated
  23  * and appropriate EVENT is issued to inform the automaton.
  24  * @param arg
  25  *
  26  * @return
  27  */
  28
  29 struct sercom_data *sercom;
  30 uint8_t msg[10];
  31 uint8_t hw_status[6];
  32 bool msg_waiting;
  33 bool wait_flag;
  34 uint8_t mode_to_go;
  35
  36 enum {
  37         LEFT=0,
  38         RIGHT
  39 };
  40
  41 void serial_comm(int status)
  42 {
  43         uint8_t buff[10];
  44         int i,nr;
  45
  46         memset(buff, 0, 10);
  47         nr = read(sercom->fd, buff, 10);
  48
  49         if(buff[0]==MSG_START) {
  50                 ROBOT_LOCK(disp);
  51                 memcpy(msg,buff,10);
  52                 msg_waiting = true;
  53                 ROBOT_UNLOCK(disp);
  54         }
  55         /*
  56         printf("received serial msg: \n");
  57         for(i=0;i<nr;i++)
  58                 printf("%02x ",buff[i]);
  59         printf("\n");
  60         */
  61 }
  62
  63 int set_actuators(uint8_t actuator)
  64 {
  65         switch(actuator) {
  66                 case LBRUSH_SERVO:
  67                         if(robot.orte.servos.brush_left==BRUSH_LEFT_OPEN) {
  68                                 close_brush_left();
  69                         }
  70                         else {
  71                                 open_brush_left();
  72                         }
  73                         break;
  74                 case RBRUSH_SERVO:
  75                         if(robot.orte.servos.brush_right==BRUSH_RIGHT_OPEN) {
  76                                 close_brush_right();
  77                         }
  78                         else {
  79                                 open_brush_right();
  80                         }
  81                         break;
  82                 case FDOOR_SERVO:
  83                         if(robot.orte.servos.door_bottom==BOTTOM_DOOR_OPEN) {
  84                                 close_bottom_door();
  85                         }
  86                         else {
  87                                 open_bottom_door();
  88                         }
  89                         break;
  90                 case BDOOR_SERVO:
  91                         if(robot.orte.servos.door_back==BACK_DOOR_OPEN) {
  92                                 close_back_door();
  93                         }
  94                         else {
  95                                 open_back_door();
  96                         }
  97                         break;
  98                 case LBRUSH_DRIVE:
  99                         if(robot.orte.drives.brush_left==LEFT_BRUSH_DRIVE_ON) {
 100                                 off_brush_left();
 101                         }
 102                         else {
 103                                 on_brush_left();
 104                         }
 105                         break;
 106                 case RBRUSH_DRIVE:
 107                         if(robot.orte.drives.brush_right==RIGHT_BRUSH_DRIVE_ON) {
 108                                 off_brush_right();
 109                         }
 110                         else {
 111                                 on_brush_right();
 112                         }
 113                         break;
 114                 case BAGR_DRIVE:
 115                         if(robot.orte.drives.bagr==BAGR_DRIVE_ON) {
 116                                 off_bagr();
 117                         }
 118                         else {
 119                                 on_bagr();
 120                         }
 121                         break;
 122                 default:
 123                         break;
 124         }
 125         return 0;
 126 }
 127
 128 int set_game_color(uint8_t col)
 129 {
 130         return 0;
 131 }
 132
 133 int move_carousel(uint8_t dir)
 134 {
 135         uint8_t act_car_pos, new_car_pos;
 136         ROBOT_LOCK(drives);
 137         act_car_pos = robot.orte.drives.carousel_pos;
 138         ROBOT_UNLOCK(drives);
 139         switch(dir) {
 140                 case LEFT:
 141                         if(act_car_pos==0)
 142                                 new_car_pos = 4;
 143                         else
 144                                 new_car_pos = --act_car_pos;
 145                         break;
 146                 case RIGHT:
 147                         if(act_car_pos==4)
 148                                 new_car_pos = 0;
 149                         else
 150                                 new_car_pos = ++act_car_pos;
 151                         break;
 152                 default:
 153                         new_car_pos = act_car_pos;
 154                         break;
 155         }
 156         ROBOT_LOCK(drives);
 157         robot.orte.drives.carousel_pos = new_car_pos;
 158         ROBOT_UNLOCK(drives);
 159         return 0;
 160 }
 161
 162 int process_msg(uint8_t *buff) {
 163         switch(buff[1]) {
 164                 case SWITCH_TO_STATUS:
 165                         uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_STATUS);
 166                         wait_flag = true;
 167                         return MODE_STATUS;
 168                         break;
 169                 case SWITCH_TO_CONTROL:
 170                         uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_CONTROL);
 171                         wait_flag = true;
 172                         return MODE_CONTROL;
 173                         break;
 174                 case SWITCH_TO_STATUS_DONE:
 175                         wait_flag = false;
 176                         mode_to_go = MODE_STATUS;
 177                         return MODE_STATUS;
 178                         break;
 179                 case SWITCH_TO_CONTROL_DONE:
 180                         wait_flag = false;
 181                         mode_to_go = MODE_CONTROL;
 182                         return MODE_CONTROL;
 183                         break;
 184                 case ACTUATORS:
 185                         set_actuators(buff[2]);
 186                         break;
 187                 case SET_COLOR:
 188                         set_game_color(buff[2]); //TODO
 189                         break;
 190                 case CAR_LEFT:
 191                         move_carousel(LEFT);
 192                         break;
 193                 case CAR_RIGHT:
 194                         move_carousel(RIGHT);
 195                         break;
 196                 case GET_READY:
 197                         //TODO robot get ready function
 198                         break;
 199                 case ROBOT_START:
 200                         //TODO robot start function
 201                         break;
 202                 default:
 203                         break;
 204         }
 205         return -1;
 206 }
 207
 208
 209 FSM_STATE_DECL(init);
 210 FSM_STATE_DECL(control);
 211 FSM_STATE_DECL(status);
 212 FSM_STATE_DECL(status_send_voltage);
 213 FSM_STATE_DECL(status_send_state);
 214 FSM_STATE_DECL(status_send_balls);
 215 FSM_STATE_DECL(status_send_hw);
 216 FSM_STATE_DECL(status_send_position);
 217 FSM_STATE_DECL(set_init_mode);
 218 FSM_STATE_DECL(wait_for_mode_switch);
 219
 220 FSM_STATE(init)
 221 {
 222
 223         if (FSM_EVENT == EV_ENTRY) {
 224                 msg_waiting = false;
 225                 sercom = robot.sercom;
 226                 FSM_TRANSITION(set_init_mode);
 227         }
 228 }
 229
 230 FSM_STATE(set_init_mode)
 231 {
 232         switch(FSM_EVENT) {
 233                 case EV_ENTRY:
 234                         uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_STATUS);
 235                         FSM_TIMER(500);
 236                         break;
 237                 case EV_TIMER:
 238                         FSM_TRANSITION(status);
 239                         break;
 240                 default:
 241                         break;
 242         }
 243 }
 244
 245 FSM_STATE(status)
 246 {
 247         uint8_t buff[10];
 248         uint8_t ret;
 249
 250         switch (FSM_EVENT) {
 251                 case EV_ENTRY:
 252                         uoled_set_color(robot.team_color);
 253                         FSM_TIMER(1000);
 254                         break;
 255                 case EV_TIMER:
 256                         uoled_send_voltage(&robot.gorte.pwr_voltage);
 257                         uoled_send_state(robot.act_fsm_state_name, strlen(robot.act_fsm_state_name));
 258                         uoled_set_balls((uint8_t*)&robot.carousel);
 259                         uoled_send_position(&robot.gorte.est_pos);
 260                         uoled_send_hw_status(robot.hw_status);
 261                         ROBOT_LOCK(disp);
 262                         if(msg_waiting) {
 263                                 memcpy(buff, msg, 10);
 264                                 msg_waiting = false;
 265                                 ROBOT_UNLOCK(disp);
 266                                 ret = process_msg(buff);
 267                                 switch(ret) {
 268                                         case MODE_CONTROL:
 269                                                 FSM_TRANSITION(wait_for_mode_switch);
 270                                                 break;
 271                                         default:
 272                                                 break;
 273                                 }
 274                         }
 275                         else
 276                                 ROBOT_UNLOCK(disp);
 277                         //FSM_TRANSITION(status_send_voltage);
 278                         FSM_TIMER(1000);
 279                         break;
 280                 default:
 281                         break;
 282         }
 283 }
 284
 285 FSM_STATE(status_send_voltage)
 286 {
 287         uint8_t buff[10];
 288         uint8_t ret;
 289
 290         switch (FSM_EVENT) {
 291                 case EV_ENTRY:
 292                         uoled_send_voltage(&robot.gorte.pwr_voltage);
 293                         FSM_TIMER(400);
 294                         break;
 295                 case EV_TIMER:
 296                         ROBOT_LOCK(disp);
 297                         if(msg_waiting) {
 298                                 memcpy(buff, msg, 10);
 299                                 msg_waiting = false;
 300                                 ROBOT_UNLOCK(disp);
 301                                 ret = process_msg(buff);
 302                                 switch(ret) {
 303                                         case MODE_CONTROL:
 304                                                 FSM_TRANSITION(wait_for_mode_switch);
 305                                                 break;
 306                                         default:
 307                                                 break;
 308                                 }
 309                         }
 310                         else
 311                                 ROBOT_UNLOCK(disp);
 312                         FSM_TRANSITION(status_send_state);
 313                         break;
 314                 default:
 315                         break;
 316         }
 317 }
 318
 319 FSM_STATE(status_send_state)
 320 {
 321         uint8_t buff[10];
 322         uint8_t ret;
 323
 324         switch (FSM_EVENT) {
 325                 case EV_ENTRY:
 326                         uoled_send_state(fsm->state_name, strlen(fsm->state_name));
 327                         FSM_TIMER(400);
 328                         break;
 329                 case EV_TIMER:
 330                         ROBOT_LOCK(disp);
 331                         if(msg_waiting) {
 332                                 memcpy(buff, msg, 10);
 333                                 msg_waiting = false;
 334                                 ROBOT_UNLOCK(disp);
 335                                 ret = process_msg(buff);
 336                                 switch(ret) {
 337                                         case MODE_CONTROL:
 338                                                 FSM_TRANSITION(wait_for_mode_switch);
 339                                                 break;
 340                                         default:
 341                                                 break;
 342                                 }
 343                         }
 344                         else
 345                                 ROBOT_UNLOCK(disp);
 346                         FSM_TRANSITION(status_send_balls);
 347                         break;
 348                 default:
 349                         break;
 350         }
 351 }
 352
 353
 354 FSM_STATE(status_send_balls)
 355 {
 356         uint8_t buff[10];
 357         uint8_t ret;
 358
 359         switch (FSM_EVENT) {
 360                 case EV_ENTRY:
 361                         uoled_set_balls((uint8_t*)&robot.carousel);
 362                         FSM_TIMER(400);
 363                         break;
 364                 case EV_TIMER:
 365                         ROBOT_LOCK(disp);
 366                         if(msg_waiting) {
 367                                 memcpy(buff, msg, 10);
 368                                 msg_waiting = false;
 369                                 ROBOT_UNLOCK(disp);
 370                                 ret = process_msg(buff);
 371                                 switch(ret) {
 372                                         case MODE_CONTROL:
 373                                                 FSM_TRANSITION(wait_for_mode_switch);
 374                                                 break;
 375                                         default:
 376                                                 break;
 377                                 }
 378                         }
 379                         else
 380                                 ROBOT_UNLOCK(disp);
 381                         FSM_TRANSITION(status_send_hw);
 382                         break;
 383                 default:
 384                         break;
 385         }
 386 }
 387
 388
 389 FSM_STATE(status_send_hw)
 390 {
 391         uint8_t buff[10];
 392         uint8_t ret;
 393
 394         switch (FSM_EVENT) {
 395                 case EV_ENTRY:
 396                         //TODO: send HW status here
 397                         FSM_TIMER(400);
 398                         break;
 399                 case EV_TIMER:
 400                         ROBOT_LOCK(disp);
 401                         if(msg_waiting) {
 402                                 memcpy(buff, msg, 10);
 403                                 msg_waiting = false;
 404                                 ROBOT_UNLOCK(disp);
 405                                 ret = process_msg(buff);
 406                                 switch(ret) {
 407                                         case MODE_CONTROL:
 408                                                 FSM_TRANSITION(wait_for_mode_switch);
 409                                                 break;
 410                                         default:
 411                                                 break;
 412                                 }
 413                         }
 414                         else
 415                                 ROBOT_UNLOCK(disp);
 416                         FSM_TRANSITION(status_send_position);
 417                         break;
 418                 default:
 419                         break;
 420         }
 421 }
 422
 423
 424 FSM_STATE(status_send_position)
 425 {
 426         uint8_t buff[10];
 427         uint8_t ret;
 428
 429         switch (FSM_EVENT) {
 430                 case EV_ENTRY:
 431                         uoled_send_position(&robot.gorte.est_pos);
 432                         FSM_TIMER(400);
 433                         break;
 434                 case EV_TIMER:
 435                         ROBOT_LOCK(disp);
 436                         if(msg_waiting) {
 437                                 memcpy(buff, msg, 10);
 438                                 msg_waiting = false;
 439                                 ROBOT_UNLOCK(disp);
 440                                 ret = process_msg(buff);
 441                                 switch(ret) {
 442                                         case MODE_CONTROL:
 443                                                 FSM_TRANSITION(wait_for_mode_switch);
 444                                                 break;
 445                                         default:
 446                                                 break;
 447                                 }
 448                         }
 449                         else
 450                                 ROBOT_UNLOCK(disp);
 451                         FSM_TRANSITION(status);
 452                         break;
 453                 default:
 454                         break;
 455         }
 456 }
 457
 458 FSM_STATE(control)
 459 {
 460         uint8_t buff[10];
 461
 462         switch (FSM_EVENT) {
 463                 case EV_ENTRY:
 464                         FSM_TIMER(1000);
 465                         break;
 466                 case EV_TIMER:
 467                         ROBOT_LOCK(disp);
 468                         if(msg_waiting) {
 469                                 memcpy(buff, msg, 10);
 470                                 msg_waiting = false;
 471                                 ROBOT_UNLOCK(disp);
 472                                 switch(process_msg(buff)) {
 473                                         case MODE_STATUS:
 474                                                 FSM_TRANSITION(wait_for_mode_switch);
 475                                                 break;
 476                                         default:
 477                                                 break;
 478                                 }
 479                         }
 480                         else
 481                                 ROBOT_UNLOCK(disp);
 482                         FSM_TIMER(1000);
 483                         break;
 484                 default:
 485                         break;
 486         }
 487 }
 488
 489
 490 FSM_STATE(wait_for_mode_switch)
 491 {
 492         uint8_t buff[10];
 493
 494         switch (FSM_EVENT) {
 495                 case EV_ENTRY:
 496                         FSM_TIMER(500);
 497                         break;
 498                 case EV_TIMER:
 499                         ROBOT_LOCK(disp);
 500                         if(msg_waiting) {
 501                                 memcpy(buff, msg, 10);
 502                                 msg_waiting = false;
 503                                 ROBOT_UNLOCK(disp);
 504                                 process_msg(buff);
 505                         }
 506                         else
 507                                 ROBOT_UNLOCK(disp);
 508                         if(wait_flag)
 509                                 break;
 510                         else {
 511                                 if(mode_to_go==MODE_STATUS)
 512                                         FSM_TRANSITION(status);
 513                                 else if(mode_to_go==MODE_CONTROL)
 514                                         FSM_TRANSITION(control);
 515                         }
 516                         break;
 517                 default:
 518                         break;
 519         }
 520 }
 521