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