src/robofsm/homologation2012.cc

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