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