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