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