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