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