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