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 FSM_STATE_DECL(go_home);
  53
  54 FSM_STATE(init)
  55 {
  56         switch (FSM_EVENT) {
  57                 case EV_ENTRY:
  58                         tcSlow = trajectoryConstraintsDefault;
  59                         tcSlow.maxv = 0.3;
  60                         tcSlow.maxacc = 0.3;
  61                         tcSlow.maxomega = 1;
  62                         FSM_TRANSITION(wait_for_start);
  63                         break;
  64                 default:
  65                         break;
  66         }
  67 }
  68
  69 void set_initial_position()
  70 {
  71         // TODO define initial position
  72         robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
  73                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
  74                                 0);
  75 }
  76
  77 void actuators_home()
  78 {
  79         //act_jaws(CLOSE);
  80         // drive lift to home position
  81         //act_lift(0, 0, 1);
  82         // unset the homing request
  83         //act_lift(0, 0, 0);
  84 }
  85
  86 FSM_STATE(wait_for_start)
  87 {
  88         pthread_t thid;
  89         #ifdef WAIT_FOR_START
  90                 ul_logdeb("WAIT_FOR_START mode set\n");
  91         #else
  92                 ul_logdeb("WAIT_FOR_START mode NOT set\n");
  93         #endif
  94         #ifdef COMPETITION
  95                 ul_logdeb("COMPETITION mode set\n");
  96         #else
  97                 ul_logdeb("COMPETITION mode NOT set\n");
  98         #endif
  99         switch (FSM_EVENT) {
 100                 case EV_ENTRY:
 101                         pthread_create(&thid, NULL, timing_thread, NULL);
 102 #ifdef WAIT_FOR_START
 103                         FSM_TIMER(1000);
 104                         break;
 105 #endif
 106                 case EV_START:
 107                         /* start competition timer */
 108                         sem_post(&robot.start);
 109                         actuators_home();
 110                         set_initial_position();
 111                         FSM_TRANSITION(aproach_buillon);
 112                         break;
 113                 case EV_TIMER:
 114                         FSM_TIMER(1000);
 115                         // We set initial position periodically in
 116                         // order for it to be updated on the display
 117                         // if the team color is changed during waiting
 118                         // for start.
 119                         set_initial_position();
 120                         if (robot.start_state == START_PLUGGED_IN)
 121                                 actuators_home();
 122                         break;
 123                 case EV_RETURN:
 124                 case EV_MOTION_ERROR:
 125                 case EV_MOTION_DONE:
 126                 //case EV_VIDLE_DONE:
 127                 case EV_SWITCH_STRATEGY:
 128                         DBG_PRINT_EVENT("unhandled event");
 129                         break;
 130                 case EV_EXIT:
 131                         break;
 132                 default:
 133                         break;
 134         }
 135 }
 136
 137 FSM_STATE(aproach_buillon)
 138 {
 139         switch(FSM_EVENT) {
 140                 case EV_ENTRY:
 141                         robot_trajectory_new(&tcSlow); // new trajectory
 142                         robot_trajectory_add_point_trans(
 143                                 0.62,
 144                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
 145                         robot_trajectory_add_final_point_trans(
 146                                 0.72,
 147                                 1.14,
 148                                 TURN_CW(DEG2RAD(180)));
 149                         break;
 150                 case EV_MOTION_DONE:
 151                 case EV_TIMER:
 152                         FSM_TRANSITION(place_buillon);
 153                         break;
 154                 default:
 155                         break;
 156         }
 157 }
 158
 159 FSM_STATE(place_buillon)
 160 {
 161         switch(FSM_EVENT) {
 162                 case EV_ENTRY:
 163                         robot_trajectory_new(&tcSlow);
 164                         robot_trajectory_add_final_point_trans(
 165                                 ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M,
 166                                 1.14,
 167                                 NO_TURN());
 168                         break;
 169                 case EV_MOTION_DONE:
 170                 case EV_TIMER:
 171                         FSM_TRANSITION(leave_buillon);
 172                         break;
 173                 default:
 174                         break;
 175         }
 176 }
 177
 178 FSM_STATE(leave_buillon)
 179 {
 180         switch(FSM_EVENT) {
 181                 case EV_ENTRY:
 182                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 183                         robot_trajectory_add_final_point_trans(
 184                                 0.64,
 185                                 1.14,
 186                                 NO_TURN());
 187                         break;
 188                 case EV_MOTION_DONE:
 189                 case EV_TIMER:
 190                         FSM_TRANSITION(push_bottle);
 191                         break;
 192                 default:
 193                         break;
 194         }
 195 }
 196
 197 FSM_STATE(push_bottle)
 198 {
 199         switch(FSM_EVENT) {
 200                 case EV_ENTRY:
 201                         robot_trajectory_new(&tcSlow); // new trajectory
 202                         robot_trajectory_add_point_trans(
 203                                 0.64,
 204                                 0.9);
 205                         robot_trajectory_add_final_point_trans(
 206                                 0.64,
 207                                 ROBOT_AXIS_TO_FRONT_M + 0.05,
 208                                 ARRIVE_FROM(DEG2RAD(270), 0.10));
 209                         break;
 210                 case EV_MOTION_DONE:
 211                         FSM_TRANSITION(leave_bottle);
 212                         break;
 213                 default:
 214                         break;
 215         }
 216 }
 217
 218 FSM_STATE(leave_bottle)
 219 {
 220         switch(FSM_EVENT) {
 221                 case EV_ENTRY:
 222                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 223                         robot_trajectory_add_final_point_trans(
 224                                 0.64,
 225                                 0.4,
 226                                 TURN_CCW(DEG2RAD(180)));
 227                         break;
 228                 case EV_MOTION_DONE:
 229                 case EV_TIMER:
 230                         FSM_TRANSITION(goto_totem);
 231                         break;
 232                 default:
 233                         break;
 234         }
 235 }
 236
 237 FSM_STATE(goto_totem)
 238 {
 239         switch(FSM_EVENT) {
 240                 case EV_ENTRY:
 241                         robot_trajectory_new(&tcSlow); // new trajectory
 242                         robot_trajectory_add_final_point_trans(
 243                                 1.1,
 244                                 0.4,
 245                                 TURN_CCW(DEG2RAD(90)));
 246                         break;
 247                 case EV_MOTION_DONE:
 248                         FSM_TRANSITION(approach_totem);
 249                         break;
 250                 default:
 251                         break;
 252         }
 253 }
 254
 255 FSM_STATE(approach_totem)
 256 {
 257         switch(FSM_EVENT) {
 258                 case EV_ENTRY:
 259                         robot_trajectory_new(&tcSlow); // new trajectory
 260                         robot_trajectory_add_final_point_trans(
 261                                 1.1,
 262                                 0.875 - ROBOT_AXIS_TO_FRONT_M - 0.05,
 263                                 ARRIVE_FROM(DEG2RAD(90), 0.10));
 264                         break;
 265                 case EV_MOTION_DONE:
 266                         FSM_TRANSITION(leave_totem);
 267                 default:
 268                         break;
 269         }
 270 }
 271
 272 FSM_STATE(leave_totem)
 273 {
 274         switch(FSM_EVENT) {
 275                 case EV_ENTRY:
 276                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 277                         robot_trajectory_add_final_point_trans(
 278                                 1.1,
 279                                 0.4,
 280                                 NO_TURN());
 281                         break;
 282                 case EV_MOTION_DONE:
 283                         FSM_TRANSITION(go_home);
 284                 default:
 285                         break;
 286         }
 287 }
 288
 289
 290 FSM_STATE(go_home)
 291 {
 292         switch(FSM_EVENT) {
 293                 case EV_ENTRY:
 294                         robot_trajectory_new(&tcSlow); // new trajectory
 295                         robot_trajectory_add_point_trans(
 296                                 0.64,
 297                                 0.6);
 298                         robot_trajectory_add_point_trans(
 299                                 0.64,
 300                                 1.0);
 301                         robot_trajectory_add_final_point_trans(
 302                                 0.35,
 303                                 1.0,
 304                                 ARRIVE_FROM(DEG2RAD(180),0.1));
 305                         break;
 306                 case EV_MOTION_DONE:
 307                 default:
 308                         break;
 309         }
 310 }
 311
 312 // totem 1100 x 1000 polovička totemu 125
 313 // druhá láhev na 1883
 314
 315 /************************************************************************
 316  * MAIN FUNCTION
 317  ************************************************************************/
 318
 319 int main()
 320 {
 321         int rv;
 322
 323         rv = robot_init();
 324         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 325
 326         robot.obstacle_avoidance_enabled = true;
 327
 328         robot.fsm.main.debug_states = 1;
 329         robot.fsm.motion.debug_states = 1;
 330         //robot.fsm.act.debug_states = 1;
 331
 332         robot.fsm.main.state = &fsm_state_main_init;
 333         //robot.fsm.main.transition_callback = trans_callback;
 334         //robot.fsm.motion.transition_callback = move_trans_callback;
 335
 336         rv = robot_start();
 337         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 338
 339         robot_destroy();
 340
 341         return 0;
 342 }