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