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  * PUBLISHER CALLBACKS - GENERIC
  25  * ---------------------------------------------------------------------- */
  26
  27 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
  28                         void *sendCallBackParam)
  29 {
  30         struct ref_pos_type *instance = (struct ref_pos_type *)vinstance;
  31
  32         ROBOT_LOCK(ref_pos);
  33         *instance = robot.ref_pos;
  34         ROBOT_UNLOCK(ref_pos);
  35 }
  36
  37 void send_est_pos_cb(const ORTESendInfo *info, void *vinstance,
  38                         void *sendCallBackParam)
  39 {
  40         struct est_pos_type *instance = (struct est_pos_type *)vinstance;
  41
  42         ROBOT_LOCK(est_pos);
  43         *instance = robot.est_pos;
  44         ROBOT_UNLOCK(est_pos);
  45 }
  46
  47 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
  48                         void *sendCallBackParam)
  49 {
  50 }
  51
  52 /* ----------------------------------------------------------------------
  53  * SUBSCRIBER CALLBACKS - GENERIC
  54  * ---------------------------------------------------------------------- */
  55
  56 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
  57                         void *recvCallBackParam)
  58 {
  59         struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
  60         static struct motion_irc_type prevInstance;
  61         static int firstRun = 1;
  62         /* spocitat prevodovy pomer */
  63         double n = (double)(28.0 / 1.0);
  64         /* vzdalenost na pulz - 4.442 je empiricka konstanta :) */
  65         double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
  66
  67         double aktk0 = 0;
  68         double aktk1 = 0;
  69         double deltaU = 0;
  70         double deltAlfa = 0;
  71
  72         switch (info->status) {
  73                 case NEW_DATA:
  74                         if(firstRun) {
  75                                 prevInstance = *instance;
  76                                 firstRun = 0;
  77                                 break;
  78                         }
  79
  80                         aktk0 = ((prevInstance.left - instance->left) >> 8) * c;
  81                         aktk1 = ((instance->right - prevInstance.right) >> 8) * c;
  82                         prevInstance = *instance;
  83
  84                         deltaU = (aktk0 + aktk1) / 2;
  85                         deltAlfa = (aktk1 - aktk0) / (2.0*ROBOT_ROTATION_RADIUS_M);
  86
  87                         struct mcl_robot_odo *odo = malloc(sizeof(struct mcl_robot_odo));
  88                         memset(odo, 0, sizeof(*odo));
  89                         odo->dx = deltaU;
  90                         odo->dy = 0;
  91                         odo->dangle = deltAlfa;
  92                         FSM_SIGNAL(LOC, EV_ODO_RECEIVED, odo);
  93                         robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
  94                         break;
  95                 case DEADLINE:
  96                         robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
  97                         DBG("ORTE deadline occurred - motion_irc receive\n");
  98                         break;
  99         }
 100 }
 101
 102 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 103                         void *recvCallBackParam)
 104 {
 105         switch (info->status) {
 106                 case NEW_DATA:
 107                         break;
 108                 case DEADLINE:
 109                         DBG("ORTE deadline occurred - motion_speed receive\n");
 110                         break;
 111         }
 112 }
 113
 114 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 115                         void *recvCallBackParam)
 116 {
 117         switch (info->status) {
 118                 case NEW_DATA:
 119                         break;
 120                 case DEADLINE:
 121                         DBG("ORTE deadline occurred - motion_status receive\n");
 122                         break;
 123         }
 124 }
 125
 126 void rcv_joy_data_cb(const ORTERecvInfo *info, void *vinstance,
 127                         void *recvCallBackParam)
 128 {
 129         struct joy_data_type *instance = (struct joy_data_type *)vinstance;
 130
 131         switch (info->status) {
 132                 case NEW_DATA:
 133                         ROBOT_LOCK(joy_data);
 134                         robot.joy_data = *instance;
 135                         ROBOT_UNLOCK(joy_data);
 136                         break;
 137                 case DEADLINE:
 138                         DBG("ORTE deadline occurred - joy_data receive\n");
 139                         break;
 140         }
 141 }
 142
 143 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 144                         void *recvCallBackParam)
 145 {
 146         switch (info->status) {
 147                 case NEW_DATA:
 148                         robot.hw_status[STATUS_POWER]=HW_STATUS_OK;
 149                         break;
 150                 case DEADLINE:
 151                         robot.hw_status[STATUS_POWER]=HW_STATUS_FAILED;
 152                         DBG("ORTE deadline occurred - pwr_voltage receive\n");
 153                         break;
 154         }
 155 }
 156
 157 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
 158                         void *recvCallBackParam)
 159 {
 160         switch (info->status) {
 161                 case NEW_DATA:
 162                         break;
 163                 case DEADLINE:
 164                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 165                         break;
 166         }
 167 }
 168
 169 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 170                         void *recvCallBackParam)
 171 {
 172         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 173         static unsigned char start_received = 0;
 174
 175         switch (info->status) {
 176                 case NEW_DATA:
 177                         if (instance->start && !start_received) {
 178                                 FSM_SIGNAL(MAIN, EV_START, NULL);
 179                                 start_received = 1;
 180                         }
 181                         break;
 182                 case DEADLINE:
 183                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 184                         break;
 185         }
 186 }
 187
 188 /* ----------------------------------------------------------------------
 189  * SUBSCRIBER CALLBACKS - EB2008
 190  * ---------------------------------------------------------------------- */
 191
 192 void rcv_servos_cb(const ORTERecvInfo *info, void *vinstance,
 193                         void *recvCallBackParam)
 194 {
 195         switch (info->status) {
 196                 case NEW_DATA:
 197                         break;
 198                 case DEADLINE:
 199                         DBG("ORTE deadline occurred - servos receive\n");
 200                         break;
 201         }
 202 }
 203
 204 void rcv_drives_cb(const ORTERecvInfo *info, void *vinstance,
 205                         void *recvCallBackParam)
 206 {
 207         struct drives_type *instance = (struct drives_type *)vinstance;
 208
 209         switch (info->status) {
 210                 case NEW_DATA:
 211                         ROBOT_LOCK(drives);
 212                         robot.drives = *instance;
 213                         ROBOT_UNLOCK(drives);
 214                         break;
 215                 case DEADLINE:
 216                         DBG("ORTE deadline occurred - drives receive\n");
 217                         break;
 218         }
 219 }
 220
 221 void rcv_laser_data_cb(const ORTERecvInfo *info, void *vinstance,
 222                         void *recvCallBackParam)
 223 {
 224         struct laser_data_type *instance = (struct laser_data_type *)vinstance;
 225
 226         switch (info->status) {
 227                 case NEW_DATA: {
 228                         struct mcl_laser_measurement *meas_angles;
 229                         meas_angles = malloc(sizeof(*meas_angles));
 230                         memset(meas_angles, 0, sizeof(*meas_angles));
 231
 232                         meas_angles->count = 1;
 233                         meas_angles->val[0] =
 234                                 (double)TIME2ANGLE(instance->period,
 235                                                    instance->measure);
 236 //                      DBG("a=%f\n", RAD2DEG(robot.meas_angles.val[0]));
 237
 238                         bool sent;
 239                         FSM_SIGNAL_TIMED(LOC, EV_LASER_RECEIVED, meas_angles, 20/*ms*/, &sent);
 240                         if (!sent) {
 241                                 DBG("%s: MCL is busy - not updating\n", __FUNCTION__);
 242                                 free(meas_angles);
 243                         }
 244                         robot.hw_status[STATUS_LASER] = HW_STATUS_OK;
 245                         break;
 246                 }
 247                 case DEADLINE:
 248                         robot.hw_status[STATUS_LASER] = HW_STATUS_FAILED;
 249                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 250                         break;
 251         }
 252 }
 253
 254 void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
 255                         void *recvCallBackParam)
 256 {
 257         struct cmu_type *instance = (struct cmu_type *)vinstance;
 258         static enum ball_color last_color = NO_BALL;
 259         static unsigned char first = 1;
 260
 261         switch (info->status) {
 262                 case NEW_DATA: {
 263                         ROBOT_LOCK(cmu);
 264                         robot.cmu = *instance;
 265                         ROBOT_UNLOCK(cmu);
 266                         if (first) {
 267                                 last_color = robot.cmu.color;
 268                                 first = 0;
 269                         }
 270                         if (robot.cmu.color != NO_BALL) {
 271                                 if (last_color != robot.cmu.color) {
 272                                         last_color = robot.cmu.color;
 273                                         FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
 274                                 }
 275                         }
 276                         robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
 277                         break;
 278                 }
 279                 case DEADLINE:
 280                         robot.hw_status[STATUS_CMU] = HW_STATUS_FAILED;
 281                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 282                         break;
 283         }
 284 }
 285
 286 void rcv_bumper_cb(const ORTERecvInfo *info, void *vinstance,
 287                         void *recvCallBackParam)
 288 {
 289         struct bumper_type *instance = (struct bumper_type *)vinstance;
 290
 291         switch (info->status) {
 292                 case NEW_DATA: {
 293                         ROBOT_LOCK(bumper);
 294                         robot.bumper = *instance;
 295                         ROBOT_UNLOCK(bumper);
 296                         break;
 297                 }
 298                 case DEADLINE:
 299                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 300                         break;
 301         }
 302 }
 303
 304 void rcv_sharps_cb(const ORTERecvInfo *info, void *vinstance,
 305                         void *recvCallBackParam)
 306 {
 307         struct sharps_type *instance = (struct sharps_type *)vinstance;
 308         //struct sharps_type *for_mcl;
 309
 310         switch (info->status) {
 311                 case NEW_DATA: {
 312                         update_map(instance);
 313                         break;
 314                 }
 315                 case DEADLINE:
 316                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 317                         break;
 318         }
 319 }
 320
 321 void robot_init_orte()
 322 {
 323         robot.orte.strength = 20;
 324         robot.gorte.strength = 20;
 325
 326         eb2008_roboorte_init(&robot.orte);
 327         generic_roboorte_init(&robot.gorte);
 328
 329         /* creating publishers */
 330         generic_publisher_motion_irc_create(&robot.gorte, NULL, &robot.gorte);
 331         generic_publisher_motion_speed_create(&robot.gorte, NULL, &robot.gorte);
 332         generic_publisher_motion_status_create(&robot.gorte, NULL, &robot.gorte);
 333         generic_publisher_ref_pos_create(&robot.gorte, send_ref_pos_cb, &robot.gorte);
 334         generic_publisher_est_pos_create(&robot.gorte, send_est_pos_cb, &robot.gorte);
 335         generic_publisher_joy_data_create(&robot.gorte, NULL, &robot.gorte);
 336         generic_publisher_pwr_voltage_create(&robot.gorte, NULL, &robot.gorte);
 337         generic_publisher_pwr_ctrl_create(&robot.gorte, NULL, &robot.gorte);
 338
 339         eb2008_publisher_servos_create(&robot.orte, send_dummy_cb, &robot.orte);
 340         eb2008_publisher_drives_create(&robot.orte, send_dummy_cb, &robot.orte);
 341
 342         /* create generic subscribers */
 343         generic_subscriber_motion_irc_create(&robot.gorte, rcv_motion_irc_cb, &robot.gorte);
 344         generic_subscriber_motion_speed_create(&robot.gorte, rcv_motion_speed_cb, &robot.gorte);
 345         generic_subscriber_motion_status_create(&robot.gorte, rcv_motion_status_cb, &robot.gorte);
 346         generic_subscriber_joy_data_create(&robot.gorte, rcv_joy_data_cb, &robot.gorte);
 347         generic_subscriber_pwr_voltage_create(&robot.gorte, rcv_pwr_voltage_cb, &robot.gorte);
 348         generic_subscriber_pwr_ctrl_create(&robot.gorte, rcv_pwr_ctrl_cb, &robot.gorte);
 349         generic_subscriber_robot_cmd_create(&robot.gorte, rcv_robot_cmd_cb, &robot.gorte);
 350
 351         /* create eb2008 subscribers */
 352         eb2008_subscriber_servos_create(&robot.orte, rcv_servos_cb, &robot.orte);
 353         eb2008_subscriber_drives_create(&robot.orte, rcv_drives_cb, &robot.orte);
 354         eb2008_subscriber_laser_data_create(&robot.orte, rcv_laser_data_cb, &robot.orte);
 355         eb2008_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
 356         eb2008_subscriber_bumper_create(&robot.orte, rcv_bumper_cb, &robot.orte);
 357         eb2008_subscriber_sharps_create(&robot.orte, rcv_sharps_cb, &robot.orte);
 358 }
 359