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