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