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                         //printf("Sharp distance: %f\n", robot.orte.puck_distance.distance);
 266                         if(robot.orte.puck_distance.distance <= 0.055) {
 267                                 FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
 268                         }
 269                         if(robot.orte.puck_distance.distance < 0.13 && robot.orte.puck_distance.distance > 0.055) {
 270                                 FSM_SIGNAL(ACT, EV_PUCK_REACHABLE, NULL);
 271                         }
 272                         break;
 273                 case DEADLINE:
 274                         // FIXME F.J. (?) inform display in case of invalid data?
 275                         // FIXME F.J. (?) limited space on the display...
 276                         //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
 277                         DBG("ORTE deadline occurred - servos receive\n");
 278                         break;
 279         }
 280 }
 281
 282 #if 0
 283 void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
 284                         void *recvCallBackParam)
 285 {
 286         struct cmu_type *instance = (struct cmu_type *)vinstance;
 287         static enum ball_color last_color = NO_BALL;
 288         static unsigned char first = 1;
 289
 290         switch (info->status) {
 291                 case NEW_DATA: {
 292                         ROBOT_LOCK(cmu);
 293                         robot.cmu = *instance;
 294                         ROBOT_UNLOCK(cmu);
 295                         if (first) {
 296                                 last_color = robot.cmu.color;
 297                                 first = 0;
 298                         }
 299                         if (robot.cmu.color != NO_BALL) {
 300                                 if (last_color != robot.cmu.color) {
 301                                         last_color = robot.cmu.color;
 302                                         //FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
 303                                 }
 304                         }
 305                         robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
 306                         break;
 307                 }
 308                 case DEADLINE:
 309                         robot.hw_status[STATUS_CMU] = HW_STATUS_FAILED;
 310                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 311                         break;
 312         }
 313 }
 314 #endif
 315
 316 #define HIST_CNT 5
 317
 318 static int cmp_double(const void *v1, const void *v2)
 319 {
 320         const double *d1 = v1, *const d2 = v2;
 321         if (d1 < d2)
 322                 return -1;
 323         else if (d1 > d2)
 324                 return +1;
 325         else
 326                 return 0;
 327 }
 328
 329 int robot_init_orte()
 330 {
 331         int rv = 0;
 332
 333         robot.orte.strength = 20;
 334
 335         rv = robottype_roboorte_init(&robot.orte);
 336         if (rv)
 337                 return rv;
 338
 339         /* creating publishers */
 340         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 341         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 342         robottype_publisher_est_pos_create(&robot.orte, send_est_pos_cb, &robot.orte);
 343         //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 344
 345         // FIXME (F.J.): what is the difference between the callback function pointer
 346         //               being NULL and being set to pointer to empty send_dummy_cb function?
 347         robottype_publisher_lift_create(&robot.orte, send_dummy_cb, &robot.orte);
 348         robottype_publisher_pusher_create(&robot.orte, send_dummy_cb, &robot.orte);
 349         robottype_publisher_chelae_create(&robot.orte, NULL, &robot.orte);
 350         robottype_publisher_belts_create(&robot.orte, NULL, &robot.orte);
 351         robottype_publisher_holder_create(&robot.orte, send_dummy_cb, &robot.orte);
 352         robottype_publisher_hokuyo_pitch_create(&robot.orte, NULL, &robot.orte);
 353
 354         /* create generic subscribers */
 355         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 356         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 357         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 358         robottype_subscriber_corr_distances_create(&robot.orte, rcv_corr_distances_cb, &robot.orte);
 359         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 360         robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 361         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 362         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 363
 364         /* create subscribers */
 365         robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte);
 366         robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
 367         //robottype_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
 368
 369         return rv;
 370 }
 371