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_UNLOCK(motion_irc);
  71
  72                         /* FSM_SIGNAL(LOC, EV_ODO_RECEIVED, odo); */
  73                         robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
  74                         break;
  75                 case DEADLINE:
  76                         robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
  77                         DBG("ORTE deadline occurred - motion_irc receive\n");
  78                         break;
  79         }
  80 }
  81
  82 void rcv_corr_distances_cb(const ORTERecvInfo *info, void *vinstance,
  83                            void *recvCallBackParam)
  84 {
  85         struct corr_distances_type *instance =
  86                 (struct corr_distances_type *)vinstance;
  87
  88         switch (info->status) {
  89                 case NEW_DATA:
  90                         /* locking should not be needed, but... */
  91                         ROBOT_LOCK(corr_distances);
  92                         robot.corr_distances = *instance;
  93                         ROBOT_UNLOCK(corr_distances);
  94
  95                         /* wake up motion-control/thread_trajectory_follower */
  96                         sem_post(&measurement_received);
  97
  98                         /*FIXME: is following OK? (pecam1) */
  99                         robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
 100                         break;
 101                 case DEADLINE:
 102                         robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
 103                         DBG("ORTE deadline occurred - corr_distances receive\n");
 104                         break;
 105         }
 106 }
 107
 108 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 109                         void *recvCallBackParam)
 110 {
 111         switch (info->status) {
 112                 case NEW_DATA:
 113                         break;
 114                 case DEADLINE:
 115                         DBG("ORTE deadline occurred - motion_speed receive\n");
 116                         break;
 117         }
 118 }
 119
 120 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 121                         void *recvCallBackParam)
 122 {
 123         switch (info->status) {
 124                 case NEW_DATA:
 125                         break;
 126                 case DEADLINE:
 127                         DBG("ORTE deadline occurred - motion_status receive\n");
 128                         break;
 129         }
 130 }
 131
 132 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 133                         void *recvCallBackParam)
 134 {
 135         switch (info->status) {
 136                 case NEW_DATA:
 137                         robot.hw_status[STATUS_POWER]=HW_STATUS_OK;
 138                         break;
 139                 case DEADLINE:
 140                         robot.hw_status[STATUS_POWER]=HW_STATUS_FAILED;
 141                         DBG("ORTE deadline occurred - pwr_voltage receive\n");
 142                         break;
 143         }
 144 }
 145
 146 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
 147                         void *recvCallBackParam)
 148 {
 149         switch (info->status) {
 150                 case NEW_DATA:
 151                         break;
 152                 case DEADLINE:
 153                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 154                         break;
 155         }
 156 }
 157
 158 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 159                         void *recvCallBackParam)
 160 {
 161         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 162         static struct robot_cmd_type last_instance;
 163         enum robot_state state;
 164
 165         switch (info->status) {
 166                 case NEW_DATA:
 167                         /* Stupid way of controlling the robot by
 168                          * pluggin in and out start connector. */
 169                         if (instance->start == last_instance.start)
 170                                 break;
 171
 172                         ROBOT_LOCK(state);
 173                         state = robot.state;
 174                         ROBOT_UNLOCK(state);
 175                         printf("in state=%d \n", state);
 176
 177                         switch (state) {
 178                                 case JUST_STARTED:
 179                                         /* On first plug-in laser is tarted */
 180                                         if (instance->start)
 181                                                 break;
 182                                         FSM_SIGNAL(MAIN, EV_LASER_POWER, NULL);
 183                                         state = LASER_STARTED;
 184                                         break;
 185
 186                                 case LASER_STARTED:
 187                                         /* Competition starts when plugged out */
 188                                         if (!instance->start)
 189                                                 break;
 190                                         FSM_SIGNAL(MAIN, EV_START, NULL);
 191                                         state = COMPETITION_STARTED;
 192                                         break;
 193
 194                                 case COMPETITION_STARTED:
 195                                         /* Subsequent plug-in stops the robot */
 196                                         if (!instance->start) {
 197                                                 robot_exit();
 198                                         }
 199                                         break;
 200                         }
 201                         last_instance = *instance;
 202                         printf("out state=%d \n", state);
 203                         ROBOT_LOCK(state);
 204                         robot.state = state;
 205                         ROBOT_UNLOCK(state);
 206                         break;
 207                 case DEADLINE:
 208                         DBG("ORTE deadline occurred - pwr_ctrl 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_UNLOCK(hokuyo);
 223                         update_map_hokuyo(instance);
 224                         break;
 225                 }
 226                 case DEADLINE:
 227                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 228                         break;
 229         }
 230 }
 231 /* ----------------------------------------------------------------------
 232  * SUBSCRIBER CALLBACKS - EB2008
 233  * ---------------------------------------------------------------------- */
 234
 235 void rcv_actuator_status_cb(const ORTERecvInfo *info, void *vinstance,
 236                             void *recvCallBackParam)
 237 {
 238         switch (info->status) {
 239                 case NEW_DATA:
 240                         if (robot.orte.actuator_status.status == 0) {
 241                                 robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_OK;
 242                         } else {
 243                                 robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_WARNING;
 244                         }
 245                         // robot.orte.actuator_status.lift_pos
 246                         // robot.orte.actuator_status.pusher_pos
 247                         //FSM_SIGNAL(ACT, STATUS_RCVD, NULL);
 248                         break;
 249                 case DEADLINE:
 250                         robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
 251                         DBG("ORTE deadline occurred - servos receive\n");
 252                         break;
 253         }
 254 }
 255
 256 void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
 257                             void *recvCallBackParam)
 258 {
 259         switch (info->status) {
 260                 case NEW_DATA:
 261                         //printf("Sharp distance: %f\n", robot.orte.puck_distance.distance);
 262                         // FIXME F.J. (?) inform display in case of invalid data?
 263                         break;
 264                 case DEADLINE:
 265                         // FIXME F.J. (?) limited space on the display...
 266                         //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
 267                         DBG("ORTE deadline occurred - servos receive\n");
 268                         break;
 269         }
 270 }
 271
 272 #if 0
 273 void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
 274                         void *recvCallBackParam)
 275 {
 276         struct cmu_type *instance = (struct cmu_type *)vinstance;
 277         static enum ball_color last_color = NO_BALL;
 278         static unsigned char first = 1;
 279
 280         switch (info->status) {
 281                 case NEW_DATA: {
 282                         ROBOT_LOCK(cmu);
 283                         robot.cmu = *instance;
 284                         ROBOT_UNLOCK(cmu);
 285                         if (first) {
 286                                 last_color = robot.cmu.color;
 287                                 first = 0;
 288                         }
 289                         if (robot.cmu.color != NO_BALL) {
 290                                 if (last_color != robot.cmu.color) {
 291                                         last_color = robot.cmu.color;
 292                                         //FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
 293                                 }
 294                         }
 295                         robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
 296                         break;
 297                 }
 298                 case DEADLINE:
 299                         robot.hw_status[STATUS_CMU] = HW_STATUS_FAILED;
 300                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 301                         break;
 302         }
 303 }
 304 #endif
 305
 306 #define HIST_CNT 5
 307
 308 static int cmp_double(const void *v1, const void *v2)
 309 {
 310         const double *d1 = v1, *const d2 = v2;
 311         if (d1 < d2)
 312                 return -1;
 313         else if (d1 > d2)
 314                 return +1;
 315         else
 316                 return 0;
 317 }
 318
 319 int robot_init_orte()
 320 {
 321         int rv = 0;
 322
 323         robot.orte.strength = 20;
 324
 325         rv = robottype_roboorte_init(&robot.orte);
 326         if (rv)
 327                 return rv;
 328
 329         /* creating publishers */
 330         robottype_publisher_motion_irc_create(&robot.orte, NULL, &robot.orte);
 331         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 332         robottype_publisher_motion_status_create(&robot.orte, NULL, &robot.orte);
 333         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 334         robottype_publisher_est_pos_create(&robot.orte, send_est_pos_cb, &robot.orte);
 335         robottype_publisher_pwr_voltage_create(&robot.orte, NULL, &robot.orte);
 336         robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 337
 338         robottype_publisher_lift_create(&robot.orte, send_dummy_cb, &robot.orte);
 339         robottype_publisher_pusher_create(&robot.orte, send_dummy_cb, &robot.orte);
 340         robottype_publisher_chelae_create(&robot.orte, NULL, &robot.orte);
 341         robottype_publisher_belts_create(&robot.orte, NULL, &robot.orte);
 342         robottype_publisher_holder_create(&robot.orte, send_dummy_cb, &robot.orte);
 343
 344         /* create generic subscribers */
 345         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 346         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 347         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 348         robottype_subscriber_corr_distances_create(&robot.orte, rcv_corr_distances_cb, &robot.orte);
 349         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 350         robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 351         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 352         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 353
 354         /* create subscribers */
 355         robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte);
 356         robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
 357         //robottype_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
 358
 359         return rv;
 360 }
 361