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