src/robofsm/robot_orte.c

   1 /*
   2  * robot_orte.c                 08/04/21
   3  *
   4  * Robot's orte stuffs.
   5  *
   6  * Copyright: (c) 2008 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 #include <orte.h>
  12 #include <roboorte_robottype.h>
  13 #include <robodata.h>
  14 #include <robot.h>
  15 #include <movehelper.h>
  16 #include <math.h>
  17 #include <robomath.h>
  18 #include <laser-nav.h>
  19 #include "map_handling.h"
  20 #include <oledlib.h>
  21 #include <string.h>
  22 #include <can_msg_def.h>
  23
  24 #ifdef ORTE_DEBUG
  25    #define DBG(format, ...) printf(format, ##__VA_ARGS__)
  26 #else
  27    #define DBG(format, ...)
  28 #endif
  29
  30 /*****FIXME:*****/
  31 extern sem_t measurement_received;
  32
  33 /* ----------------------------------------------------------------------
  34  * PUBLISHER CALLBACKS - GENERIC
  35  * ---------------------------------------------------------------------- */
  36
  37 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
  38                         void *sendCallBackParam)
  39 {
  40         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  41
  42         ROBOT_LOCK(ref_pos);
  43         *instance = robot.ref_pos;
  44         ROBOT_UNLOCK(ref_pos);
  45 }
  46
  47 void send_est_pos_uzv_cb(const ORTESendInfo *info, void *vinstance,
  48                         void *sendCallBackParam)
  49 {
  50         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  51
  52         ROBOT_LOCK(est_pos_uzv);
  53         *instance = robot.est_pos_uzv;
  54         ROBOT_UNLOCK(est_pos_uzv);
  55 }
  56
  57 void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
  58                         void *sendCallBackParam)
  59 {
  60         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  61
  62         ROBOT_LOCK(est_pos_odo);
  63         *instance = robot.est_pos_odo;
  64         ROBOT_UNLOCK(est_pos_odo);
  65 }
  66
  67 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
  68                         void *sendCallBackParam)
  69 {
  70 }
  71
  72 /* ----------------------------------------------------------------------
  73  * SUBSCRIBER CALLBACKS - GENERIC
  74  * ---------------------------------------------------------------------- */
  75
  76 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
  77                         void *recvCallBackParam)
  78 {
  79         struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
  80         double dleft, dright, dtang, dphi;
  81         static bool first_run = true;
  82         /* spocitat prevodovy pomer */
  83         const double n = (double)(28.0 / 1.0);
  84
  85         /* vzdalenost na pulz IRC */
  86         const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
  87
  88         switch (info->status) {
  89                 case NEW_DATA:
  90                         if (first_run) {
  91                                 ROBOT_LOCK(motion_irc);
  92                                 robot.motion_irc = *instance;
  93                                 ROBOT_UNLOCK(motion_irc);
  94                                 first_run = false;
  95                                 break;
  96                         }
  97
  98                         dleft = ((robot.motion_irc.left - instance->left) >> 8) * c;
  99                         dright = ((instance->right - robot.motion_irc.right) >> 8) * c;
 100
 101                         dtang = (dleft + dright) / 2.0;
 102                         dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
 103
 104                         ROBOT_LOCK(est_pos_odo);
 105                         double a = robot.est_pos_odo.phi;
 106                         robot.est_pos_odo.x += dtang*cos(a);
 107                         robot.est_pos_odo.y += dtang*sin(a);
 108                         robot.est_pos_odo.phi += dphi;
 109                         ROBOT_UNLOCK(est_pos_odo);
 110
 111                         /* locking should not be needed, but... */
 112                         ROBOT_LOCK(motion_irc);
 113                         robot.motion_irc = *instance;
 114                         robot.motion_irc_received = 1;
 115                         ROBOT_UNLOCK(motion_irc);
 116
 117                         robot.odometry_works = true;
 118
 119                         robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
 120                         break;
 121                 case DEADLINE:
 122                         robot.odometry_works = false;
 123                         robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
 124                         DBG("ORTE deadline occurred - motion_irc receive\n");
 125                         break;
 126         }
 127 }
 128
 129 void rcv_corr_distances_cb(const ORTERecvInfo *info, void *vinstance,
 130                            void *recvCallBackParam)
 131 {
 132         struct corr_distances_type *instance =
 133                 (struct corr_distances_type *)vinstance;
 134
 135         switch (info->status) {
 136                 case NEW_DATA:
 137                         /* locking should not be needed, but... */
 138                         ROBOT_LOCK(corr_distances);
 139                         robot.corr_distances = *instance;
 140                         ROBOT_UNLOCK(corr_distances);
 141
 142                         /* wake up motion-control/thread_trajectory_follower */
 143                         sem_post(&measurement_received);
 144
 145                         /*FIXME: is following OK? (pecam1) */
 146                         robot.hw_status[STATUS_UZV] = HW_STATUS_OK;
 147                         break;
 148                 case DEADLINE:
 149                         robot.hw_status[STATUS_UZV] = HW_STATUS_FAILED;
 150
 151                         break;
 152         }
 153 }
 154
 155 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 156                         void *recvCallBackParam)
 157 {
 158         switch (info->status) {
 159                 case NEW_DATA:
 160                         break;
 161                 case DEADLINE:
 162                         DBG("ORTE deadline occurred - motion_speed receive\n");
 163                         break;
 164         }
 165 }
 166
 167 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 168                         void *recvCallBackParam)
 169 {
 170         switch (info->status) {
 171                 case NEW_DATA:
 172                         break;
 173                 case DEADLINE:
 174                         DBG("ORTE deadline occurred - motion_status receive\n");
 175                         break;
 176         }
 177 }
 178
 179 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 180                         void *recvCallBackParam)
 181 {
 182         switch (info->status) {
 183                 case NEW_DATA:
 184                         robot.hw_status[STATUS_PWR]=HW_STATUS_OK;
 185                         break;
 186                 case DEADLINE:
 187                         robot.hw_status[STATUS_PWR]=HW_STATUS_FAILED;
 188                         DBG("ORTE deadline occurred - pwr_voltage receive\n");
 189                         break;
 190         }
 191 }
 192
 193 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
 194                         void *recvCallBackParam)
 195 {
 196         switch (info->status) {
 197                 case NEW_DATA:
 198                         break;
 199                 case DEADLINE:
 200                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 201                         break;
 202         }
 203 }
 204
 205 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 206                         void *recvCallBackParam)
 207 {
 208         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 209         static struct robot_cmd_type last_instance;
 210
 211         switch (info->status) {
 212                 case NEW_DATA:
 213                         /* Stupid way of controlling the robot by
 214                          * pluggin in and out start connector. */
 215                         switch (robot.state) {
 216                                 case POWER_ON:
 217                                         if (instance->start) {
 218                                                 robot.hw_status[STATUS_STA] = HW_STATUS_WARNING;
 219                                         } else {
 220                                                 robot.hw_status[STATUS_STA] = HW_STATUS_OK;
 221                                         }
 222                                         if (!instance->start) {
 223                                                 robot.state = START_PLUGGED_IN;
 224                                                 printf("START_PLUGGED_IN\n");
 225                                         }
 226                                         break;
 227
 228                                 case START_PLUGGED_IN:
 229                                         robot.hw_status[STATUS_STA] = HW_STATUS_OK;
 230                                         /* Competition starts when plugged out */
 231                                         if (instance->start) {
 232                                                 FSM_SIGNAL(MAIN, EV_START, NULL);
 233                                                 robot.state = COMPETITION_STARTED;
 234                                                 printf("STARTED\n");
 235                                         }
 236                                         break;
 237
 238                                 case COMPETITION_STARTED: {
 239                                         /* Subsequent plug-in stops the robot */
 240                                         static int num = 0;
 241                                         if (!instance->start) {
 242                                                 robot.hw_status[STATUS_STA] = HW_STATUS_WARNING;
 243                                                 if (++num == 10)
 244                                                         robot_exit();
 245                                         }
 246                                         break;
 247                                 }
 248                         }
 249                         last_instance = *instance;
 250                         break;
 251                 case DEADLINE:
 252                         robot.hw_status[STATUS_STA] = HW_STATUS_FAILED;
 253                         DBG("ORTE deadline occurred - robot_cmd receive\n");
 254                         break;
 255         }
 256 }
 257
 258 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 259                         void *recvCallBackParam)
 260 {
 261         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 262
 263         switch (info->status) {
 264                 case NEW_DATA: {
 265                         ROBOT_LOCK(hokuyo);
 266                         robot.hokuyo = *instance;
 267                         robot.hw_status[STATUS_HOK] = HW_STATUS_OK;
 268                         ROBOT_UNLOCK(hokuyo);
 269                         update_map_hokuyo(instance);
 270                         break;
 271                 }
 272                 case DEADLINE:
 273                         robot.hw_status[STATUS_HOK] = HW_STATUS_FAILED;
 274                         //system("killall -9 hokuyo");
 275                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 276                         break;
 277         }
 278 }
 279
 280 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 281                         void *recvCallBackParam)
 282 {
 283         struct camera_result_type *instance = (struct camera_result_type *)vinstance;
 284
 285         switch (info->status) {
 286                 case NEW_DATA: {
 287                         ROBOT_LOCK(camera_result);
 288                         robot.game_conf = instance->lot;
 289                         ROBOT_UNLOCK(camera_result);
 290                         robot.hw_status[STATUS_CAM] = HW_STATUS_OK;
 291                         break;
 292                 }
 293                 case DEADLINE:
 294                         if (robot.orte.camera_control.on) {
 295                                 robot.hw_status[STATUS_CAM] = HW_STATUS_FAILED;
 296                                 //system("killall -9 rozpuk");
 297                         }
 298
 299                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 300                         break;
 301         }
 302 }
 303 /* ----------------------------------------------------------------------
 304  * SUBSCRIBER CALLBACKS - EB2008
 305  * ---------------------------------------------------------------------- */
 306
 307 void rcv_actuator_status_cb(const ORTERecvInfo *info, void *vinstance,
 308                             void *recvCallBackParam)
 309 {
 310         static unsigned short old_lift_pos=0;
 311         static unsigned short old_pusher_pos=0;
 312         switch (info->status) {
 313                 case NEW_DATA:
 314                         // new data arrived and requested position equals actual position
 315                         ROBOT_LOCK(lift_cmd);
 316                         if (robot.orte.actuator_status.lift_pos == robot.orte.lift.pos &&
 317                             robot.lift_cmd > 0) {
 318                                 FSM_SIGNAL(ACT, EV_LIFT_IN_POS, NULL);
 319                                 robot.lift_cmd--;
 320                         }
 321                         ROBOT_UNLOCK(lift_cmd);
 322                         if (robot.orte.actuator_status.pusher_pos == robot.orte.pusher.pos &&
 323                                                 robot.orte.actuator_status.pusher_pos != old_pusher_pos)
 324                                 FSM_SIGNAL(ACT, EV_PUSHER_IN_POS, NULL);
 325                         if (robot.orte.actuator_status.status & ~LIFT_ENDSW_MASK) {
 326                                 robot.hw_status[STATUS_LFT]=HW_STATUS_WARNING;
 327                         } else {
 328                                 robot.hw_status[STATUS_LFT]=HW_STATUS_OK;
 329                         }
 330                         old_lift_pos   = robot.orte.actuator_status.lift_pos;
 331                         old_pusher_pos = robot.orte.actuator_status.pusher_pos;
 332                         break;
 333                 case DEADLINE:
 334                         robot.hw_status[STATUS_LFT]=HW_STATUS_FAILED;
 335                         DBG("ORTE deadline occurred - actuator_status receive\n");
 336                         break;
 337         }
 338 }
 339
 340 void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
 341                             void *recvCallBackParam)
 342 {
 343         switch (info->status) {
 344         case NEW_DATA: {
 345                         double old_distance_value = robot.puck_distance;
 346
 347                         ROBOT_LOCK(sharps);
 348                         robot.puck_distance = robot.orte.puck_distance.distance;
 349                         ROBOT_UNLOCK(sharps);
 350
 351                         if (PUCK_INSIDE(robot.orte.puck_distance.distance) && !PUCK_INSIDE(old_distance_value)) {
 352                                 // signal puck is inside
 353                                 FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
 354                         }
 355                         if (PUCK_REACHABLE(robot.orte.puck_distance.distance) && !PUCK_REACHABLE(old_distance_value)) {
 356                                 // signal puck is reachable
 357                                 FSM_SIGNAL(MAIN, EV_PUCK_REACHABLE, NULL);
 358                         }
 359                 }
 360                 break;
 361         case DEADLINE:
 362                 // FIXME F.J. (?) inform display in case of invalid data?
 363                 // FIXME F.J. (?) limited space on the display...
 364                 //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
 365                 DBG("ORTE deadline occurred - servos receive\n");
 366                 break;
 367         }
 368 }
 369
 370 void rcv_puck_inside_cb(const ORTERecvInfo *info, void *vinstance,
 371                             void *recvCallBackParam)
 372 {
 373         switch (info->status) {
 374         case NEW_DATA: {
 375                 if (robot.use_back_switch) {
 376                         static bool previous_switch_status = 0;
 377                         if (robot.orte.puck_inside.status & 0x02) {
 378                                 //FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
 379
 380                                 /* React on the first switch or when moving backward. */
 381                                 if ((previous_switch_status & 0x02) == 0 ||
 382                                     robot.moves_backward) {
 383                                         FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
 384                                 }
 385                                 previous_switch_status = robot.orte.puck_inside.status;
 386                         }
 387                 }
 388                 {
 389                         static bool previous = false;
 390                         bool switched = ((robot.orte.puck_inside.status & 0x01) != 0);
 391
 392                         if (switched && !previous) {
 393                                 FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
 394                         }
 395                         previous = switched;
 396
 397                 }
 398                 robot.hw_status[STATUS_CHE]=HW_STATUS_OK;
 399         }
 400                 break;
 401         case DEADLINE:
 402                 robot.hw_status[STATUS_CHE]=HW_STATUS_FAILED;
 403                 DBG("ORTE deadline occurred - servos receive\n");
 404                 break;
 405         }
 406 }
 407
 408 #define HIST_CNT 5
 409 #if 0
 410 static int cmp_double(const void *v1, const void *v2)
 411 {
 412         const double *d1 = v1, *const d2 = v2;
 413         if (d1 < d2)
 414                 return -1;
 415         else if (d1 > d2)
 416                 return +1;
 417         else
 418                 return 0;
 419 }
 420 #endif
 421
 422 int robot_init_orte()
 423 {
 424         int rv = 0;
 425
 426         robot.orte.strength = 20;
 427
 428         rv = robottype_roboorte_init(&robot.orte);
 429         if (rv)
 430                 return rv;
 431
 432         /* creating publishers */
 433         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 434         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 435         robottype_publisher_est_pos_uzv_create(&robot.orte, send_est_pos_uzv_cb, &robot.orte);
 436         robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
 437         //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 438
 439         // I didn't know what was the difference between the callback function pointer
 440         // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
 441         // IS CRUCIAL!
 442         //   - NULL: message is published only when OrtePublicationSend called
 443         //   - pointer to empty function: message is published periodically
 444         robottype_publisher_lift_create(&robot.orte, send_dummy_cb, &robot.orte);
 445         robottype_publisher_pusher_create(&robot.orte, send_dummy_cb, &robot.orte);
 446         robottype_publisher_chelae_create(&robot.orte, send_dummy_cb, &robot.orte);
 447         robottype_publisher_belts_create(&robot.orte, send_dummy_cb, &robot.orte);
 448         robottype_publisher_holder_create(&robot.orte, send_dummy_cb, &robot.orte);
 449         robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
 450         robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
 451         robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
 452         robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
 453
 454         /* create generic subscribers */
 455         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 456         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 457         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 458         robottype_subscriber_corr_distances_create(&robot.orte, rcv_corr_distances_cb, &robot.orte);
 459         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 460         //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 461         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 462         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 463
 464         /* create subscribers */
 465         robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte);
 466         //robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
 467         robottype_subscriber_puck_inside_create(&robot.orte, rcv_puck_inside_cb, &robot.orte);
 468         robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 469
 470         return rv;
 471 }
 472