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