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