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(est_pos_odo);
 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(est_pos_odo);
 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.odometry_works = true;
 119
 120                         robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
 121                         break;
 122                 case DEADLINE:
 123                         robot.odometry_works = false;
 124                         robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
 125                         DBG("ORTE deadline occurred - motion_irc receive\n");
 126                         break;
 127         }
 128 }
 129
 130 void rcv_corr_distances_cb(const ORTERecvInfo *info, void *vinstance,
 131                            void *recvCallBackParam)
 132 {
 133         struct corr_distances_type *instance =
 134                 (struct corr_distances_type *)vinstance;
 135
 136         switch (info->status) {
 137                 case NEW_DATA:
 138                         /* locking should not be needed, but... */
 139                         ROBOT_LOCK(corr_distances);
 140                         robot.corr_distances = *instance;
 141                         ROBOT_UNLOCK(corr_distances);
 142
 143                         /* wake up motion-control/thread_trajectory_follower */
 144                         sem_post(&measurement_received);
 145
 146                         /*FIXME: is following OK? (pecam1) */
 147                         robot.hw_status[STATUS_UZV] = HW_STATUS_OK;
 148                         break;
 149                 case DEADLINE:
 150                         robot.hw_status[STATUS_UZV] = HW_STATUS_FAILED;
 151
 152                         break;
 153         }
 154 }
 155
 156 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 157                         void *recvCallBackParam)
 158 {
 159         switch (info->status) {
 160                 case NEW_DATA:
 161                         break;
 162                 case DEADLINE:
 163                         DBG("ORTE deadline occurred - motion_speed receive\n");
 164                         break;
 165         }
 166 }
 167
 168 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 169                         void *recvCallBackParam)
 170 {
 171         switch (info->status) {
 172                 case NEW_DATA:
 173                         break;
 174                 case DEADLINE:
 175                         DBG("ORTE deadline occurred - motion_status receive\n");
 176                         break;
 177         }
 178 }
 179
 180 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 181                         void *recvCallBackParam)
 182 {
 183         switch (info->status) {
 184                 case NEW_DATA:
 185                         robot.hw_status[STATUS_PWR]=HW_STATUS_OK;
 186                         break;
 187                 case DEADLINE:
 188                         robot.hw_status[STATUS_PWR]=HW_STATUS_FAILED;
 189                         DBG("ORTE deadline occurred - pwr_voltage receive\n");
 190                         break;
 191         }
 192 }
 193
 194 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
 195                         void *recvCallBackParam)
 196 {
 197         switch (info->status) {
 198                 case NEW_DATA:
 199                         break;
 200                 case DEADLINE:
 201                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 202                         break;
 203         }
 204 }
 205
 206 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 207                         void *recvCallBackParam)
 208 {
 209         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 210         static struct robot_cmd_type last_instance;
 211
 212         switch (info->status) {
 213                 case NEW_DATA:
 214                         /* Stupid way of controlling the robot by
 215                          * pluggin in and out start connector. */
 216                         switch (robot.state) {
 217                                 case POWER_ON:
 218                                         if (instance->start) {
 219                                                 robot.hw_status[STATUS_STA] = HW_STATUS_WARNING;
 220                                         } else {
 221                                                 robot.hw_status[STATUS_STA] = HW_STATUS_OK;
 222                                         }
 223                                         if (!instance->start) {
 224                                                 robot.state = START_PLUGGED_IN;
 225                                                 printf("START_PLUGGED_IN\n");
 226                                         }
 227                                         break;
 228
 229                                 case START_PLUGGED_IN:
 230                                         robot.hw_status[STATUS_STA] = HW_STATUS_OK;
 231                                         /* Competition starts when plugged out */
 232                                         if (instance->start) {
 233                                                 FSM_SIGNAL(MAIN, EV_START, NULL);
 234                                                 robot.state = COMPETITION_STARTED;
 235                                                 printf("STARTED\n");
 236                                         }
 237                                         break;
 238
 239                                 case COMPETITION_STARTED: {
 240                                         /* Subsequent plug-in stops the robot */
 241                                         static int num = 0;
 242                                         if (!instance->start) {
 243                                                 robot.hw_status[STATUS_STA] = HW_STATUS_WARNING;
 244                                                 if (++num == 10)
 245                                                         robot_exit();
 246                                         }
 247                                         break;
 248                                 }
 249                         }
 250                         last_instance = *instance;
 251                         break;
 252                 case DEADLINE:
 253                         robot.hw_status[STATUS_STA] = HW_STATUS_FAILED;
 254                         DBG("ORTE deadline occurred - robot_cmd receive\n");
 255                         break;
 256         }
 257 }
 258
 259 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 260                         void *recvCallBackParam)
 261 {
 262         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 263
 264         switch (info->status) {
 265                 case NEW_DATA: {
 266                         ROBOT_LOCK(hokuyo);
 267                         robot.hokuyo = *instance;
 268                         robot.hw_status[STATUS_HOK] = HW_STATUS_OK;
 269                         ROBOT_UNLOCK(hokuyo);
 270                         update_map_hokuyo(instance);
 271                         break;
 272                 }
 273                 case DEADLINE:
 274                         robot.hw_status[STATUS_HOK] = HW_STATUS_FAILED;
 275                         //system("killall -9 hokuyo");
 276                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 277                         break;
 278         }
 279 }
 280
 281 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 282                         void *recvCallBackParam)
 283 {
 284         struct camera_result_type *instance = (struct camera_result_type *)vinstance;
 285
 286         switch (info->status) {
 287                 case NEW_DATA: {
 288                         ROBOT_LOCK(camera_result);
 289                         robot.game_conf = instance->lot;
 290                         ROBOT_UNLOCK(camera_result);
 291                         robot.hw_status[STATUS_CAM] = HW_STATUS_OK;
 292                         break;
 293                 }
 294                 case DEADLINE:
 295                         if (robot.orte.camera_control.on) {
 296                                 robot.hw_status[STATUS_CAM] = HW_STATUS_FAILED;
 297                                 //system("killall -9 rozpuk");
 298                         }
 299
 300                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 301                         break;
 302         }
 303 }
 304 /* ----------------------------------------------------------------------
 305  * SUBSCRIBER CALLBACKS - EB2008
 306  * ---------------------------------------------------------------------- */
 307
 308 void rcv_actuator_status_cb(const ORTERecvInfo *info, void *vinstance,
 309                             void *recvCallBackParam)
 310 {
 311         static unsigned short old_lift_pos=0;
 312         static unsigned short old_pusher_pos=0;
 313         switch (info->status) {
 314                 case NEW_DATA:
 315                         // new data arrived and requested position equals actual position
 316                         ROBOT_LOCK(lift_cmd);
 317                         if (robot.orte.actuator_status.lift_pos == robot.orte.lift.pos &&
 318                             robot.lift_cmd > 0) {
 319                                 FSM_SIGNAL(ACT, EV_LIFT_IN_POS, NULL);
 320                                 robot.lift_cmd--;
 321                         }
 322                         ROBOT_UNLOCK(lift_cmd);
 323                         if (robot.orte.actuator_status.pusher_pos == robot.orte.pusher.pos &&
 324                                                 robot.orte.actuator_status.pusher_pos != old_pusher_pos)
 325                                 FSM_SIGNAL(ACT, EV_PUSHER_IN_POS, NULL);
 326                         if (robot.orte.actuator_status.status & ~LIFT_ENDSW_MASK) {
 327                                 robot.hw_status[STATUS_LFT]=HW_STATUS_WARNING;
 328                         } else {
 329                                 robot.hw_status[STATUS_LFT]=HW_STATUS_OK;
 330                         }
 331                         old_lift_pos   = robot.orte.actuator_status.lift_pos;
 332                         old_pusher_pos = robot.orte.actuator_status.pusher_pos;
 333                         break;
 334                 case DEADLINE:
 335                         robot.hw_status[STATUS_LFT]=HW_STATUS_FAILED;
 336                         DBG("ORTE deadline occurred - actuator_status receive\n");
 337                         break;
 338         }
 339 }
 340
 341 void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
 342                             void *recvCallBackParam)
 343 {
 344         switch (info->status) {
 345         case NEW_DATA: {
 346                         double old_distance_value = robot.puck_distance;
 347
 348                         ROBOT_LOCK(sharps);
 349                         robot.puck_distance = robot.orte.puck_distance.distance;
 350                         ROBOT_UNLOCK(sharps);
 351
 352                         if (PUCK_INSIDE(robot.orte.puck_distance.distance) && !PUCK_INSIDE(old_distance_value)) {
 353                                 // signal puck is inside
 354                                 FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
 355                         }
 356                         if (PUCK_REACHABLE(robot.orte.puck_distance.distance) && !PUCK_REACHABLE(old_distance_value)) {
 357                                 // signal puck is reachable
 358                                 FSM_SIGNAL(MAIN, EV_PUCK_REACHABLE, NULL);
 359                         }
 360                 }
 361                 break;
 362         case DEADLINE:
 363                 // FIXME F.J. (?) inform display in case of invalid data?
 364                 // FIXME F.J. (?) limited space on the display...
 365                 //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
 366                 DBG("ORTE deadline occurred - servos receive\n");
 367                 break;
 368         }
 369 }
 370
 371 void rcv_puck_inside_cb(const ORTERecvInfo *info, void *vinstance,
 372                             void *recvCallBackParam)
 373 {
 374         switch (info->status) {
 375         case NEW_DATA: {
 376                 if (robot.use_back_switch) {
 377                         static bool previous_switch_status = 0;
 378                         if (robot.orte.puck_inside.status & 0x02) {
 379                                 //FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
 380
 381                                 /* React on the first switch or when moving backward. */
 382                                 if ((previous_switch_status & 0x02) == 0 ||
 383                                     robot.moves_backward) {
 384                                         FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
 385                                 }
 386                                 previous_switch_status = robot.orte.puck_inside.status;
 387                         }
 388                 }
 389                 {
 390                         static bool previous = false;
 391                         bool switched = ((robot.orte.puck_inside.status & 0x01) != 0);
 392
 393                         if (switched && !previous) {
 394                                 FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
 395                         }
 396                         previous = switched;
 397
 398                 }
 399                 robot.hw_status[STATUS_CHE]=HW_STATUS_OK;
 400         }
 401                 break;
 402         case DEADLINE:
 403                 robot.hw_status[STATUS_CHE]=HW_STATUS_FAILED;
 404                 DBG("ORTE deadline occurred - servos receive\n");
 405                 break;
 406         }
 407 }
 408
 409 #define HIST_CNT 5
 410 #if 0
 411 static int cmp_double(const void *v1, const void *v2)
 412 {
 413         const double *d1 = v1, *const d2 = v2;
 414         if (d1 < d2)
 415                 return -1;
 416         else if (d1 > d2)
 417                 return +1;
 418         else
 419                 return 0;
 420 }
 421 #endif
 422
 423 int robot_init_orte()
 424 {
 425         int rv = 0;
 426
 427         robot.orte.strength = 20;
 428
 429         rv = robottype_roboorte_init(&robot.orte);
 430         if (rv)
 431                 return rv;
 432
 433         /* creating publishers */
 434         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 435         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 436         robottype_publisher_est_pos_uzv_create(&robot.orte, send_est_pos_uzv_cb, &robot.orte);
 437         robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
 438         //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 439
 440         // I didn't know what was the difference between the callback function pointer
 441         // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
 442         // IS CRUCIAL!
 443         //   - NULL: message is published only when OrtePublicationSend called
 444         //   - pointer to empty function: message is published periodically
 445         robottype_publisher_lift_create(&robot.orte, send_dummy_cb, &robot.orte);
 446         robottype_publisher_pusher_create(&robot.orte, send_dummy_cb, &robot.orte);
 447         robottype_publisher_chelae_create(&robot.orte, send_dummy_cb, &robot.orte);
 448         robottype_publisher_belts_create(&robot.orte, send_dummy_cb, &robot.orte);
 449         robottype_publisher_holder_create(&robot.orte, send_dummy_cb, &robot.orte);
 450         robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
 451         robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
 452         robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
 453         robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
 454
 455         /* create generic subscribers */
 456         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 457         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 458         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 459         robottype_subscriber_corr_distances_create(&robot.orte, rcv_corr_distances_cb, &robot.orte);
 460         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 461         //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 462         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 463         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 464
 465         /* create subscribers */
 466         robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte);
 467         //robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
 468         robottype_subscriber_puck_inside_create(&robot.orte, rcv_puck_inside_cb, &robot.orte);
 469         robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 470
 471         return rv;
 472 }
 473