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 /* from homologation */
  43 /* movement states - buillon */
  44 FSM_STATE_DECL(aproach_buillon);
  45 /* Pushing the bottle */
  46 FSM_STATE_DECL(push_bottle);
  47 /* New states */
  48 FSM_STATE_DECL(reach_central_buillon);
  49 FSM_STATE_DECL(leave_central_buillon);
  50 FSM_STATE_DECL(push_bottle_bw);
  51 FSM_STATE_DECL(return_home);
  52
  53 FSM_STATE(init)
  54 {
  55         switch (FSM_EVENT) {
  56                 case EV_ENTRY:
  57                         tcSlow = trajectoryConstraintsDefault;
  58                         tcSlow.maxv = 0.3;
  59                         tcSlow.maxacc = 0.3;
  60                         tcSlow.maxomega = 1;
  61                         FSM_TRANSITION(wait_for_start);
  62                         break;
  63                 default:
  64                         break;
  65         }
  66 }
  67
  68 void set_initial_position()
  69 {
  70         // TODO define initial position
  71         robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
  72                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
  73                                 0);
  74 }
  75
  76 void actuators_home()
  77 {
  78         //act_jaws(CLOSE);
  79         // drive lift to home position
  80         //act_lift(0, 0, 1);
  81         // unset the homing request
  82         //act_lift(0, 0, 0);
  83 }
  84
  85 FSM_STATE(wait_for_start)
  86 {
  87         pthread_t thid;
  88         #ifdef WAIT_FOR_START
  89                 ul_logdeb("WAIT_FOR_START mode set\n");
  90         #else
  91                 ul_logdeb("WAIT_FOR_START mode NOT set\n");
  92         #endif
  93         #ifdef COMPETITION
  94                 ul_logdeb("COMPETITION mode set\n");
  95         #else
  96                 ul_logdeb("COMPETITION mode NOT set\n");
  97         #endif
  98         switch (FSM_EVENT) {
  99                 case EV_ENTRY:
 100                         pthread_create(&thid, NULL, timing_thread, NULL);
 101 #ifdef WAIT_FOR_START
 102                         FSM_TIMER(1000);
 103                         break;
 104 #endif
 105                 case EV_START:
 106                         /* start competition timer */
 107                         sem_post(&robot.start);
 108                         actuators_home();
 109                         set_initial_position();
 110                         FSM_TRANSITION(aproach_buillon);
 111                         break;
 112                 case EV_TIMER:
 113                         FSM_TIMER(1000);
 114                         // We set initial position periodically in
 115                         // order for it to be updated on the display
 116                         // if the team color is changed during waiting
 117                         // for start.
 118                         set_initial_position();
 119                         if (robot.start_state == START_PLUGGED_IN)
 120                                 actuators_home();
 121                         break;
 122                 case EV_RETURN:
 123                 case EV_MOTION_ERROR:
 124                 case EV_MOTION_DONE:
 125                 //case EV_VIDLE_DONE:
 126                 case EV_SWITCH_STRATEGY:
 127                         DBG_PRINT_EVENT("unhandled event");
 128                         break;
 129                 case EV_EXIT:
 130                         break;
 131                 default:
 132                         break;
 133         }
 134 }
 135
 136 FSM_STATE(aproach_buillon)
 137 {
 138         switch(FSM_EVENT) {
 139                 case EV_ENTRY:
 140                         robot_trajectory_new(&tcSlow); // new trajectory
 141                         robot_trajectory_add_point_trans(
 142                                 0.65,
 143                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
 144                         robot_trajectory_add_point_trans(
 145                                 0.65,
 146                                 1.3);
 147                         robot_trajectory_add_final_point_trans(
 148                                 0.5,
 149                                 1.1,
 150                                 NO_TURN());
 151                         break;
 152                 case EV_MOTION_DONE:
 153                 case EV_TIMER:
 154                         FSM_TRANSITION(reach_central_buillon);
 155                         break;
 156                 default:
 157                         break;
 158         }
 159 }
 160
 161 FSM_STATE(push_bottle)
 162 {
 163         switch(FSM_EVENT) {
 164                 case EV_ENTRY:
 165                         robot_trajectory_new(&tcSlow); // new trajectory
 166                         robot_trajectory_add_point_trans(
 167                                 0.64,
 168                                 0.7);
 169                         robot_trajectory_add_final_point_trans(
 170                                 0.64 + 0.08,
 171                                 ROBOT_AXIS_TO_FRONT_M + 0.05,
 172                                 ARRIVE_FROM(DEG2RAD(270), 0.10));
 173                         break;
 174                 case EV_MOTION_DONE:
 175                         FSM_TRANSITION(reach_central_buillon);
 176                         break;
 177                 default:
 178                         break;
 179         }
 180 }
 181
 182 FSM_STATE(reach_central_buillon)
 183 {
 184         switch(FSM_EVENT) {
 185                 case EV_ENTRY:
 186                         robot_trajectory_new(&tcSlow); // new trajectory
 187                         robot_trajectory_add_point_trans(
 188                                 0.64,
 189                                 0.7);
 190                         robot_trajectory_add_point_trans(
 191                                 1.0,
 192                                 0.45);
 193                         robot_trajectory_add_final_point_trans(
 194                                 1.3,
 195                                 0.58,
 196                                 NO_TURN());
 197                         break;
 198                 case EV_MOTION_DONE:
 199                         FSM_TRANSITION(leave_central_buillon);
 200                         break;
 201                 default:
 202                         break;
 203         }
 204 }
 205
 206 FSM_STATE(leave_central_buillon)
 207 {
 208         switch(FSM_EVENT) {
 209                 case EV_ENTRY:
 210                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 211                         robot_trajectory_add_final_point_trans(
 212                                 1.0,
 213                                 0.45,
 214                                 NO_TURN());
 215                         break;
 216                 case EV_MOTION_DONE:
 217                         FSM_TRANSITION(push_bottle_bw);
 218                         break;
 219                 default:
 220                         break;
 221         }
 222 }
 223
 224 FSM_STATE(push_bottle_bw)
 225 {
 226         switch(FSM_EVENT) {
 227                 case EV_ENTRY:
 228                         robot_trajectory_new_backward(&tcSlow); // new trajectory
 229                         robot_trajectory_add_point_trans(
 230                                 0.7,
 231                                 0.3);
 232                         robot_trajectory_add_final_point_trans(
 233                                 0.64+0.08,
 234                                 ROBOT_AXIS_TO_BACK_M + 0.05,
 235                                 ARRIVE_FROM(DEG2RAD(270), 0.10));
 236                         break;
 237                 case EV_MOTION_DONE:
 238                         FSM_TRANSITION(return_home);
 239                         break;
 240                 default:
 241                         break;
 242         }
 243 }
 244
 245 FSM_STATE(return_home)
 246 {
 247         switch(FSM_EVENT) {
 248                 case EV_ENTRY:
 249                         robot_trajectory_new(&tcSlow); // new trajectory
 250                         robot_trajectory_add_point_trans(
 251                                 0.64 + 0.08,
 252                                 0.7);
 253                         robot_trajectory_add_final_point_trans(
 254                                 0.4,
 255                                 1.0,
 256                                 ARRIVE_FROM(DEG2RAD(180), 0.10));
 257                         break;
 258                 case EV_MOTION_DONE:
 259                         break;
 260                 default:
 261                         break;
 262         }
 263 }
 264 /************************************************************************
 265  * MAIN FUNCTION
 266  ************************************************************************/
 267
 268 int main()
 269 {
 270         int rv;
 271
 272         rv = robot_init();
 273         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 274
 275         robot.obstacle_avoidance_enabled = true;
 276
 277         robot.fsm.main.debug_states = 1;
 278         robot.fsm.motion.debug_states = 1;
 279         //robot.fsm.act.debug_states = 1;
 280
 281         robot.fsm.main.state = &fsm_state_main_init;
 282         //robot.fsm.main.transition_callback = trans_callback;
 283         //robot.fsm.motion.transition_callback = move_trans_callback;
 284
 285         rv = robot_start();
 286         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 287
 288         robot_destroy();
 289
 290         return 0;
 291 }