src/robofsm/robot_orte.c

   1 /**
   2  * @file robot_orte.c
   3  * @date 2008-2010
   4  *
   5  * @brief Module providing communication through ORTE
   6  */
   7
   8 /*
   9  * robot_orte.c                 08/04/21
  10  *
  11  * Robot's orte stuffs.
  12  *
  13  * Copyright: (c) 2008-2010 CTU Dragons
  14  *            CTU FEE - Department of Control Engineering
  15  * License: GNU GPL v.2
  16  */
  17
  18 #include <orte.h>
  19 #include <roboorte_robottype.h>
  20 #include "robodata.h"
  21 #include <robot.h>
  22 #include <movehelper.h>
  23 #include <math.h>
  24 #include <robomath.h>
  25 #include "map_handling.h"
  26 #include "match-timing.h"
  27 #include <string.h>
  28 #include <can_msg_def.h>
  29 #include <actuators.h>
  30 #include <ul_log.h>
  31
  32 UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
  33
  34 #ifdef ORTE_DEBUG
  35    #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
  36 #else
  37    #define DBG(format, ...)
  38 #endif
  39
  40 /*****FIXME:*****/
  41 extern sem_t measurement_received;
  42
  43 /* ----------------------------------------------------------------------
  44  * PUBLISHER CALLBACKS - GENERIC
  45  * ---------------------------------------------------------------------- */
  46
  47 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
  48                         void *sendCallBackParam)
  49 {
  50         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  51
  52         ROBOT_LOCK(ref_pos);
  53         *instance = robot.ref_pos;
  54         ROBOT_UNLOCK(ref_pos);
  55 }
  56
  57 void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
  58                         void *sendCallBackParam)
  59 {
  60         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  61
  62         ROBOT_LOCK(est_pos_odo);
  63         *instance = robot.est_pos_odo;
  64         ROBOT_UNLOCK(est_pos_odo);
  65 }
  66
  67 void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
  68                         void *sendCallBackParam)
  69 {
  70         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  71
  72         ROBOT_LOCK(est_pos_indep_odo);
  73         *instance = robot.est_pos_indep_odo;
  74         ROBOT_UNLOCK(est_pos_indep_odo);
  75 }
  76
  77 static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
  78                         void *sendCallBackParam)
  79 {
  80         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  81
  82         robot_get_est_pos(&instance->x, &instance->y, &instance->phi);
  83 }
  84
  85 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
  86                         void *sendCallBackParam)
  87 {
  88 }
  89
  90 void send_match_time_cb(const ORTESendInfo *info, void *vinstance,
  91                         void *sendCallBackParam)
  92 {
  93         struct match_time_type *instance = (struct match_time_type *)vinstance;
  94
  95         if (robot.start_state == POWER_ON || robot.start_state == START_PLUGGED_IN) {
  96                 instance->time = 90;
  97         } else {
  98                 instance->time = 90 - robot_current_time();
  99         }
 100 }
 101 /* ----------------------------------------------------------------------
 102  * SUBSCRIBER CALLBACKS - GENERIC
 103  * ---------------------------------------------------------------------- */
 104 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
 105                         void *recvCallBackParam)
 106 {
 107         struct odo_data_type *instance = (struct odo_data_type *)vinstance;
 108         double dleft, dright, dtang, dphi;
 109         static bool first_run = true;
 110         /* spocitat prevodovy pomer */
 111         const double n = (double)(1.0 / 1.0);
 112
 113         /* vzdalenost na pulz IRC */
 114         const double c = (M_PI*2*ODOMETRY_WHEEL_RADIUS_M) / (n * 4*4096.0);
 115
 116         switch (info->status) {
 117                 case NEW_DATA:
 118                         if (first_run) {
 119                                 ROBOT_LOCK(odo_data);
 120                                 robot.odo_data = *instance;
 121                                 ROBOT_UNLOCK(odo_data);
 122                                 first_run = false;
 123                                 break;
 124                         }
 125
 126                         dleft = ((robot.odo_data.left - instance->left) >> 8) * c * robot.odo_cal_a;    // TODO >> 8 ?
 127                         dright = ((instance->right - robot.odo_data.right) >> 8) * c * robot.odo_cal_b;
 128
 129                         dtang = (dleft + dright) / 2.0;
 130                         dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
 131
 132                         ROBOT_LOCK(est_pos_indep_odo);
 133                         robot.odo_distance_a +=dleft;
 134                         robot.odo_distance_b +=dright;
 135                         double a = robot.est_pos_indep_odo.phi;
 136                         robot.est_pos_indep_odo.x += dtang*cos(a);
 137                         robot.est_pos_indep_odo.y += dtang*sin(a);
 138                         robot.est_pos_indep_odo.phi += dphi;
 139                         ROBOT_UNLOCK(est_pos_indep_odo);
 140
 141                         ROBOT_LOCK(odo_data);
 142                         robot.odo_data = *instance;
 143                         ROBOT_UNLOCK(odo_data);
 144
 145                         robot.indep_odometry_works = true;
 146
 147                         /* wake up motion-control/thread_trajectory_follower */
 148                         sem_post(&measurement_received);
 149
 150                         //robot.hw_status[COMPONENT_ODO] = STATUS_OK;
 151                         break;
 152                 case DEADLINE:
 153                         robot.indep_odometry_works = false;
 154                         //robot.hw_status[COMPONENT_ODO] = STATUS_FAILED;
 155                         DBG("ORTE deadline occurred - odo_data receive\n");
 156                         break;
 157         }
 158 }
 159
 160 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
 161                         void *recvCallBackParam)
 162 {
 163         struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
 164         double dleft, dright, dtang, dphi;
 165         static bool first_run = true;
 166         /* spocitat prevodovy pomer */
 167         const double n = (double)(ROBOT_MOTOR_GEARBOX_RATIO / 1.0);
 168
 169         /* vzdalenost na pulz IRC */
 170         const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*ROBOT_MOTOR_IRC_RESOLUTION);
 171         switch (info->status) {
 172                 case NEW_DATA:
 173                         if (first_run) {
 174                                 ROBOT_LOCK(motion_irc);
 175                                 robot.motion_irc = *instance;
 176                                 ROBOT_UNLOCK(motion_irc);
 177                                 first_run = false;
 178                                 break;
 179                         }
 180
 181
 182                         /* FIXME maybe it is not correct to do this nasty hack here (switch dleft and dright),
 183                         what is the right solution?
 184                         This was neccessary to view motor odometry correctly in robomon. */
 185                         dright = ((robot.motion_irc.left - instance->left) >> 8) * c  * robot.odo_cal_b;
 186                         dleft = ((instance->right - robot.motion_irc.right) >> 8) * c  * robot.odo_cal_a;
 187
 188                         dtang = (dleft + dright) / 2.0;
 189                         dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
 190
 191                         ROBOT_LOCK(est_pos_odo);
 192                         robot.odo_distance_a +=dleft;
 193                         robot.odo_distance_b +=dright;
 194                         double a = robot.est_pos_odo.phi;
 195                         robot.est_pos_odo.x += dtang*cos(a);
 196                         robot.est_pos_odo.y += dtang*sin(a);
 197                         robot.est_pos_odo.phi += dphi;
 198                         ROBOT_UNLOCK(est_pos_odo);
 199
 200                         /* locking should not be needed, but... */
 201                         ROBOT_LOCK(motion_irc);
 202                         robot.motion_irc = *instance;
 203                         robot.motion_irc_received = 1;
 204                         ROBOT_UNLOCK(motion_irc);
 205
 206                         robot.odometry_works = true;
 207
 208                         robot.status[COMPONENT_MOTOR] = STATUS_OK;
 209                         break;
 210                 case DEADLINE:
 211                         robot.odometry_works = false;
 212                         robot.status[COMPONENT_MOTOR] = STATUS_FAILED;
 213                         DBG("ORTE deadline occurred - motion_irc receive\n");
 214                         break;
 215         }
 216 }
 217
 218 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 219                         void *recvCallBackParam)
 220 {
 221         switch (info->status) {
 222                 case NEW_DATA:
 223                         break;
 224                 case DEADLINE:
 225                         DBG("ORTE deadline occurred - motion_speed receive\n");
 226                         break;
 227         }
 228 }
 229
 230 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 231                         void *recvCallBackParam)
 232 {
 233         switch (info->status) {
 234                 case NEW_DATA:
 235                         break;
 236                 case DEADLINE:
 237                         DBG("ORTE deadline occurred - motion_status receive\n");
 238                         break;
 239         }
 240 }
 241
 242 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 243                         void *recvCallBackParam)
 244 {
 245         switch (info->status) {
 246                 case NEW_DATA:
 247                         robot.status[COMPONENT_POWER]=STATUS_OK;
 248                         break;
 249                 case DEADLINE:
 250                         robot.status[COMPONENT_POWER]=STATUS_FAILED;
 251                         DBG("ORTE deadline occurred - pwr_voltage receive\n");
 252                         break;
 253         }
 254 }
 255
 256 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
 257                         void *recvCallBackParam)
 258 {
 259         switch (info->status) {
 260                 case NEW_DATA:
 261                         break;
 262                 case DEADLINE:
 263                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 264                         break;
 265         }
 266 }
 267
 268 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 269                         void *recvCallBackParam)
 270 {
 271         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 272         static struct robot_cmd_type last_instance;
 273
 274         switch (info->status) {
 275                 case NEW_DATA:
 276                         /* Stupid way of controlling the robot by
 277                          * pluggin in and out start connector. */
 278                         switch (robot.start_state) {
 279                                 case POWER_ON:
 280                                         if (!instance->start_condition) {
 281                                                 robot.status[COMPONENT_START] = STATUS_WARNING;
 282                                                 robot.start_state = START_PLUGGED_IN;
 283                                                 ul_logmsg("START_PLUGGED_IN\n");
 284                                         }
 285                                         break;
 286
 287                                 case START_PLUGGED_IN:
 288                                         robot.status[COMPONENT_START] = STATUS_OK;
 289                                         /* Competition starts when plugged out */
 290                                         if (instance->start_condition) {
 291                                                 FSM_SIGNAL(MAIN, EV_START, NULL);
 292                                                 robot.start_state = COMPETITION_STARTED;
 293                                                 ul_logmsg("STARTED\n");
 294                                         }
 295                                         break;
 296
 297                                 case COMPETITION_STARTED: {
 298                                         /* Subsequent plug-in stops the robot */
 299                                         static int num = 0;
 300                                         if (!instance->start_condition) {
 301                                                 robot.status[COMPONENT_START] = STATUS_WARNING;
 302                                                 if (++num == 10)
 303                                                         robot_exit();
 304                                         }
 305                                         break;
 306                                 }
 307                         }
 308                         last_instance = *instance;
 309                         break;
 310                 case DEADLINE:
 311                         robot.status[COMPONENT_START] = STATUS_FAILED;
 312                         DBG("ORTE deadline occurred - robot_cmd receive\n");
 313                         break;
 314         }
 315 }
 316
 317 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 318                         void *recvCallBackParam)
 319 {
 320         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 321
 322         switch (info->status) {
 323                 case NEW_DATA: {
 324                         ROBOT_LOCK(hokuyo);
 325                         robot.hokuyo = *instance;
 326                         robot.status[COMPONENT_HOKUYO] = STATUS_OK;
 327                         ROBOT_UNLOCK(hokuyo);
 328                         if(!robot.ignore_hokuyo)
 329                         {
 330                                 update_map_hokuyo(instance);
 331                         }
 332                         break;
 333                 }
 334                 case DEADLINE:
 335                         robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
 336                         //system("killall -9 hokuyo");
 337                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 338                         break;
 339         }
 340 }
 341
 342 // FIXME: this is not up to date (Filip, 2010)
 343 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 344                         void *recvCallBackParam)
 345 {
 346         struct camera_result_type *instance = (struct camera_result_type *)vinstance;
 347
 348         switch (info->status) {
 349                 case NEW_DATA: {
 350                         ROBOT_LOCK(camera_result);
 351                         robot.corns_conf_side = instance->side;
 352                         robot.corns_conf_center = instance->center;
 353                         ROBOT_UNLOCK(camera_result);
 354                         robot.status[COMPONENT_CAMERA] = STATUS_OK;
 355                         break;
 356                 }
 357                 case DEADLINE:
 358                         if (robot.orte.camera_control.on) {
 359                                 robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
 360                                 //system("killall -9 rozpuk");
 361                         }
 362
 363                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 364                         break;
 365         }
 366 }
 367 /* ----------------------------------------------------------------------
 368  * SUBSCRIBER CALLBACKS - EB2008
 369  * ---------------------------------------------------------------------- */
 370
 371 void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance,
 372                             void *recvCallBackParam)
 373 {
 374         struct jaws_status_type *instance = (struct jaws_status_type *)vinstance;
 375         static int last_response_left = 0;
 376         static int last_response_right = 0;
 377         switch (info->status) {
 378                 case NEW_DATA:
 379                         // new data arrived and requested position equals actual position
 380                         if (instance->flags.left == 0 &&
 381                             instance->flags.right == 0)
 382                                 robot.status[COMPONENT_JAWS] = STATUS_OK;
 383                         else
 384                                 robot.status[COMPONENT_JAWS] = STATUS_WARNING;
 385
 386                         if (instance->response.left != last_response_left &&
 387                             instance->response.right != last_response_right &&
 388                             instance->response.left == act_jaw_left_get_last_reqest() &&
 389                             instance->response.left == act_jaw_right_get_last_reqest())
 390                                 FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL);
 391
 392                         last_response_left = instance->response.left;
 393                         last_response_right = instance->response.right;
 394                         break;
 395                 case DEADLINE:
 396                         robot.status[COMPONENT_JAWS] = STATUS_FAILED;
 397                         DBG("ORTE deadline occurred - actuator_status receive\n");
 398                         break;
 399         }
 400 }
 401
 402 void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance,
 403                             void *recvCallBackParam)
 404 {
 405         struct lift_status_type *instance = (struct lift_status_type *)vinstance;
 406         static int last_response = 0;
 407         switch (info->status) {
 408                 case NEW_DATA:
 409                         // new data arrived and requested position equals actual position
 410                         if (instance->flags == 0)
 411                                 robot.status[COMPONENT_LIFT] = STATUS_OK;
 412                         else
 413                                 robot.status[COMPONENT_LIFT] = STATUS_WARNING;
 414
 415                         if (instance->response != last_response &&
 416                             instance->response == act_lift_get_last_reqest())
 417                                 FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL);
 418                         last_response = instance->response;
 419                         break;
 420                 case DEADLINE:
 421                         robot.status[COMPONENT_LIFT] = STATUS_FAILED;
 422                         DBG("ORTE deadline occurred - actuator_status receive\n");
 423                         break;
 424         }
 425 }
 426
 427 void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
 428                            void *recvCallBackParam)
 429 {
 430         struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
 431         static bool last_strategy;
 432         switch (info->status) {
 433                 case NEW_DATA:
 434                         robot.team_color = instance->team_color;
 435
 436                         if (!last_strategy && instance->strategy) {
 437                                         /* strategy switching */
 438                                         FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
 439                         }
 440                         last_strategy = instance->strategy;
 441                         break;
 442                 case DEADLINE:
 443                         break;
 444         }
 445 }
 446
 447 void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
 448                            void *recvCallBackParam)
 449 {
 450         struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance;
 451         static bool last_left, last_right;
 452         switch (info->status) {
 453                 case NEW_DATA:
 454                         if (instance->bumper_right_across || instance->bumper_left_across || instance->bumper_rear_left || instance->bumper_rear_right)
 455                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
 456
 457                         if (instance->bumper_left || instance->bumper_right) {
 458                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
 459                         }
 460                         break;
 461                 case DEADLINE:
 462                         break;
 463         }
 464 }
 465
 466 #define HIST_CNT 5
 467 #if 0
 468 static int cmp_double(const void *v1, const void *v2)
 469 {
 470         const double *d1 = v1, *const d2 = v2;
 471         if (d1 < d2)
 472                 return -1;
 473         else if (d1 > d2)
 474                 return +1;
 475         else
 476                 return 0;
 477 }
 478 #endif
 479
 480 int robot_init_orte()
 481 {
 482         int rv = 0;
 483
 484         robot.orte.strength = 20;
 485
 486         rv = robottype_roboorte_init(&robot.orte);
 487         if (rv)
 488                 return rv;
 489
 490         /* creating publishers */
 491         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 492         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 493         robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
 494         robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte);
 495         robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte);
 496         robottype_publisher_match_time_create(&robot.orte, send_match_time_cb, &robot.orte);
 497         //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 498
 499         // I didn't know what was the difference between the callback function pointer
 500         // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
 501         // IS CRUCIAL!
 502         //   - NULL: message is published only when OrtePublicationSend called
 503         //   - pointer to empty function: message is published periodically
 504         robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
 505         robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
 506         robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
 507         robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
 508         robottype_publisher_jaws_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
 509         robottype_publisher_lift_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
 510
 511         /* create generic subscribers */
 512         robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
 513         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 514         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 515         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 516         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 517         //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 518         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 519         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 520         robottype_subscriber_jaws_status_create(&robot.orte, rcv_jaws_status_cb, &robot.orte);
 521         robottype_subscriber_lift_status_create(&robot.orte, rcv_lift_status_cb, &robot.orte);
 522         robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
 523         robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte);
 524         robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 525
 526         return rv;
 527 }
 528