src/robofsm/competition2012.cc

   1 /*
   2  * competition.cc       12/02/28
   3  *
   4  * Robot's control program intended for homologation (approval phase) on Eurobot 2012.
   5  *
   6  * Copyright: (c) 2012 CTU Flamingos
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 #define FSM_MAIN
  12 #include <robot.h>
  13 #include <fsm.h>
  14 #include <unistd.h>
  15 #include <math.h>
  16 #include <movehelper.h>
  17 #include <map.h>
  18 #include <sharp.h>
  19 #include <robomath.h>
  20 #include <string.h>
  21 #include <robodim.h>
  22 #include <error.h>
  23 #include "actuators.h"
  24 #include <trgen.h>
  25 #include "match-timing.h"
  26 #include <ul_log.h>
  27
  28 UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file */
  29
  30 /************************************************************************
  31  * Trajectory constraints used, are initialized in the init state
  32  ************************************************************************/
  33
  34 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
  35
  36 /************************************************************************
  37  * FSM STATES DECLARATION
  38  ************************************************************************/
  39
  40 FSM_STATE_DECL(init);
  41 FSM_STATE_DECL(wait_for_start);
  42 /* movement states - buillon */
  43 FSM_STATE_DECL(aproach_buillon);
  44 FSM_STATE_DECL(place_buillon);
  45 FSM_STATE_DECL(leave_buillon);
  46 /* Pushing the bottle */
  47 FSM_STATE_DECL(push_bottle);
  48 FSM_STATE_DECL(leave_bottle);
  49 FSM_STATE_DECL(goto_totem);
  50 FSM_STATE_DECL(approach_totem);
  51 FSM_STATE_DECL(leave_totem);
  52
  53 FSM_STATE_DECL(goto_totem2);
  54 FSM_STATE_DECL(approach_totem2);
  55 FSM_STATE_DECL(leave_totem2);
  56
  57 FSM_STATE_DECL(go_home);
  58
  59 FSM_STATE(init)
  60 {
  61         switch (FSM_EVENT) {
  62                 case EV_ENTRY:
  63                         tcSlow = trajectoryConstraintsDefault;
  64                         tcSlow.maxv = 0.3;
  65                         tcSlow.maxacc = 0.3;
  66                         tcSlow.maxomega = 1;
  67                         FSM_TRANSITION(wait_for_start);
  68                         break;
  69                 default:
  70                         break;
  71         }
  72 }
  73
  74 void set_initial_position()
  75 {
  76         // TODO define initial position
  77         robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
  78                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
  79                                 0);
  80 }
  81
  82 void actuators_home()
  83 {
  84         //act_jaws(CLOSE);
  85         // drive lift to home position
  86         //act_lift(0, 0, 1);
  87         // unset the homing request
  88         //act_lift(0, 0, 0);
  89 }
  90
  91 FSM_STATE(wait_for_start)
  92 {
  93         pthread_t thid;
  94         #ifdef WAIT_FOR_START
  95                 ul_logdeb("WAIT_FOR_START mode set\n");
  96         #else
  97                 ul_logdeb("WAIT_FOR_START mode NOT set\n");
  98         #endif
  99         #ifdef COMPETITION
 100                 ul_logdeb("COMPETITION mode set\n");
 101         #else
 102                 ul_logdeb("COMPETITION mode NOT set\n");
 103         #endif
 104         switch (FSM_EVENT) {
 105                 case EV_ENTRY:
 106                         pthread_create(&thid, NULL, timing_thread, NULL);
 107 #ifdef WAIT_FOR_START
 108                         FSM_TIMER(1000);
 109                         break;
 110 #endif
 111                 case EV_START:
 112                         /* start competition timer */
 113                         sem_post(&robot.start);
 114                         actuators_home();
 115                         set_initial_position();
 116                         FSM_TRANSITION(aproach_buillon);
 117                         break;
 118                 case EV_TIMER:
 119                         FSM_TIMER(1000);
 120                         // We set initial position periodically in
 121                         // order for it to be updated on the display
 122                         // if the team color is changed during waiting
 123                         // for start.
 124                         set_initial_position();
 125                         if (robot.start_state == START_PLUGGED_IN)
 126                                 actuators_home();
 127                         break;
 128                 case EV_RETURN:
 129                 case EV_MOTION_ERROR:
 130                 case EV_MOTION_DONE:
 131                 //case EV_VIDLE_DONE:
 132                 case EV_SWITCH_STRATEGY:
 133                         DBG_PRINT_EVENT("unhandled event");
 134                         break;
 135                 case EV_EXIT:
 136                         break;
 137                 default:
 138                         break;
 139         }
 140 }
 141
 142 FSM_STATE(aproach_buillon)
 143 {
 144         switch(FSM_EVENT) {
 145                 case EV_ENTRY:
 146                         robot_trajectory_new(&tcSlow); // new trajectory
 147                         robot_trajectory_add_point_trans(
 148                                 0.62,
 149                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
 150                         robot_trajectory_add_final_point_trans(
 151                                 0.72,
 152                                 1.14,
 153                                 TURN_CW(DEG2RAD(180)));
 154                         break;
 155                 case EV_MOTION_DONE:
 156                 case EV_TIMER:
 157                         FSM_TRANSITION(place_buillon);
 158                         break;
 159                 default:
 160                         break;
 161         }
 162 }
 163
 164 FSM_STATE(place_buillon)
 165 {
 166         switch(FSM_EVENT) {
 167                 case EV_ENTRY:
 168                         robot_trajectory_new(&tcSlow);
 169                         robot_trajectory_add_final_point_trans(
 170                                 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M,
 171                                 1.14,
 172                                 NO_TURN());
 173                         break;
 174                 case EV_MOTION_DONE:
 175                 case EV_TIMER:
 176                         FSM_TRANSITION(leave_buillon);
 177                         break;
 178                 default:
 179                         break;
 180         }
 181 }
 182
 183 FSM_STATE(leave_buillon)
 184 {
 185         switch(FSM_EVENT) {
 186                 case EV_ENTRY:
 187                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 188                         robot_trajectory_add_final_point_trans(
 189                                 0.64,
 190                                 1.14,
 191                                 NO_TURN());
 192                         break;
 193                 case EV_MOTION_DONE:
 194                 case EV_TIMER:
 195                         FSM_TRANSITION(push_bottle);
 196                         break;
 197                 default:
 198                         break;
 199         }
 200 }
 201
 202 FSM_STATE(push_bottle)
 203 {
 204         switch(FSM_EVENT) {
 205                 case EV_ENTRY:
 206                         robot_trajectory_new(&tcSlow); // new trajectory
 207                         robot_trajectory_add_point_trans(
 208                                 0.64,
 209                                 0.9);
 210                         robot_trajectory_add_final_point_trans(
 211                                 0.64,
 212                                 ROBOT_AXIS_TO_FRONT_M + 0.05,
 213                                 ARRIVE_FROM(DEG2RAD(270), 0.10));
 214                         break;
 215                 case EV_MOTION_DONE:
 216                         FSM_TRANSITION(leave_bottle);
 217                         break;
 218                 default:
 219                         break;
 220         }
 221 }
 222
 223 FSM_STATE(leave_bottle)
 224 {
 225         switch(FSM_EVENT) {
 226                 case EV_ENTRY:
 227                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 228                         robot_trajectory_add_final_point_trans(
 229                                 0.64,
 230                                 0.4,
 231                                 TURN_CCW(DEG2RAD(180)));
 232                         break;
 233                 case EV_MOTION_DONE:
 234                 case EV_TIMER:
 235                         FSM_TRANSITION(goto_totem);
 236                         break;
 237                 default:
 238                         break;
 239         }
 240 }
 241
 242 FSM_STATE(goto_totem)
 243 {
 244         switch(FSM_EVENT) {
 245                 case EV_ENTRY:
 246                         robot_trajectory_new(&tcSlow); // new trajectory
 247                         robot_trajectory_add_final_point_trans(
 248                                 1.1,
 249                                 0.4,
 250                                 TURN_CCW(DEG2RAD(90)));
 251                         break;
 252                 case EV_MOTION_DONE:
 253                         FSM_TRANSITION(approach_totem);
 254                         break;
 255                 default:
 256                         break;
 257         }
 258 }
 259
 260 FSM_STATE(approach_totem)
 261 {
 262         switch(FSM_EVENT) {
 263                 case EV_ENTRY:
 264                         robot_trajectory_new(&tcSlow); // new trajectory
 265                         robot_trajectory_add_final_point_trans(
 266                                 1.1,
 267                                 0.875 - ROBOT_AXIS_TO_FRONT_M - 0.05,
 268                                 ARRIVE_FROM(DEG2RAD(90), 0.10));
 269                         break;
 270                 case EV_MOTION_DONE:
 271                         FSM_TRANSITION(leave_totem);
 272                 default:
 273                         break;
 274         }
 275 }
 276
 277 FSM_STATE(leave_totem)
 278 {
 279         switch(FSM_EVENT) {
 280                 case EV_ENTRY:
 281                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 282                         robot_trajectory_add_final_point_trans(
 283                                 1.1,
 284                                 0.4,
 285                                 NO_TURN());
 286                         break;
 287                 case EV_MOTION_DONE:
 288                         FSM_TRANSITION(goto_totem2);
 289                 default:
 290                         break;
 291         }
 292 }
 293
 294 FSM_STATE(goto_totem2)
 295 {
 296         switch(FSM_EVENT) {
 297                 case EV_ENTRY:
 298                         robot_trajectory_new(&tcSlow); // new trajectory
 299                         robot_trajectory_add_point_trans(
 300                                 0.64,
 301                                 0.6);
 302                         robot_trajectory_add_point_trans(
 303                                 0.64,
 304                                 1.3);
 305                         robot_trajectory_add_final_point_trans(
 306                                 1.1,
 307                                 1.6,
 308                                 ARRIVE_FROM(DEG2RAD(270),0.1));
 309                         break;
 310                 case EV_MOTION_DONE:
 311                         FSM_TRANSITION(approach_totem2);
 312                 default:
 313                         break;
 314         }
 315 }
 316
 317
 318 FSM_STATE(approach_totem2)
 319 {
 320         switch(FSM_EVENT) {
 321                 case EV_ENTRY:
 322                         robot_trajectory_new(&tcSlow); // new trajectory
 323                         robot_trajectory_add_final_point_trans(
 324                                 1.1,
 325                                 1.125 + ROBOT_AXIS_TO_FRONT_M + 0.05,
 326                                 ARRIVE_FROM(DEG2RAD(270), 0.10));
 327                         break;
 328                 case EV_MOTION_DONE:
 329                         FSM_TRANSITION(leave_totem2);
 330                 default:
 331                         break;
 332         }
 333 }
 334
 335 FSM_STATE(leave_totem2)
 336 {
 337         switch(FSM_EVENT) {
 338                 case EV_ENTRY:
 339                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 340                         robot_trajectory_add_final_point_trans(
 341                                 1.1,
 342                                 1.6,
 343                                 NO_TURN());
 344                         break;
 345                 case EV_MOTION_DONE:
 346                         FSM_TRANSITION(go_home);
 347                 default:
 348                         break;
 349         }
 350 }
 351
 352 FSM_STATE(go_home)
 353 {
 354         switch(FSM_EVENT) {
 355                 case EV_ENTRY:
 356                         robot_trajectory_new(&tcSlow); // new trajectory
 357                         robot_trajectory_add_final_point_trans(
 358                                 0.5 + ROBOT_AXIS_TO_FRONT_M,
 359                                 1.75,
 360                                 ARRIVE_FROM(DEG2RAD(180),0.1));
 361                         break;
 362                 case EV_MOTION_DONE:
 363                 default:
 364                         break;
 365         }
 366 }
 367
 368 // totem 1100 x 1000 polovička totemu 125
 369 // druhá láhev na 1883
 370
 371 /************************************************************************
 372  * MAIN FUNCTION
 373  ************************************************************************/
 374
 375 int main()
 376 {
 377         int rv;
 378
 379         rv = robot_init();
 380         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 381
 382         robot.obstacle_avoidance_enabled = true;
 383
 384         robot.fsm.main.debug_states = 1;
 385         robot.fsm.motion.debug_states = 1;
 386         //robot.fsm.act.debug_states = 1;
 387
 388         robot.fsm.main.state = &fsm_state_main_init;
 389         //robot.fsm.main.transition_callback = trans_callback;
 390         //robot.fsm.motion.transition_callback = move_trans_callback;
 391
 392         rv = robot_start();
 393         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 394
 395         robot_destroy();
 396
 397         return 0;
 398 }