src/robofsm/robot.h

   1 /**
   2  * robot.h          08/04/20
   3  *
   4  * Robot's data structures for the Eurobot 2008.
   5  *
   6  * Copyright: (c) 2008 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 #ifndef ROBOT_H
  12 #define ROBOT_H
  13
  14 #include <stdint.h>
  15 #include <stdio.h>
  16 #include <trgenconstr.h>
  17 #include <robottype.h>
  18 #include <robottype.h>
  19 #include <roboorte_robottype.h>
  20 #include <robodim.h>
  21 #include <roboevent.h>
  22 #include <fsm.h>
  23 #include <robot_config.h>
  24 //#include <ul_log.h>
  25
  26 //UL_LOG_CUST(ulogd_robot_h); /* Log domain name = ulogd + name of the file */
  27
  28 /**
  29  * Competition parameters
  30  */
  31 enum team_color {
  32         TC_WHITE = 0,       /* Coordinate transformation does not apply */
  33         TC_GREEN,
  34         TC_YELLOW
  35 };
  36
  37 enum robot_start_state {
  38     POWER_ON = 0,
  39     START_PLUGGED_IN,
  40     COMPETITION_STARTED,
  41 };
  42
  43 /**
  44  * FSM
  45  */
  46
  47 /* Mapping from FSM ID to field in robot.fsm */
  48 #define ROBOT_FSM_MAIN main
  49 #define ROBOT_FSM_MOTION motion
  50 #define ROBOT_FSM_DISPLAY display
  51 #define ROBOT_FSM_ACT act
  52
  53 #define FSM_GET_BY_ID(fsm_id) (&robot.fsm.ROBOT_FSM_##fsm_id)
  54
  55 /**
  56  * LOCKING MANIPULATION
  57  */
  58
  59 #ifdef CONFIG_LOCK_CHECKING
  60 #define LOCK_CHECK_COUNT 10
  61 struct lock_log {
  62     pthread_mutex_t *locked[LOCK_CHECK_COUNT];
  63     int idx;
  64 };
  65
  66 extern struct lock_log robot_lock_log;
  67
  68 #define __LOCK_CHECK(mutex) robot_lock_log.locked[robot_lock_log.idx++] = mutex;
  69 #define __UNLOCK_CHECK(mutex)                       \
  70     if (robot_lock_log.locked[--robot_lock_log.idx] != mutex || \
  71         robot_lock_log.idx < 0)                 \
  72         ul_logerr("!!! Locking error %s:%d\n", __FILE__, __LINE__);
  73 #else
  74 #define __LOCK_CHECK(mutex)
  75 #define __UNLOCK_CHECK(mutex)
  76 #endif
  77 /**
  78  * Locks the robot structure.
  79  * @param var Field in the structure you are going to work with.
  80  */
  81 #define ROBOT_LOCK(var)                          \
  82     do {                             \
  83         pthread_mutex_lock(&robot.__robot_lock_##var);   \
  84         __LOCK_CHECK(&robot.__robot_lock_##var);     \
  85     } while(0)
  86
  87 /**
  88  * Unlocks the robot structure.
  89  * @param var Field in the structure, which was locked by ROBOT_LOCK.
  90  */
  91 #define ROBOT_UNLOCK(var)                       \
  92     do {                                \
  93         __UNLOCK_CHECK(&robot.__robot_lock_##var);      \
  94         pthread_mutex_unlock(&robot.__robot_lock_##var);    \
  95     } while(0)
  96
  97 /* Mapping of robot structure fields to locks */
  98 //#define __robot_lock_ lock    /* ROBOT_LOCK() */
  99 #define __robot_lock_ref_pos            lock_ref_pos
 100 #define __robot_lock_est_pos_odo        lock_est_pos_odo
 101 #define __robot_lock_est_pos_indep_odo  lock_est_pos_indep_odo
 102 #define __robot_lock_joy_data           lock_joy_data
 103 #define __robot_lock_meas_angles        lock_meas_angles
 104 #define __robot_lock_drives             lock
 105 #define __robot_lock_sharps             lock
 106 #define __robot_lock_hokuyo             lock
 107 #define __robot_lock_sick               lock
 108 #define __robot_lock_sick551            lock
 109 #define __robot_lock_cmu                lock
 110 #define __robot_lock_bumper             lock
 111 #define __robot_lock_disp               lock_disp
 112 #define __robot_lock_motion_irc         lock
 113 #define __robot_lock_odo_data           lock
 114 #define __robot_lock_corr_distances     lock
 115 #define __robot_lock_camera_result      lock_camera
 116
 117 enum robot_status {
 118     STATUS_OK,
 119     STATUS_WARNING,
 120     STATUS_FAILED,
 121 };
 122
 123 enum robot_component {
 124         COMPONENT_MOTOR,
 125         COMPONENT_ODOMETRY,
 126         COMPONENT_CAMERA,
 127         COMPONENT_POWER,
 128         COMPONENT_HOKUYO,
 129         COMPONENT_SICK,
 130         COMPONENT_SICK551,
 131         COMPONENT_START,
 132         COMPONENT_JAWS,
 133         COMPONENT_LIFT,
 134
 135         ROBOT_COMPONENT_NUMBER
 136 };
 137
 138 /* robot's main data structure */
 139 struct robot {
 140     pthread_mutex_t lock;
 141     pthread_mutex_t lock_ref_pos;
 142     pthread_mutex_t lock_est_pos_odo;
 143     pthread_mutex_t lock_est_pos_indep_odo;
 144     pthread_mutex_t lock_meas_angles;
 145     pthread_mutex_t lock_joy_data;
 146     pthread_mutex_t lock_disp;
 147     pthread_mutex_t lock_camera;
 148
 149     /* competition parameters */
 150         enum team_color team_color;
 151
 152     /** State variable used for controlling the robot by pluggin
 153      * in and out start connector. */
 154     enum robot_start_state start_state;
 155
 156     /** Temporary storage for new trajectory. After the trajectory creation is
 157      * finished, this trajectory is submitted to fsmmove. */
 158     void *new_trajectory;
 159
 160     unsigned char isTrajectory;
 161     sem_t start;
 162
 163     struct fsm_main_loop main_loop;
 164
 165     /* partial FSMs */
 166     struct {
 167         struct robo_fsm main;
 168         struct robo_fsm motion;
 169         struct robo_fsm act;
 170     } fsm;
 171
 172     /* actual position */
 173     struct robot_pos_type ref_pos;
 174     /* estimated position */
 175     struct robot_pos_type est_pos_odo;
 176     struct robot_pos_type est_pos_indep_odo;
 177
 178     /** True if est_pos_odo is updated according to reception of motion_irc */
 179     bool odometry_works;
 180     /** True if est_pos_indep_odo is updated according to reception of motion_indep_odo */
 181     bool indep_odometry_works;
 182
 183     bool use_back_bumpers;
 184     bool use_left_bumper;
 185     bool use_right_bumper;
 186
 187     /** True iff at least one wheel rotates backward */
 188     bool moves_backward;
 189
 190     /*
 191      * sometimes H8S does not send queried odometry
 192      * following flag has been added for EKF estimator,
 193      * since is has been hardly disturbed by missing measurement
 194      * (taken as sudden zero velocities)
 195      */
 196     bool motion_irc_received;
 197
 198     /* orte */
 199     struct robottype_orte_data orte;
 200
 201     /* sensors */
 202     struct motion_irc_type motion_irc;  /* motor odometry */
 203     struct odo_data_type odo_data;      /* independent odometry */
 204     struct corr_distances_type corr_distances;  /* ultrasound */
 205
 206         struct lidar_scan_type hokuyo;
 207         bool ignore_hokuyo;
 208
 209         struct lidar_scan_type sick;
 210         bool ignore_sick;
 211
 212         struct lidar_scan_type sick551;
 213         bool ignore_sick551;
 214
 215         /* variables for target detection */
 216         bool target_loaded;
 217         bool target_valid;
 218         int target_ang;
 219         char target_color[3];
 220
 221     struct map *map;    /* Map for pathplanning (no locking) */
 222
 223     enum robot_status status[ROBOT_COMPONENT_NUMBER];
 224
 225     char corns_conf_center;
 226     char corns_conf_side;
 227     struct corns_group *corns;
 228
 229     bool obstacle_avoidance_enabled;
 230
 231     /** is set to true in separate thread when there is short time left */
 232     bool short_time_to_end;
 233     bool check_turn_safety;
 234
 235     float odo_cal_b;
 236     float odo_cal_a;
 237     float odo_distance_a;
 238     float odo_distance_b;
 239 }; /* robot */
 240
 241 extern struct robot robot;
 242
 243 #ifdef __cplusplus
 244 extern "C" {
 245 #endif
 246
 247 int robot_init() __attribute__ ((warn_unused_result));
 248 int robot_start() __attribute__ ((warn_unused_result));
 249 void robot_exit();
 250 void robot_destroy();
 251 void robot_get_est_pos_trans(double *x, double *y, double *phi);
 252 void robot_get_est_pos(double *x, double *y, double *phi);
 253
 254 /* Hack to easily disable display if it is not configured */
 255 void serial_comm(int status);
 256
 257
 258
 259 FSM_STATE_FULL_DECL(main, init);
 260 FSM_STATE_FULL_DECL(motion, init);
 261 FSM_STATE_FULL_DECL(disp, init);
 262 FSM_STATE_FULL_DECL(act, wait_for_command);
 263
 264 #ifdef __cplusplus
 265 }
 266 #endif
 267
 268 /*Thread priorities*/
 269 #define THREAD_PRIO_TRAJ_FOLLOWER 90    /* As high as possible */
 270 #define THREAD_PRIO_TRAJ_RECLAC 18
 271 #define OBST_FORGETING_PRIO 17  /* Priority of the thread for forgeting detected obstacles. */
 272
 273
 274
 275 #endif  /* ROBOT_H */