src/robofsm/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_robottype.h>
  13 #include <roboorte.h>
  14 #include <robodata.h>
  15 #include <robot.h>
  16 #include <movehelper.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_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 128                         void *recvCallBackParam)
 129 {
 130         switch (info->status) {
 131                 case NEW_DATA:
 132                         robot.hw_status[STATUS_POWER]=HW_STATUS_OK;
 133                         break;
 134                 case DEADLINE:
 135                         robot.hw_status[STATUS_POWER]=HW_STATUS_FAILED;
 136                         DBG("ORTE deadline occurred - pwr_voltage receive\n");
 137                         break;
 138         }
 139 }
 140
 141 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
 142                         void *recvCallBackParam)
 143 {
 144         switch (info->status) {
 145                 case NEW_DATA:
 146                         break;
 147                 case DEADLINE:
 148                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 149                         break;
 150         }
 151 }
 152
 153 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 154                         void *recvCallBackParam)
 155 {
 156         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 157         static struct robot_cmd_type last_instance;
 158         enum robot_state state;
 159
 160         switch (info->status) {
 161                 case NEW_DATA:
 162                         /* Stupid way of controlling the robot by
 163                          * pluggin in and out start connector. */
 164                         if (instance->start == last_instance.start)
 165                                 break;
 166
 167                         ROBOT_LOCK(state);
 168                         state = robot.state;
 169                         ROBOT_UNLOCK(state);
 170                         printf("in state=%d \n", state);
 171
 172                         switch (state) {
 173                                 case JUST_STARTED:
 174                                         /* On first plug-in laser is tarted */
 175                                         if (instance->start)
 176                                                 break;
 177                                         FSM_SIGNAL(MAIN, EV_LASER_POWER, NULL);
 178                                         state = LASER_STARTED;
 179                                         break;
 180
 181                                 case LASER_STARTED:
 182                                         /* Competition starts when plugged out */
 183                                         if (!instance->start)
 184                                                 break;
 185                                         FSM_SIGNAL(MAIN, EV_START, NULL);
 186                                         state = COMPETITION_STARTED;
 187                                         break;
 188
 189                                 case COMPETITION_STARTED:
 190                                         /* Subsequent plug-in stops the robot */
 191                                         if (!instance->start) {
 192                                                 robot_exit();
 193                                         }
 194                                         break;
 195                         }
 196                         last_instance = *instance;
 197                         printf("out state=%d \n", state);
 198                         ROBOT_LOCK(state);
 199                         robot.state = state;
 200                         ROBOT_UNLOCK(state);
 201                         break;
 202                 case DEADLINE:
 203                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 204                         break;
 205         }
 206 }
 207
 208 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 209                         void *recvCallBackParam)
 210 {
 211         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 212
 213         switch (info->status) {
 214                 case NEW_DATA: {
 215                         ROBOT_LOCK(hokuyo);
 216                         robot.hokuyo = *instance;
 217                         ROBOT_UNLOCK(hokuyo);
 218                         update_map_hokuyo(instance);
 219                         break;
 220                 }
 221                 case DEADLINE:
 222                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 223                         break;
 224         }
 225 }
 226 /* ----------------------------------------------------------------------
 227  * SUBSCRIBER CALLBACKS - EB2008
 228  * ---------------------------------------------------------------------- */
 229
 230 void rcv_actuator_status_cb(const ORTERecvInfo *info, void *vinstance,
 231                             void *recvCallBackParam)
 232 {
 233         switch (info->status) {
 234                 case NEW_DATA:
 235                         if (robot.orte.actuator_status.status == 0) {
 236                                 robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_OK;
 237                         } else {
 238                                 robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_WARNING;
 239                         }
 240                         // robot.orte.actuator_status.lift_pos
 241                         // robot.orte.actuator_status.pusher_pos
 242                         //FSM_SIGNAL(ACT, STATUS_RCVD, NULL);
 243                         break;
 244                 case DEADLINE:
 245                         robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
 246                         DBG("ORTE deadline occurred - servos receive\n");
 247                         break;
 248         }
 249 }
 250
 251 void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
 252                             void *recvCallBackParam)
 253 {
 254         switch (info->status) {
 255                 case NEW_DATA:
 256                         // FIXME (?) inform display in case of invalid data?
 257                         break;
 258                 case DEADLINE:
 259                         // FIXME (?) limited space on the display...
 260                         //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
 261                         DBG("ORTE deadline occurred - servos receive\n");
 262                         break;
 263         }
 264 }
 265
 266 #if 0
 267 void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
 268                         void *recvCallBackParam)
 269 {
 270         struct cmu_type *instance = (struct cmu_type *)vinstance;
 271         static enum ball_color last_color = NO_BALL;
 272         static unsigned char first = 1;
 273
 274         switch (info->status) {
 275                 case NEW_DATA: {
 276                         ROBOT_LOCK(cmu);
 277                         robot.cmu = *instance;
 278                         ROBOT_UNLOCK(cmu);
 279                         if (first) {
 280                                 last_color = robot.cmu.color;
 281                                 first = 0;
 282                         }
 283                         if (robot.cmu.color != NO_BALL) {
 284                                 if (last_color != robot.cmu.color) {
 285                                         last_color = robot.cmu.color;
 286                                         FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
 287                                 }
 288                         }
 289                         robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
 290                         break;
 291                 }
 292                 case DEADLINE:
 293                         robot.hw_status[STATUS_CMU] = HW_STATUS_FAILED;
 294                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 295                         break;
 296         }
 297 }
 298 #endif
 299
 300 #define HIST_CNT 5
 301
 302 static int cmp_double(const void *v1, const void *v2)
 303 {
 304         const double *d1 = v1, *const d2 = v2;
 305         if (d1 < d2)
 306                 return -1;
 307         else if (d1 > d2)
 308                 return +1;
 309         else
 310                 return 0;
 311 }
 312
 313 int robot_init_orte()
 314 {
 315         int rv = 0;
 316
 317         robot.orte.strength = 20;
 318
 319         rv = robottype_roboorte_init(&robot.orte);
 320         if (rv)
 321                 return rv;
 322
 323         /* creating publishers */
 324         robottype_publisher_motion_irc_create(&robot.orte, NULL, &robot.orte);
 325         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 326         robottype_publisher_motion_status_create(&robot.orte, NULL, &robot.orte);
 327         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 328         robottype_publisher_est_pos_create(&robot.orte, send_est_pos_cb, &robot.orte);
 329         robottype_publisher_pwr_voltage_create(&robot.orte, NULL, &robot.orte);
 330         robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 331
 332         robottype_publisher_lift_create(&robot.orte, send_dummy_cb, &robot.orte);
 333         robottype_publisher_pusher_create(&robot.orte, send_dummy_cb, &robot.orte);
 334         robottype_publisher_chelae_create(&robot.orte, send_dummy_cb, &robot.orte);
 335         robottype_publisher_belts_create(&robot.orte, send_dummy_cb, &robot.orte);
 336         robottype_publisher_picker_create(&robot.orte, send_dummy_cb, &robot.orte); /* FIXME temporary till LPCs not ready */
 337         robottype_publisher_holder_create(&robot.orte, send_dummy_cb, &robot.orte);
 338
 339         /* create generic subscribers */
 340         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 341         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 342         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 343         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 344         robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 345         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 346         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 347
 348         /* create subscribers */
 349         robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte);
 350         robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
 351         //robottype_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
 352
 353         return rv;
 354 }
 355