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