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 /* ----------------------------------------------------------------------
  25  * PUBLISHER CALLBACKS - GENERIC
  26  * ---------------------------------------------------------------------- */
  27
  28 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
  29                         void *sendCallBackParam)
  30 {
  31         struct ref_pos_type *instance = (struct ref_pos_type *)vinstance;
  32
  33         ROBOT_LOCK(ref_pos);
  34         *instance = robot.ref_pos;
  35         ROBOT_UNLOCK(ref_pos);
  36 }
  37
  38 void send_est_pos_cb(const ORTESendInfo *info, void *vinstance,
  39                         void *sendCallBackParam)
  40 {
  41         struct est_pos_type *instance = (struct est_pos_type *)vinstance;
  42
  43         ROBOT_LOCK(est_pos);
  44         *instance = robot.est_pos;
  45         ROBOT_UNLOCK(est_pos);
  46 }
  47
  48 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
  49                         void *sendCallBackParam)
  50 {
  51 }
  52
  53 /* ----------------------------------------------------------------------
  54  * SUBSCRIBER CALLBACKS - GENERIC
  55  * ---------------------------------------------------------------------- */
  56
  57 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
  58                         void *recvCallBackParam)
  59 {
  60         struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
  61         static struct motion_irc_type prevInstance;
  62         static int firstRun = 1;
  63         /* spocitat prevodovy pomer */
  64         double n = (double)(28.0 / 1.0);
  65         /* vzdalenost na pulz - 4.442 je empiricka konstanta :) */
  66         double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
  67
  68         double aktk0 = 0;
  69         double aktk1 = 0;
  70         double deltaU = 0;
  71         double deltAlfa = 0;
  72
  73         switch (info->status) {
  74                 case NEW_DATA:
  75                         if(firstRun) {
  76                                 prevInstance = *instance;
  77                                 firstRun = 0;
  78                                 break;
  79                         }
  80
  81                         aktk0 = ((prevInstance.left - instance->left) >> 8) * c;
  82                         aktk1 = ((instance->right - prevInstance.right) >> 8) * c;
  83                         prevInstance = *instance;
  84
  85                         deltaU = (aktk0 + aktk1) / 2;
  86                         deltAlfa = (aktk1 - aktk0) / (2.0*ROBOT_ROTATION_RADIUS_M);
  87
  88                         struct mcl_robot_odo *odo = malloc(sizeof(struct mcl_robot_odo));
  89                         memset(odo, 0, sizeof(*odo));
  90                         odo->dx = deltaU;
  91                         odo->dy = 0;
  92                         odo->dangle = deltAlfa;
  93                         /* FSM_SIGNAL(LOC, EV_ODO_RECEIVED, odo); */
  94                         robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
  95                         break;
  96                 case DEADLINE:
  97                         robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
  98                         DBG("ORTE deadline occurred - motion_irc receive\n");
  99                         break;
 100         }
 101 }
 102
 103 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 104                         void *recvCallBackParam)
 105 {
 106         switch (info->status) {
 107                 case NEW_DATA:
 108                         break;
 109                 case DEADLINE:
 110                         DBG("ORTE deadline occurred - motion_speed receive\n");
 111                         break;
 112         }
 113 }
 114
 115 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 116                         void *recvCallBackParam)
 117 {
 118         switch (info->status) {
 119                 case NEW_DATA:
 120                         break;
 121                 case DEADLINE:
 122                         DBG("ORTE deadline occurred - motion_status receive\n");
 123                         break;
 124         }
 125 }
 126
 127 void rcv_joy_data_cb(const ORTERecvInfo *info, void *vinstance,
 128                         void *recvCallBackParam)
 129 {
 130         struct joy_data_type *instance = (struct joy_data_type *)vinstance;
 131
 132         switch (info->status) {
 133                 case NEW_DATA:
 134                         ROBOT_LOCK(joy_data);
 135                         robot.joy_data = *instance;
 136                         ROBOT_UNLOCK(joy_data);
 137                         break;
 138                 case DEADLINE:
 139                         DBG("ORTE deadline occurred - joy_data receive\n");
 140                         break;
 141         }
 142 }
 143
 144 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 145                         void *recvCallBackParam)
 146 {
 147         switch (info->status) {
 148                 case NEW_DATA:
 149                         robot.hw_status[STATUS_POWER]=HW_STATUS_OK;
 150                         break;
 151                 case DEADLINE:
 152                         robot.hw_status[STATUS_POWER]=HW_STATUS_FAILED;
 153                         DBG("ORTE deadline occurred - pwr_voltage receive\n");
 154                         break;
 155         }
 156 }
 157
 158 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
 159                         void *recvCallBackParam)
 160 {
 161         switch (info->status) {
 162                 case NEW_DATA:
 163                         break;
 164                 case DEADLINE:
 165                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 166                         break;
 167         }
 168 }
 169
 170 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 171                         void *recvCallBackParam)
 172 {
 173         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 174         static struct robot_cmd_type last_instance;
 175         enum robot_state state;
 176
 177         switch (info->status) {
 178                 case NEW_DATA:
 179                         /* Stupid way of controlling the robot by
 180                          * pluggin in and out start connector. */
 181                         if (instance->start == last_instance.start)
 182                                 break;
 183
 184                         ROBOT_LOCK(state);
 185                         state = robot.state;
 186                         ROBOT_UNLOCK(state);
 187                         printf("in state=%d \n", state);
 188
 189                         switch (state) {
 190                                 case JUST_STARTED:
 191                                         /* On first plug-in laser is tarted */
 192                                         if (instance->start)
 193                                                 break;
 194                                         FSM_SIGNAL(MAIN, EV_LASER_POWER, NULL);
 195                                         state = LASER_STARTED;
 196                                         break;
 197
 198                                 case LASER_STARTED:
 199                                         /* Competition starts when plugged out */
 200                                         if (!instance->start)
 201                                                 break;
 202                                         FSM_SIGNAL(MAIN, EV_START, NULL);
 203                                         state = COMPETITION_STARTED;
 204                                         break;
 205
 206                                 case COMPETITION_STARTED:
 207                                         /* Subsequent plug-in stops the robot */
 208                                         if (!instance->start) {
 209                                                 robot_exit();
 210                                         }
 211                                         break;
 212                         }
 213                         last_instance = *instance;
 214                         printf("out state=%d \n", state);
 215                         ROBOT_LOCK(state);
 216                         robot.state = state;
 217                         ROBOT_UNLOCK(state);
 218                         break;
 219                 case DEADLINE:
 220                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 221                         break;
 222         }
 223 }
 224
 225 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 226                         void *recvCallBackParam)
 227 {
 228         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 229
 230         switch (info->status) {
 231                 case NEW_DATA: {
 232                         ROBOT_LOCK(hokuyo);
 233                         robot.hokuyo = *instance;
 234                         ROBOT_UNLOCK(hokuyo);
 235                         update_map_hokuyo(instance);
 236                         break;
 237                 }
 238                 case DEADLINE:
 239                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 240                         break;
 241         }
 242 }
 243 /* ----------------------------------------------------------------------
 244  * SUBSCRIBER CALLBACKS - EB2008
 245  * ---------------------------------------------------------------------- */
 246
 247 void rcv_servos_cb(const ORTERecvInfo *info, void *vinstance,
 248                         void *recvCallBackParam)
 249 {
 250         switch (info->status) {
 251                 case NEW_DATA:
 252                         break;
 253                 case DEADLINE:
 254                         DBG("ORTE deadline occurred - servos receive\n");
 255                         break;
 256         }
 257 }
 258
 259 void rcv_drives_cb(const ORTERecvInfo *info, void *vinstance,
 260                         void *recvCallBackParam)
 261 {
 262         struct drives_type *instance = (struct drives_type *)vinstance;
 263
 264         switch (info->status) {
 265                 case NEW_DATA:
 266                         ROBOT_LOCK(drives);
 267                         robot.drives = *instance;
 268                         ROBOT_UNLOCK(drives);
 269                         break;
 270                 case DEADLINE:
 271                         DBG("ORTE deadline occurred - drives receive\n");
 272                         break;
 273         }
 274 }
 275
 276 void rcv_laser_data_cb(const ORTERecvInfo *info, void *vinstance,
 277                         void *recvCallBackParam)
 278 {
 279         struct laser_data_type *instance = (struct laser_data_type *)vinstance;
 280
 281         switch (info->status) {
 282                 case NEW_DATA: {
 283                         struct mcl_laser_measurement *meas_angles;
 284                         meas_angles = malloc(sizeof(*meas_angles));
 285                         memset(meas_angles, 0, sizeof(*meas_angles));
 286
 287                         meas_angles->count = 1;
 288                         meas_angles->val[0] =
 289                                 (double)TIME2ANGLE(instance->period,
 290                                                    instance->measure);
 291 //                      DBG("a=%f\n", RAD2DEG(robot.meas_angles.val[0]));
 292
 293 #if 0
 294                         /* Commented out because FSM_SIGNAL_TIMED was removed  */
 295                         bool sent;
 296                         FSM_SIGNAL_TIMED(LOC, EV_LASER_RECEIVED, meas_angles, 20/*ms*/, &sent);
 297                         if (!sent) {
 298                                 DBG("%s: MCL is busy - not updating\n", __FUNCTION__);
 299                                 free(meas_angles);
 300                         }
 301 #endif
 302                         robot.hw_status[STATUS_LASER] = HW_STATUS_OK;
 303                         break;
 304                 }
 305                 case DEADLINE:
 306                         robot.hw_status[STATUS_LASER] = HW_STATUS_FAILED;
 307                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 308                         break;
 309         }
 310 }
 311
 312 void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
 313                         void *recvCallBackParam)
 314 {
 315         struct cmu_type *instance = (struct cmu_type *)vinstance;
 316         static enum ball_color last_color = NO_BALL;
 317         static unsigned char first = 1;
 318
 319         switch (info->status) {
 320                 case NEW_DATA: {
 321                         ROBOT_LOCK(cmu);
 322                         robot.cmu = *instance;
 323                         ROBOT_UNLOCK(cmu);
 324                         if (first) {
 325                                 last_color = robot.cmu.color;
 326                                 first = 0;
 327                         }
 328                         if (robot.cmu.color != NO_BALL) {
 329                                 if (last_color != robot.cmu.color) {
 330                                         last_color = robot.cmu.color;
 331                                         FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
 332                                 }
 333                         }
 334                         robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
 335                         break;
 336                 }
 337                 case DEADLINE:
 338                         robot.hw_status[STATUS_CMU] = HW_STATUS_FAILED;
 339                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 340                         break;
 341         }
 342 }
 343
 344 void rcv_bumper_cb(const ORTERecvInfo *info, void *vinstance,
 345                         void *recvCallBackParam)
 346 {
 347         struct bumper_type *instance = (struct bumper_type *)vinstance;
 348
 349         switch (info->status) {
 350                 case NEW_DATA: {
 351                         ROBOT_LOCK(bumper);
 352                         robot.bumper = *instance;
 353                         ROBOT_UNLOCK(bumper);
 354                         break;
 355                 }
 356                 case DEADLINE:
 357                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 358                         break;
 359         }
 360 }
 361
 362 #define HIST_CNT 5
 363
 364 static int cmp_double(const void *v1, const void *v2)
 365 {
 366         const double *d1 = v1, *const d2 = v2;
 367         if (d1 < d2)
 368                 return -1;
 369         else if (d1 > d2)
 370                 return +1;
 371         else
 372                 return 0;
 373 }
 374
 375 static inline double *sharp_ptr(struct sharps_type *sharps, int index)
 376 {
 377         switch (index) {
 378                 case 0: return &sharps->front_left; break;
 379                 case 1: return &sharps->front_right; break;
 380                 case 2: return &sharps->left; break;
 381                 case 3: return &sharps->right; break;
 382                 default: return &sharps->front_left; break;
 383         }
 384
 385 }
 386
 387 void rcv_sharps_cb(const ORTERecvInfo *info, void *vinstance,
 388                         void *recvCallBackParam)
 389 {
 390         struct sharps_type *instance = (struct sharps_type *)vinstance;
 391         static struct sharps_type history[HIST_CNT];
 392         static double history_sorted[HIST_CNT];
 393         static int ind = -1;
 394         int i, s;
 395         //struct sharps_type *for_mcl;
 396
 397         switch (info->status) {
 398                 case NEW_DATA: {
 399                         ROBOT_LOCK(sharps);
 400                         robot.sharps = *instance;
 401                         ROBOT_UNLOCK(sharps);
 402                         if (ind == -1) {
 403                                 for (i=0; i<HIST_CNT; i++) {
 404                                         history[i].front_left = 0.8;
 405                                         history[i].front_right = 0.8;
 406                                         history[i].left = 0.8;
 407                                         history[i].right = 0.8;
 408                                 }
 409                         }
 410                         ind++;
 411                         if (ind >= HIST_CNT) {
 412                                 ind = 0;
 413                         }
 414                         history[ind] = *instance;
 415
 416                         for (s=0; s<4; s++) {
 417                                 for (i=0; i<HIST_CNT; i++) {
 418                                         history_sorted[i] = *sharp_ptr(&history[i], s);
 419                                 }
 420                                 qsort(history_sorted, HIST_CNT, sizeof(history_sorted[0]), cmp_double);
 421                                 *sharp_ptr(instance, s) = history_sorted[HIST_CNT/2]; /* Get median */
 422                         }
 423
 424                         update_map(instance);
 425                         break;
 426                 }
 427                 case DEADLINE:
 428                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 429                         break;
 430         }
 431 }
 432
 433 int robot_init_orte()
 434 {
 435         int rv = 0;
 436
 437         robot.orte.strength = 20;
 438
 439         rv = robottype_roboorte_init(&robot.orte);
 440         if (rv)
 441                 return rv;
 442
 443         /* creating publishers */
 444         robottype_publisher_motion_irc_create(&robot.orte, NULL, &robot.orte);
 445         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 446         robottype_publisher_motion_status_create(&robot.orte, NULL, &robot.orte);
 447         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 448         robottype_publisher_est_pos_create(&robot.orte, send_est_pos_cb, &robot.orte);
 449         robottype_publisher_joy_data_create(&robot.orte, NULL, &robot.orte);
 450         robottype_publisher_pwr_voltage_create(&robot.orte, NULL, &robot.orte);
 451         robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 452
 453         robottype_publisher_servos_create(&robot.orte, send_dummy_cb, &robot.orte);
 454         robottype_publisher_drives_create(&robot.orte, send_dummy_cb, &robot.orte);
 455         robottype_publisher_laser_cmd_create(&robot.orte, NULL, NULL);
 456
 457         /* create generic subscribers */
 458         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 459         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 460         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 461         robottype_subscriber_joy_data_create(&robot.orte, rcv_joy_data_cb, &robot.orte);
 462         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 463         robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 464         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 465         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 466
 467         /* create eb2008 subscribers */
 468         robottype_subscriber_servos_create(&robot.orte, rcv_servos_cb, &robot.orte);
 469         robottype_subscriber_drives_create(&robot.orte, rcv_drives_cb, &robot.orte);
 470         robottype_subscriber_laser_data_create(&robot.orte, rcv_laser_data_cb, &robot.orte);
 471         robottype_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
 472         robottype_subscriber_bumper_create(&robot.orte, rcv_bumper_cb, &robot.orte);
 473         robottype_subscriber_sharps_create(&robot.orte, rcv_sharps_cb, &robot.orte);
 474
 475         return rv;
 476 }
 477