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