src/robofsm/homologation.cc

   1 /*
   2  * homologation.cc       08/04/29
   3  *
   4  * Robot's control program intended for homologation (approval phase) on Eurobot 2009.
   5  *
   6  * Copyright: (c) 2009 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 #ifndef DEBUG
  12 #define DEBUG
  13 #endif
  14
  15 #define FSM_MAIN
  16 #include <robodata.h>
  17 #include <robot.h>
  18 #include <fsm.h>
  19 #include <unistd.h>
  20 #include <math.h>
  21 #include <movehelper.h>
  22 #include <map.h>
  23 #include <sharp.h>
  24 #include <robomath.h>
  25 #include <string.h>
  26 #include <robodim.h>
  27 #include <error.h>
  28 #include "corns_configs.h"
  29 #include <trgen.h>
  30
  31 /************************************************************************
  32  * Trajectory constraints used, are initialized in the init state
  33  ************************************************************************/
  34
  35 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
  36
  37 /************************************************************************
  38  * Functions to manipulate map, compute distances, choose corns etc.
  39  ************************************************************************/
  40
  41 static const Point containerPosition(PLAYGROUND_WIDTH_M - 0.25, 0.02); // blue container Position
  42
  43 // returns pointer to next real (non-fake) corn which was not yet collected
  44 // TODO: note: use this for "short_time_to_end: situation only, otherwise
  45 // it is better to try to rush more corns at once
  46 struct corn * choose_next_corn()
  47 {
  48         Point cornPosition;
  49
  50         double minDistance = 2 * PLAYGROUND_HEIGHT_M; // "infinity"
  51         struct corn *minCorn = NULL, *corn;
  52         // robot.corns->corns[NUM_OF_FAKE_CORNS] is first non-fake corn in the robot.corns structure
  53         for(corn = &robot.corns->corns[NUM_OF_FAKE_CORNS]; corn < &robot.corns->corns[robot.corns->corns_count]; corn++) {
  54                 cornPosition.x = corn->position.x;
  55                 cornPosition.y = corn->position.y;
  56                 double distance = cornPosition.distanceTo(containerPosition);
  57
  58                 if (distance < minDistance && corn->was_collected == false) {
  59                         minDistance = distance;
  60                         minCorn = corn;
  61                 }
  62         }
  63
  64         if (minCorn) printf("\tmin distance was: %.3f  ", minDistance);
  65         return minCorn;
  66 }
  67
  68 /**
  69  * Computes and returns line to point distance
  70  * @param[in] p the point coords
  71  * @param[in] lp1 coords of one of the points on the line
  72  * @param[in] lp2 coords of the second point on the line
  73  */
  74 double get_point_to_line_distance(const Point &p, const Point &lp1, const Point &lp2)
  75 {
  76         double distance = fabs((lp2.x - lp1.x)*(lp1.y-p.y) - (lp1.x - p.x)*(lp2.y - lp1.y))
  77                 / sqrt((lp2.x - lp1.x)*(lp2.x - lp1.x) + (lp2.y - lp1.y)*(lp2.y - lp1.y));
  78         return distance;
  79 }
  80
  81 Pos * get_corn_approach_position(struct corn *corn)
  82 {
  83         const Point approxContainerPosition(PLAYGROUND_WIDTH_M - 0.15, 0.02); // blue container Position
  84         Pos *p = new Pos; // robot position result
  85
  86         Point cornPosition(corn->position.x, corn->position.y);
  87         double a = approxContainerPosition.angleTo(cornPosition);
  88
  89         p->x = cornPosition.x + cos(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.1);
  90         p->y = cornPosition.y + sin(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.1);
  91         p->phi = a + M_PI;
  92
  93         return p;
  94 }
  95
  96
  97 /************************************************************************
  98  * FSM STATES DECLARATION
  99  ************************************************************************/
 100
 101 /* initial and starting states */
 102 FSM_STATE_DECL(init);
 103 FSM_STATE_DECL(wait_for_start);
 104 FSM_STATE_DECL(wait);
 105 /* movement states */
 106 FSM_STATE_DECL(climb_the_slope);
 107 FSM_STATE_DECL(sledge_down);
 108 FSM_STATE_DECL(to_container_diag);
 109 FSM_STATE_DECL(to_container_ortho);
 110 FSM_STATE_DECL(experiment);
 111
 112 /************************************************************************
 113  * INITIAL AND STARTING STATES
 114  ************************************************************************/
 115
 116 FSM_STATE(init)
 117 {
 118         switch (FSM_EVENT) {
 119                 case EV_ENTRY:
 120                         tcFast = trajectoryConstraintsDefault;
 121                         tcFast.maxv = 1;
 122                         tcFast.maxacc = 0.3;
 123                         tcFast.maxomega = 2;
 124                         tcSlow = trajectoryConstraintsDefault;
 125                         tcSlow.maxv = 0.3;
 126                         tcSlow.maxacc = 0.3;
 127                         tcVerySlow = trajectoryConstraintsDefault;
 128                         tcVerySlow.maxv = 0.05;
 129                         tcVerySlow.maxacc = 0.05;
 130                         FSM_TRANSITION(wait_for_start);
 131                         break;
 132                 default:
 133                         break;
 134         }
 135 }
 136
 137 FSM_STATE(wait_for_start)
 138 {
 139         #ifdef COMPETITON
 140                 printf("COMPETITION mode set");
 141         #endif
 142         switch (FSM_EVENT) {
 143 #ifdef WAIT_FOR_START
 144                 case EV_ENTRY:
 145                         break
 146                 case EV_START: {
 147                         pthread_t thid;
 148
 149                         /* start competition timer */
 150                         pthread_create(&thid, NULL, wait_for_end, NULL);
 151                         pthread_create(&thid, NULL, wait_to_deposition, NULL);
 152                         }
 153 #else
 154                 case EV_START:
 155                 case EV_ENTRY:
 156 #endif
 157                         //FIXME:
 158                         robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
 159                                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
 160                                                 DEG2RAD(180));
 161                         FSM_TRANSITION(climb_the_slope);
 162                         break;
 163                 case EV_RETURN:
 164                 case EV_TIMER:
 165                 case EV_GOAL_NOT_REACHABLE:
 166                 case EV_ACTION_DONE:
 167                 case EV_ACTION_ERROR:
 168                 case EV_MOTION_DONE:
 169                         DBG_PRINT_EVENT("unhandled event");
 170                         break;
 171                 case EV_EXIT:
 172                         robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center);
 173                         break;
 174         }
 175 }
 176
 177
 178 /************************************************************************
 179  * MOVEMENT STATES
 180  ************************************************************************/
 181
 182 FSM_STATE(climb_the_slope)
 183 {
 184         switch(FSM_EVENT) {
 185                 case EV_ENTRY:
 186                         robot.ignore_hokuyo = true;
 187                         robot_trajectory_new_backward(&tcSlow);
 188                         robot_trajectory_add_point_trans(
 189                                 0.5 - ROBOT_AXIS_TO_BACK_M,
 190                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
 191                         /* position for collecting oranges*/
 192                         robot_trajectory_add_final_point_trans(
 193                                 SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.04,
 194                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.02,
 195                                 NO_TURN());
 196                         break;
 197                 case EV_MOTION_DONE:
 198                         FSM_TRANSITION(wait);
 199                         break;
 200                 case EV_START:
 201                 case EV_TIMER:
 202                 case EV_RETURN:
 203                 case EV_ACTION_DONE:
 204                 case EV_ACTION_ERROR:
 205                 case EV_GOAL_NOT_REACHABLE:
 206                         DBG_PRINT_EVENT("unhandled event");
 207                 case EV_EXIT:
 208                         break;
 209         }
 210 }
 211
 212 FSM_STATE(wait)
 213 {
 214         DBG_PRINT_EVENT("Loading oranges");
 215         FSM_TIMER(5000);
 216         switch (FSM_EVENT) {
 217                 case EV_TIMER:
 218                         FSM_TRANSITION(sledge_down);
 219                         break;
 220                 default:
 221                         break;
 222         }
 223 }
 224
 225 FSM_STATE(sledge_down)
 226 {
 227         switch(FSM_EVENT) {
 228                 case EV_ENTRY:
 229                         robot_trajectory_new(&tcFast);
 230                         robot_trajectory_add_point_trans(
 231                                 1 -ROBOT_AXIS_TO_BACK_M,
 232                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.02);
 233                         robot_trajectory_add_point_trans(
 234                                 SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26,
 235                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
 236                         robot_trajectory_add_final_point_trans(0.50, PLAYGROUND_HEIGHT_M - 0.6, NO_TURN());
 237                         break;
 238                 case EV_MOTION_DONE:
 239                         FSM_TRANSITION(to_container_diag);
 240                         //FSM_TRANSITION(to_container_ortho);
 241                         break;
 242                 case EV_START:
 243                 case EV_TIMER:
 244                 case EV_RETURN:
 245                 case EV_ACTION_DONE:
 246                 case EV_ACTION_ERROR:
 247                 case EV_GOAL_NOT_REACHABLE:
 248                         DBG_PRINT_EVENT("unhandled event");
 249                 case EV_EXIT:
 250                         robot.ignore_hokuyo = false;
 251                         break;
 252         }
 253 }
 254
 255 FSM_STATE(to_container_diag)
 256 {
 257         switch(FSM_EVENT) {
 258                 case EV_ENTRY:
 259                         robot_trajectory_new(&tcFast);
 260                         // face the rim with front of the robot
 261                         //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
 262                         // face the rim with back of the robot
 263                         robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M-0.6, 0.35);
 264                         robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
 265                         break;
 266                 case EV_START:
 267                 case EV_TIMER:
 268                 case EV_RETURN:
 269                 case EV_ACTION_DONE:
 270                 case EV_ACTION_ERROR:
 271                 case EV_MOTION_DONE:
 272                 case EV_GOAL_NOT_REACHABLE:
 273                         DBG_PRINT_EVENT("unhandled event");
 274                 case EV_EXIT:
 275                         break;
 276         }
 277 }
 278
 279 FSM_STATE(to_container_ortho)
 280 {
 281         switch(FSM_EVENT) {
 282                 case EV_ENTRY:
 283                         robot_trajectory_new(&tcFast);
 284                         robot_trajectory_add_point_trans(
 285                                 SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.15,
 286                                 PLAYGROUND_HEIGHT_M - 0.355);
 287                         robot_trajectory_add_point_trans(0.55, PLAYGROUND_HEIGHT_M - 0.65);
 288                         robot_trajectory_add_point_trans(0.90, PLAYGROUND_HEIGHT_M - 0.75);
 289                         robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.60, PLAYGROUND_HEIGHT_M - 0.7);
 290                         robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.35, PLAYGROUND_HEIGHT_M - 0.9);
 291
 292                         // face the rim with front of the robot
 293                         //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
 294                         // face the rim with back of the robot
 295                         robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.40, TURN_CCW(DEG2RAD(90)));
 296                         break;
 297                 case EV_START:
 298                 case EV_TIMER:
 299                 case EV_RETURN:
 300                 case EV_ACTION_DONE:
 301                 case EV_ACTION_ERROR:
 302                 case EV_MOTION_DONE:
 303                 case EV_GOAL_NOT_REACHABLE:
 304                         DBG_PRINT_EVENT("unhandled event");
 305                 case EV_EXIT:
 306                         break;
 307         }
 308 }
 309
 310 static int cnt = 0;
 311 FSM_STATE(experiment)
 312 {
 313         switch(FSM_EVENT) {
 314                 case EV_ENTRY: {
 315                                 double x, y, phi;
 316                                 robot_get_est_pos(&x, &y, &phi);
 317                                 printf("experiment: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
 318                                         cnt, x, y, phi);
 319                                 struct corn * corn = choose_next_corn();
 320                                 Pos *p = get_corn_approach_position(corn /*&robot.corns->corns[cnt++]*/);
 321                                 corn->was_collected = true;
 322                                 //robot_trajectory_new(&tcFast);
 323                                 //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
 324                                 robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
 325                                 delete(p);
 326                                 break;
 327                         }
 328                 case EV_MOTION_DONE:
 329                         FSM_TRANSITION(experiment);
 330                         break;
 331                 case EV_START:
 332                 case EV_TIMER:
 333                 case EV_RETURN:
 334                 case EV_ACTION_DONE:
 335                 case EV_ACTION_ERROR:
 336                 case EV_GOAL_NOT_REACHABLE:
 337                         DBG_PRINT_EVENT("unhandled event");
 338                 case EV_EXIT:
 339                         break;
 340         }
 341 }
 342
 343
 344
 345 /************************************************************************
 346  * MAIN FUNCTION
 347  ************************************************************************/
 348
 349 int main()
 350 {
 351         int rv;
 352
 353         rv = robot_init();
 354         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 355
 356         robot.obstacle_avoidance_enabled = true;
 357
 358         robot.fsm.main.debug_states = 1;
 359         //robot.fsm.motion.debug_states = 1;
 360         //robot.fsm.act.debug_states = 1;
 361
 362         robot.fsm.main.state = &fsm_state_main_init;
 363         //robot.fsm.main.transition_callback = trans_callback;
 364         //robot.fsm.motion.transition_callback = move_trans_callback;
 365
 366         rv = robot_start();
 367         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 368
 369         robot_destroy();
 370
 371         return 0;
 372 }
 373
 374 /************************************************************************
 375  * STATE SKELETON
 376  ************************************************************************/
 377
 378 /*
 379 FSM_STATE()
 380 {
 381         switch(FSM_EVENT) {
 382                 case EV_ENTRY:
 383                         break;
 384                 case EV_START:
 385                 case EV_TIMER:
 386                 case EV_RETURN:
 387                 case EV_ACTION_DONE:
 388                 case EV_ACTION_ERROR:
 389                 case EV_MOTION_DONE:
 390                 case EV_GOAL_NOT_REACHABLE:
 391                         DBG_PRINT_EVENT("unhandled event");
 392                 case EV_EXIT:
 393                         break;
 394         }
 395 }
 396 */