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                                         robot.hw_status[STATUS_STA] = HW_STATUS_WARN;
 184 //                                      FSM_SIGNAL(MAIN, EV_LASER_POWER, NULL);
 185                                         state = LASER_STARTED;
 186                                         break;
 187
 188                                 case LASER_STARTED:
 189                                         /* Competition starts when plugged out */
 190                                         if (!instance->start)
 191                                                 break;
 192                                         robot.hw_status[STATUS_STA] = HW_STATUS_OK;
 193                                         FSM_SIGNAL(MAIN, EV_START, NULL);
 194                                         state = COMPETITION_STARTED;
 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                         printf("out state=%d \n", state);
 206                         ROBOT_LOCK(state);
 207                         robot.state = state;
 208                         ROBOT_UNLOCK(state);
 209                         break;
 210                 case DEADLINE:
 211                         robot.hw_status[STATUS_STA] = HW_STATUS_FAILED;
 212                         DBG("ORTE deadline occurred - robot_cmd receive\n");
 213                         break;
 214         }
 215 }
 216
 217 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 218                         void *recvCallBackParam)
 219 {
 220         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 221
 222         switch (info->status) {
 223                 case NEW_DATA: {
 224                         ROBOT_LOCK(hokuyo);
 225                         robot.hokuyo = *instance;
 226                         robot.hw_status[STATUS_HOK] = HW_STATUS_OK;
 227                         ROBOT_UNLOCK(hokuyo);
 228                         update_map_hokuyo(instance);
 229                         break;
 230                 }
 231                 case DEADLINE:
 232                         robot.hw_status[STATUS_HOK] = HW_STATUS_FAILED;
 233                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 234                         break;
 235         }
 236 }
 237 /* ----------------------------------------------------------------------
 238  * SUBSCRIBER CALLBACKS - EB2008
 239  * ---------------------------------------------------------------------- */
 240
 241 void rcv_actuator_status_cb(const ORTERecvInfo *info, void *vinstance,
 242                             void *recvCallBackParam)
 243 {
 244         switch (info->status) {
 245                 case NEW_DATA:
 246                         if (robot.orte.actuator_status.status == 0) {
 247                                 robot.hw_status[STATUS_LFT]=HW_STATUS_OK;
 248                                 //FIXME
 249                         } else {
 250                                 robot.hw_status[STATUS_LFT]=HW_STATUS_WARNING;
 251                                 //FIXME
 252                         }
 253                         //FSM_SIGNAL(ACT, STATUS_RCVD, NULL);
 254                         break;
 255                 case DEADLINE:
 256                         robot.hw_status[STATUS_LFT]=HW_STATUS_FAILED;
 257                         //FIXME
 258                         DBG("ORTE deadline occurred - actuator_status receive\n");
 259                         break;
 260         }
 261 }
 262
 263 void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
 264                             void *recvCallBackParam)
 265 {
 266         switch (info->status) {
 267                 case NEW_DATA:
 268                         //printf("Sharp distance: %f\n", robot.orte.puck_distance.distance);
 269                         if(robot.orte.puck_distance.distance <= 0.055) {
 270                                 FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
 271                         }
 272                         if(robot.orte.puck_distance.distance < 0.13 && robot.orte.puck_distance.distance > 0.055) {
 273                                 FSM_SIGNAL(ACT, EV_PUCK_REACHABLE, NULL);
 274                         }
 275                         break;
 276                 case DEADLINE:
 277                         // FIXME F.J. (?) inform display in case of invalid data?
 278                         // FIXME F.J. (?) limited space on the display...
 279                         //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
 280                         DBG("ORTE deadline occurred - servos receive\n");
 281                         break;
 282         }
 283 }
 284
 285 #if 0
 286 void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
 287                         void *recvCallBackParam)
 288 {
 289         struct cmu_type *instance = (struct cmu_type *)vinstance;
 290         static enum ball_color last_color = NO_BALL;
 291         static unsigned char first = 1;
 292
 293         switch (info->status) {
 294                 case NEW_DATA: {
 295                         ROBOT_LOCK(cmu);
 296                         robot.cmu = *instance;
 297                         ROBOT_UNLOCK(cmu);
 298                         if (first) {
 299                                 last_color = robot.cmu.color;
 300                                 first = 0;
 301                         }
 302                         if (robot.cmu.color != NO_BALL) {
 303                                 if (last_color != robot.cmu.color) {
 304                                         last_color = robot.cmu.color;
 305                                         //FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
 306                                 }
 307                         }
 308                         robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
 309                         break;
 310                 }
 311                 case DEADLINE:
 312                         robot.hw_status[STATUS_CMU] = HW_STATUS_FAILED;
 313                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 314                         break;
 315         }
 316 }
 317 #endif
 318
 319 #define HIST_CNT 5
 320
 321 static int cmp_double(const void *v1, const void *v2)
 322 {
 323         const double *d1 = v1, *const d2 = v2;
 324         if (d1 < d2)
 325                 return -1;
 326         else if (d1 > d2)
 327                 return +1;
 328         else
 329                 return 0;
 330 }
 331
 332 int robot_init_orte()
 333 {
 334         int rv = 0;
 335
 336         robot.orte.strength = 20;
 337
 338         rv = robottype_roboorte_init(&robot.orte);
 339         if (rv)
 340                 return rv;
 341
 342         /* creating publishers */
 343         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 344         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 345         robottype_publisher_est_pos_create(&robot.orte, send_est_pos_cb, &robot.orte);
 346         //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 347
 348         // I didn't know what was the difference between the callback function pointer
 349         // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
 350         // IS CRUCIAL!
 351         //   - NULL: message is published only when OrtePublicationSend called
 352         //   - pointer to empty function: message is published periodically
 353         robottype_publisher_lift_create(&robot.orte, send_dummy_cb, &robot.orte);
 354         robottype_publisher_pusher_create(&robot.orte, send_dummy_cb, &robot.orte);
 355         robottype_publisher_chelae_create(&robot.orte, send_dummy_cb, &robot.orte);
 356         robottype_publisher_belts_create(&robot.orte, send_dummy_cb, &robot.orte);
 357         robottype_publisher_holder_create(&robot.orte, send_dummy_cb, &robot.orte);
 358         robottype_publisher_hokuyo_pitch_create(&robot.orte, send_dummy_cb, &robot.orte);
 359
 360         /* create generic subscribers */
 361         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 362         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 363         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 364         robottype_subscriber_corr_distances_create(&robot.orte, rcv_corr_distances_cb, &robot.orte);
 365         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 366         robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 367         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 368         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 369
 370         /* create subscribers */
 371         robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte);
 372         robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
 373         //robottype_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
 374
 375         return rv;
 376 }
 377