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