src/robofsm/eb2008/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_generic.h>
  13 #include <roboorte_eb2008.h>
  14 #include <robodata.h>
  15 #include <robot_eb2008.h>
  16 #include <movehelper_eb2008.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                         if (instance->start == last_instance.start)
 180                                 break;
 181
 182                         ROBOT_LOCK(state);
 183                         state = robot.state;
 184                         ROBOT_UNLOCK(state);
 185                         printf("in state=%d \n", state);
 186
 187                         switch (state) {
 188                                 case JUST_STARTED:
 189                                         if (instance->start)
 190                                                 break;
 191                                         FSM_SIGNAL(MAIN, EV_LASER_POWER, NULL);
 192                                         state = LASER_STARTED;
 193                                         break;
 194
 195                                 case LASER_STARTED:
 196                                         if (!instance->start)
 197                                                 break;
 198                                         FSM_SIGNAL(MAIN, EV_START, NULL);
 199                                         state = COMPETITION_STARTED;
 200                                         break;
 201
 202                                 case COMPETITION_STARTED:
 203                                         if (!instance->start) {
 204                                                 robot_exit();
 205                                         }
 206                                         break;
 207                         }
 208                         last_instance = *instance;
 209                         printf("out state=%d \n", state);
 210                         ROBOT_LOCK(state);
 211                         robot.state = state;
 212                         ROBOT_UNLOCK(state);
 213                         break;
 214                 case DEADLINE:
 215                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 216                         break;
 217         }
 218 }
 219
 220 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 221                         void *recvCallBackParam)
 222 {
 223         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 224
 225         switch (info->status) {
 226                 case NEW_DATA: {
 227                         ROBOT_LOCK(hokuyo);
 228                         robot.hokuyo = *instance;
 229                         ROBOT_UNLOCK(hokuyo);
 230                         printf("hokyo: %d %d %d\n",instance->data1, instance->data2, instance->data3);
 231                         update_map_hokuyo(instance);
 232                         break;
 233                 }
 234                 case DEADLINE:
 235                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 236                         break;
 237         }
 238 }
 239 /* ----------------------------------------------------------------------
 240  * SUBSCRIBER CALLBACKS - EB2008
 241  * ---------------------------------------------------------------------- */
 242
 243 void rcv_servos_cb(const ORTERecvInfo *info, void *vinstance,
 244                         void *recvCallBackParam)
 245 {
 246         switch (info->status) {
 247                 case NEW_DATA:
 248                         break;
 249                 case DEADLINE:
 250                         DBG("ORTE deadline occurred - servos receive\n");
 251                         break;
 252         }
 253 }
 254
 255 void rcv_drives_cb(const ORTERecvInfo *info, void *vinstance,
 256                         void *recvCallBackParam)
 257 {
 258         struct drives_type *instance = (struct drives_type *)vinstance;
 259
 260         switch (info->status) {
 261                 case NEW_DATA:
 262                         ROBOT_LOCK(drives);
 263                         robot.drives = *instance;
 264                         ROBOT_UNLOCK(drives);
 265                         break;
 266                 case DEADLINE:
 267                         DBG("ORTE deadline occurred - drives receive\n");
 268                         break;
 269         }
 270 }
 271
 272 void rcv_laser_data_cb(const ORTERecvInfo *info, void *vinstance,
 273                         void *recvCallBackParam)
 274 {
 275         struct laser_data_type *instance = (struct laser_data_type *)vinstance;
 276
 277         switch (info->status) {
 278                 case NEW_DATA: {
 279                         struct mcl_laser_measurement *meas_angles;
 280                         meas_angles = malloc(sizeof(*meas_angles));
 281                         memset(meas_angles, 0, sizeof(*meas_angles));
 282
 283                         meas_angles->count = 1;
 284                         meas_angles->val[0] =
 285                                 (double)TIME2ANGLE(instance->period,
 286                                                    instance->measure);
 287 //                      DBG("a=%f\n", RAD2DEG(robot.meas_angles.val[0]));
 288
 289                         bool sent;
 290                         FSM_SIGNAL_TIMED(LOC, EV_LASER_RECEIVED, meas_angles, 20/*ms*/, &sent);
 291                         if (!sent) {
 292                                 DBG("%s: MCL is busy - not updating\n", __FUNCTION__);
 293                                 free(meas_angles);
 294                         }
 295                         robot.hw_status[STATUS_LASER] = HW_STATUS_OK;
 296                         break;
 297                 }
 298                 case DEADLINE:
 299                         robot.hw_status[STATUS_LASER] = HW_STATUS_FAILED;
 300                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 301                         break;
 302         }
 303 }
 304
 305 void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
 306                         void *recvCallBackParam)
 307 {
 308         struct cmu_type *instance = (struct cmu_type *)vinstance;
 309         static enum ball_color last_color = NO_BALL;
 310         static unsigned char first = 1;
 311
 312         switch (info->status) {
 313                 case NEW_DATA: {
 314                         ROBOT_LOCK(cmu);
 315                         robot.cmu = *instance;
 316                         ROBOT_UNLOCK(cmu);
 317                         if (first) {
 318                                 last_color = robot.cmu.color;
 319                                 first = 0;
 320                         }
 321                         if (robot.cmu.color != NO_BALL) {
 322                                 if (last_color != robot.cmu.color) {
 323                                         last_color = robot.cmu.color;
 324                                         FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
 325                                 }
 326                         }
 327                         robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
 328                         break;
 329                 }
 330                 case DEADLINE:
 331                         robot.hw_status[STATUS_CMU] = HW_STATUS_FAILED;
 332                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 333                         break;
 334         }
 335 }
 336
 337 void rcv_bumper_cb(const ORTERecvInfo *info, void *vinstance,
 338                         void *recvCallBackParam)
 339 {
 340         struct bumper_type *instance = (struct bumper_type *)vinstance;
 341
 342         switch (info->status) {
 343                 case NEW_DATA: {
 344                         ROBOT_LOCK(bumper);
 345                         robot.bumper = *instance;
 346                         ROBOT_UNLOCK(bumper);
 347                         break;
 348                 }
 349                 case DEADLINE:
 350                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 351                         break;
 352         }
 353 }
 354
 355 #define HIST_CNT 5
 356
 357 static int cmp_double(const void *v1, const void *v2)
 358 {
 359         const double *d1 = v1, *const d2 = v2;
 360         if (d1 < d2)
 361                 return -1;
 362         else if (d1 > d2)
 363                 return +1;
 364         else
 365                 return 0;
 366 }
 367
 368 static inline double *sharp_ptr(struct sharps_type *sharps, int index)
 369 {
 370         switch (index) {
 371                 case 0: return &sharps->front_left; break;
 372                 case 1: return &sharps->front_right; break;
 373                 case 2: return &sharps->left; break;
 374                 case 3: return &sharps->right; break;
 375                 default: return &sharps->front_left; break;
 376         }
 377
 378 }
 379
 380 void rcv_sharps_cb(const ORTERecvInfo *info, void *vinstance,
 381                         void *recvCallBackParam)
 382 {
 383         struct sharps_type *instance = (struct sharps_type *)vinstance;
 384         static struct sharps_type history[HIST_CNT];
 385         static double history_sorted[HIST_CNT];
 386         static int ind = -1;
 387         int i, s;
 388         //struct sharps_type *for_mcl;
 389
 390         switch (info->status) {
 391                 case NEW_DATA: {
 392                         ROBOT_LOCK(sharps);
 393                         robot.sharps = *instance;
 394                         ROBOT_UNLOCK(sharps);
 395                         if (ind == -1) {
 396                                 for (i=0; i<HIST_CNT; i++) {
 397                                         history[i].front_left = 0.8;
 398                                         history[i].front_right = 0.8;
 399                                         history[i].left = 0.8;
 400                                         history[i].right = 0.8;
 401                                 }
 402                         }
 403                         ind++;
 404                         if (ind >= HIST_CNT) {
 405                                 ind = 0;
 406                         }
 407                         history[ind] = *instance;
 408
 409                         for (s=0; s<4; s++) {
 410                                 for (i=0; i<HIST_CNT; i++) {
 411                                         history_sorted[i] = *sharp_ptr(&history[i], s);
 412                                 }
 413                                 qsort(history_sorted, HIST_CNT, sizeof(history_sorted[0]), cmp_double);
 414                                 *sharp_ptr(instance, s) = history_sorted[HIST_CNT/2]; /* Get median */
 415                         }
 416
 417                         update_map(instance);
 418                         break;
 419                 }
 420                 case DEADLINE:
 421                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 422                         break;
 423         }
 424 }
 425
 426 int robot_init_orte()
 427 {
 428         int rv = 0;
 429
 430         robot.orte.strength = 20;
 431         robot.gorte.strength = 20;
 432
 433         rv = eb2008_roboorte_init(&robot.orte);
 434         if (rv)
 435                 return rv;
 436
 437         rv = generic_roboorte_init(&robot.gorte);
 438         if (rv)
 439                 return rv;
 440
 441         /* creating publishers */
 442         generic_publisher_motion_irc_create(&robot.gorte, NULL, &robot.gorte);
 443         generic_publisher_motion_speed_create(&robot.gorte, NULL, &robot.gorte);
 444         generic_publisher_motion_status_create(&robot.gorte, NULL, &robot.gorte);
 445         generic_publisher_ref_pos_create(&robot.gorte, send_ref_pos_cb, &robot.gorte);
 446         generic_publisher_est_pos_create(&robot.gorte, send_est_pos_cb, &robot.gorte);
 447         generic_publisher_joy_data_create(&robot.gorte, NULL, &robot.gorte);
 448         generic_publisher_pwr_voltage_create(&robot.gorte, NULL, &robot.gorte);
 449         generic_publisher_pwr_ctrl_create(&robot.gorte, NULL, &robot.gorte);
 450
 451         eb2008_publisher_servos_create(&robot.orte, send_dummy_cb, &robot.orte);
 452         eb2008_publisher_drives_create(&robot.orte, send_dummy_cb, &robot.orte);
 453         eb2008_publisher_laser_cmd_create(&robot.orte, NULL, NULL);
 454
 455         /* create generic subscribers */
 456         generic_subscriber_motion_irc_create(&robot.gorte, rcv_motion_irc_cb, &robot.gorte);
 457         generic_subscriber_motion_speed_create(&robot.gorte, rcv_motion_speed_cb, &robot.gorte);
 458         generic_subscriber_motion_status_create(&robot.gorte, rcv_motion_status_cb, &robot.gorte);
 459         generic_subscriber_joy_data_create(&robot.gorte, rcv_joy_data_cb, &robot.gorte);
 460         generic_subscriber_pwr_voltage_create(&robot.gorte, rcv_pwr_voltage_cb, &robot.gorte);
 461         generic_subscriber_pwr_ctrl_create(&robot.gorte, rcv_pwr_ctrl_cb, &robot.gorte);
 462         generic_subscriber_robot_cmd_create(&robot.gorte, rcv_robot_cmd_cb, &robot.gorte);
 463         generic_subscriber_hokuyo_scan_create(&robot.gorte, rcv_hokuyo_scan_cb, &robot.gorte);
 464
 465         /* create eb2008 subscribers */
 466         eb2008_subscriber_servos_create(&robot.orte, rcv_servos_cb, &robot.orte);
 467         eb2008_subscriber_drives_create(&robot.orte, rcv_drives_cb, &robot.orte);
 468         eb2008_subscriber_laser_data_create(&robot.orte, rcv_laser_data_cb, &robot.orte);
 469         eb2008_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
 470         eb2008_subscriber_bumper_create(&robot.orte, rcv_bumper_cb, &robot.orte);
 471         eb2008_subscriber_sharps_create(&robot.orte, rcv_sharps_cb, &robot.orte);
 472
 473         return rv;
 474 }
 475