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                         if (instance->start) {
 177                                 robot.hw_status[STATUS_STA] = HW_STATUS_WARNING;
 178                         } else {
 179                                 robot.hw_status[STATUS_STA] = HW_STATUS_OK;
 180                         }
 181                         switch (robot.state) {
 182                                 case POWER_ON:
 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                                         /* Competition starts when plugged out */
 191                                         if (instance->start) {
 192                                                 FSM_SIGNAL(MAIN, EV_START, NULL);
 193                                                 robot.state = COMPETITION_STARTED;
 194                                                 printf("STARTED\n");
 195                                         }
 196                                         break;
 197
 198                                 case COMPETITION_STARTED:
 199                                         /* Subsequent plug-in stops the robot */
 200                                         if (!instance->start) {
 201                                                 //robot_exit();
 202                                         }
 203                                         break;
 204                         }
 205                         last_instance = *instance;
 206                         break;
 207                 case DEADLINE:
 208                         robot.hw_status[STATUS_STA] = HW_STATUS_FAILED;
 209                         DBG("ORTE deadline occurred - robot_cmd receive\n");
 210                         break;
 211         }
 212 }
 213
 214 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 215                         void *recvCallBackParam)
 216 {
 217         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 218
 219         switch (info->status) {
 220                 case NEW_DATA: {
 221                         ROBOT_LOCK(hokuyo);
 222                         robot.hokuyo = *instance;
 223                         robot.hw_status[STATUS_HOK] = HW_STATUS_OK;
 224                         ROBOT_UNLOCK(hokuyo);
 225                         update_map_hokuyo(instance);
 226                         break;
 227                 }
 228                 case DEADLINE:
 229                         robot.hw_status[STATUS_HOK] = HW_STATUS_FAILED;
 230                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 231                         break;
 232         }
 233 }
 234
 235 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 236                         void *recvCallBackParam)
 237 {
 238         struct camera_result_type *instance = (struct camera_result_type *)vinstance;
 239
 240         switch (info->status) {
 241                 case NEW_DATA: {
 242                         ROBOT_LOCK(camera_result);
 243                         robot.game_conf = instance->lot;
 244                         ROBOT_UNLOCK(camera_result);
 245                         robot.hw_status[STATUS_CAM] = HW_STATUS_OK;
 246                         break;
 247                 }
 248                 case DEADLINE:
 249                         robot.hw_status[STATUS_CAM] = HW_STATUS_FAILED;
 250                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 251                         break;
 252         }
 253 }
 254 /* ----------------------------------------------------------------------
 255  * SUBSCRIBER CALLBACKS - EB2008
 256  * ---------------------------------------------------------------------- */
 257
 258 void rcv_actuator_status_cb(const ORTERecvInfo *info, void *vinstance,
 259                             void *recvCallBackParam)
 260 {
 261         static unsigned short old_lift_pos=0;
 262         static unsigned short old_pusher_pos=0;
 263         switch (info->status) {
 264                 case NEW_DATA:
 265                         // new data arrived and requested position equals actual position
 266                         if (robot.orte.actuator_status.lift_pos == robot.orte.lift.pos &&
 267                                                 robot.orte.actuator_status.lift_pos != old_lift_pos)
 268                                 FSM_SIGNAL(ACT, EV_LIFT_IN_POS, NULL);
 269                         if (robot.orte.actuator_status.pusher_pos == robot.orte.pusher.pos &&
 270                                                 robot.orte.actuator_status.pusher_pos != old_pusher_pos)
 271                                 FSM_SIGNAL(ACT, EV_PUSHER_IN_POS, NULL);
 272                         if (robot.orte.actuator_status.status & ~LIFT_ENDSW_MASK) {
 273                                 robot.hw_status[STATUS_LFT]=HW_STATUS_WARNING;
 274                         } else {
 275                                 robot.hw_status[STATUS_LFT]=HW_STATUS_OK;
 276                         }
 277                         old_lift_pos   = robot.orte.actuator_status.lift_pos;
 278                         old_pusher_pos = robot.orte.actuator_status.pusher_pos;
 279                         break;
 280                 case DEADLINE:
 281                         robot.hw_status[STATUS_LFT]=HW_STATUS_FAILED;
 282                         DBG("ORTE deadline occurred - actuator_status receive\n");
 283                         break;
 284         }
 285 }
 286
 287 void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
 288                             void *recvCallBackParam)
 289 {
 290         switch (info->status) {
 291         case NEW_DATA: {
 292                         double old_distance_value = robot.puck_distance;
 293
 294                         ROBOT_LOCK(sharps);
 295                         robot.puck_distance = robot.orte.puck_distance.distance;
 296                         ROBOT_UNLOCK(sharps);
 297
 298                         if (PUCK_INSIDE(robot.orte.puck_distance.distance) && !PUCK_INSIDE(old_distance_value)) {
 299                                 // signal puck is inside
 300                                 FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
 301                         }
 302                         if (PUCK_REACHABLE(robot.orte.puck_distance.distance) && !PUCK_REACHABLE(old_distance_value)) {
 303                                 // signal puck is reachable
 304                                 FSM_SIGNAL(MAIN, EV_PUCK_REACHABLE, NULL);
 305                         }
 306                 }
 307                 break;
 308         case DEADLINE:
 309                 // FIXME F.J. (?) inform display in case of invalid data?
 310                 // FIXME F.J. (?) limited space on the display...
 311                 //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
 312                 DBG("ORTE deadline occurred - servos receive\n");
 313                 break;
 314         }
 315 }
 316
 317 void rcv_puck_inside_cb(const ORTERecvInfo *info, void *vinstance,
 318                             void *recvCallBackParam)
 319 {
 320         static bool previous_switch_status;
 321         switch (info->status) {
 322         case NEW_DATA: {
 323                 if (robot.use_back_switch) {
 324                         if (robot.orte.puck_inside.status & 0x01) {
 325                                 //FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
 326
 327                                 /* React on the first switch or when moving backward. */
 328                                 if ((previous_switch_status & 0x01) == 0 ||
 329                                     robot.moves_backward) {
 330                                         FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
 331                                 }
 332                                 previous_switch_status = robot.orte.puck_inside.status;
 333                         }
 334                 }
 335                 robot.hw_status[STATUS_CHE]=HW_STATUS_OK;
 336         }
 337                 break;
 338         case DEADLINE:
 339                 robot.hw_status[STATUS_CHE]=HW_STATUS_FAILED;
 340                 DBG("ORTE deadline occurred - servos receive\n");
 341                 break;
 342         }
 343 }
 344
 345 #define HIST_CNT 5
 346 #if 0
 347 static int cmp_double(const void *v1, const void *v2)
 348 {
 349         const double *d1 = v1, *const d2 = v2;
 350         if (d1 < d2)
 351                 return -1;
 352         else if (d1 > d2)
 353                 return +1;
 354         else
 355                 return 0;
 356 }
 357 #endif
 358
 359 int robot_init_orte()
 360 {
 361         int rv = 0;
 362
 363         robot.orte.strength = 20;
 364
 365         rv = robottype_roboorte_init(&robot.orte);
 366         if (rv)
 367                 return rv;
 368
 369         /* creating publishers */
 370         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 371         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 372         robottype_publisher_est_pos_create(&robot.orte, send_est_pos_cb, &robot.orte);
 373         //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 374
 375         // I didn't know what was the difference between the callback function pointer
 376         // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
 377         // IS CRUCIAL!
 378         //   - NULL: message is published only when OrtePublicationSend called
 379         //   - pointer to empty function: message is published periodically
 380         robottype_publisher_lift_create(&robot.orte, send_dummy_cb, &robot.orte);
 381         robottype_publisher_pusher_create(&robot.orte, send_dummy_cb, &robot.orte);
 382         robottype_publisher_chelae_create(&robot.orte, send_dummy_cb, &robot.orte);
 383         robottype_publisher_belts_create(&robot.orte, send_dummy_cb, &robot.orte);
 384         robottype_publisher_holder_create(&robot.orte, send_dummy_cb, &robot.orte);
 385         robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
 386         robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
 387         robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
 388         robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
 389
 390         /* create generic subscribers */
 391         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 392         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 393         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 394         robottype_subscriber_corr_distances_create(&robot.orte, rcv_corr_distances_cb, &robot.orte);
 395         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 396         //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 397         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 398         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 399
 400         /* create subscribers */
 401         robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte);
 402         //robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
 403         robottype_subscriber_puck_inside_create(&robot.orte, rcv_puck_inside_cb, &robot.orte);
 404         robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 405
 406         return rv;
 407 }
 408