src/robofsm/homologation2012.cc

   1 /*
   2  * homologation.cc       12/03/15
   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 /* Pushing the bottle */
  45 FSM_STATE_DECL(push_bottle);
  46
  47
  48 FSM_STATE(init)
  49 {
  50         switch (FSM_EVENT) {
  51                 case EV_ENTRY:
  52                         tcSlow = trajectoryConstraintsDefault;
  53                         tcSlow.maxv = 0.3;
  54                         tcSlow.maxacc = 0.3;
  55                         tcSlow.maxomega = 1;
  56                         FSM_TRANSITION(wait_for_start);
  57                         break;
  58                 default:
  59                         break;
  60         }
  61 }
  62
  63 void set_initial_position()
  64 {
  65         // TODO define initial position
  66         robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
  67                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
  68                                 0);
  69 }
  70
  71 void actuators_home()
  72 {
  73         //act_jaws(CLOSE);
  74         // drive lift to home position
  75         //act_lift(0, 0, 1);
  76         // unset the homing request
  77         //act_lift(0, 0, 0);
  78 }
  79
  80 FSM_STATE(wait_for_start)
  81 {
  82         pthread_t thid;
  83         #ifdef WAIT_FOR_START
  84                 ul_logdeb("WAIT_FOR_START mode set\n");
  85         #else
  86                 ul_logdeb("WAIT_FOR_START mode NOT set\n");
  87         #endif
  88         #ifdef COMPETITION
  89                 ul_logdeb("COMPETITION mode set\n");
  90         #else
  91                 ul_logdeb("COMPETITION mode NOT set\n");
  92         #endif
  93         switch (FSM_EVENT) {
  94                 case EV_ENTRY:
  95                         pthread_create(&thid, NULL, timing_thread, NULL);
  96 #ifdef WAIT_FOR_START
  97                         FSM_TIMER(1000);
  98                         break;
  99 #endif
 100                 case EV_START:
 101                         /* start competition timer */
 102                         sem_post(&robot.start);
 103                         actuators_home();
 104                         set_initial_position();
 105                         FSM_TRANSITION(aproach_buillon);
 106                         break;
 107                 case EV_TIMER:
 108                         FSM_TIMER(1000);
 109                         // We set initial position periodically in
 110                         // order for it to be updated on the display
 111                         // if the team color is changed during waiting
 112                         // for start.
 113                         set_initial_position();
 114                         if (robot.start_state == START_PLUGGED_IN)
 115                                 actuators_home();
 116                         break;
 117                 case EV_RETURN:
 118                 case EV_MOTION_ERROR:
 119                 case EV_MOTION_DONE:
 120                 //case EV_VIDLE_DONE:
 121                 case EV_SWITCH_STRATEGY:
 122                         DBG_PRINT_EVENT("unhandled event");
 123                         break;
 124                 case EV_EXIT:
 125                         break;
 126                 default:
 127                         break;
 128         }
 129 }
 130
 131 FSM_STATE(aproach_buillon)
 132 {
 133         switch(FSM_EVENT) {
 134                 case EV_ENTRY:
 135                         robot_trajectory_new(&tcSlow); // new trajectory
 136                         robot_trajectory_add_point_trans(
 137                                 0.65,
 138                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
 139                         robot_trajectory_add_point_trans(
 140                                 0.65,
 141                                 1.3);
 142                         robot_trajectory_add_final_point_trans(
 143                                 0.5,
 144                                 1.1,
 145                                 NO_TURN());
 146                         break;
 147                 case EV_MOTION_DONE:
 148                 case EV_TIMER:
 149                         FSM_TRANSITION(push_bottle);
 150                         break;
 151                 default:
 152                         break;
 153         }
 154 }
 155
 156 FSM_STATE(push_bottle)
 157 {
 158         switch(FSM_EVENT) {
 159                 case EV_ENTRY:
 160                         robot_trajectory_new(&tcSlow); // new trajectory
 161                         robot_trajectory_add_point_trans(
 162                                 0.64,
 163                                 0.7);
 164                         robot_trajectory_add_final_point_trans(
 165                                 0.64 + 0.08,
 166                                 ROBOT_AXIS_TO_FRONT_M + 0.05,
 167                                 ARRIVE_FROM(DEG2RAD(270), 0.10));
 168                         break;
 169                 case EV_MOTION_DONE:
 170                         break;
 171                 default:
 172                         break;
 173         }
 174 }
 175 /************************************************************************
 176  * MAIN FUNCTION
 177  ************************************************************************/
 178
 179 int main()
 180 {
 181         int rv;
 182
 183         rv = robot_init();
 184         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 185
 186         robot.obstacle_avoidance_enabled = true;
 187
 188         robot.fsm.main.debug_states = 1;
 189         robot.fsm.motion.debug_states = 1;
 190         //robot.fsm.act.debug_states = 1;
 191
 192         robot.fsm.main.state = &fsm_state_main_init;
 193         //robot.team_color = BLUE;
 194         //robot.fsm.main.transition_callback = trans_callback;
 195         //robot.fsm.motion.transition_callback = move_trans_callback;
 196
 197         rv = robot_start();
 198         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 199
 200         robot_destroy();
 201
 202         return 0;
 203 }