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