src/robofsm/eb2008/robot_eb2008.h

   1 /**
   2  * robot_eb2008.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_EB2008_H
  12 #define ROBOT_EB2008_H
  13
  14 #include <stdint.h>
  15 #include <stdio.h>
  16 #include <mcl_laser.h>
  17 #include <trgenconstr.h>
  18 #include <robottype.h>
  19 #include <robottype_eb2008.h>
  20 #include <roboorte_generic.h>
  21 #include <roboorte_eb2008.h>
  22 #include <robodim_eb2008.h>
  23 #include <roboevent_eb2008.h>
  24 #include <fsm.h>
  25 #include <servos_eb2008.h>
  26 #include <robot_config.h>
  27
  28 #define FSM_STATE_NAME_MAX_LEN 40
  29
  30 /* Select robot color. If we are BLUE, no coordinate transformation will
  31  * be performed. */
  32 // #define WE_ARE_RED
  33 #define WE_ARE_BLUE
  34
  35 /**
  36  * Competition parameters
  37  */
  38 enum team_color {
  39         BLUE = 0,
  40         RED
  41 };
  42
  43 enum ball_color {
  44         NO_BALL = 0,
  45         BLUE_BALL,
  46         RED_BALL,
  47         WHITE_BALL
  48 };
  49
  50 /**
  51  * FSM
  52  */
  53 /* amount of FSM */
  54 #define FSM_CNT         5
  55
  56 /* FSM ids */
  57 enum robot_fsm_id {
  58         FSM_ID_MAIN     = 0,
  59         FSM_ID_MOTION   = 1,
  60         FSM_ID_LOC      = 2,
  61         FSM_ID_JOY      = 3,
  62         FSM_ID_DISP     = 4
  63 };
  64
  65 #define LAS_CNT         12      /* dont forget to change CRA!! */
  66 /* index of information in times array */
  67 #define LAS_MEAS_CNTI   0       /* counter index of measured times */
  68 #define LAS_MEAS_PERI   1       /* period index */
  69 #define LAS_MEAS_DATAI  2       /* data index */
  70
  71 /**
  72  * LOCKING MANIPULATION
  73  */
  74
  75 #ifdef CONFIG_LOCK_CHECKING
  76 #define LOCK_CHECK_COUNT 10
  77 struct lock_log {
  78         pthread_mutex_t *locked[LOCK_CHECK_COUNT];
  79         int idx;
  80 };
  81
  82 extern struct lock_log robot_lock_log;
  83
  84 #define __LOCK_CHECK(mutex) robot_lock_log.locked[robot_lock_log.idx++] = mutex;
  85 #define __UNLOCK_CHECK(mutex)                                           \
  86         if (robot_lock_log.locked[--robot_lock_log.idx] != mutex ||     \
  87             robot_lock_log.idx < 0)                                     \
  88                 printf("!!! Locking error %s:%d\n", __FILE__, __LINE__);
  89 #else
  90 #define __LOCK_CHECK(mutex)
  91 #define __UNLOCK_CHECK(mutex)
  92 #endif
  93 /**
  94  * Locks the robot structure.
  95  * @param var Field in the structure you are going to work with.
  96  */
  97 #define ROBOT_LOCK(var)                                          \
  98         do {                                                     \
  99                 pthread_mutex_lock(&robot.__robot_lock_##var);   \
 100                 __LOCK_CHECK(&robot.__robot_lock_##var);         \
 101         } while(0)
 102
 103 /**
 104  * Unlocks the robot structure.
 105  * @param var Field in the structure, which was locked by ROBOT_LOCK.
 106  */
 107 #define ROBOT_UNLOCK(var)                                               \
 108         do {                                                            \
 109                 __UNLOCK_CHECK(&robot.__robot_lock_##var);              \
 110                 pthread_mutex_unlock(&robot.__robot_lock_##var);        \
 111         } while(0)
 112
 113 /* Mapping of robot structure fields to locks */
 114 //#define __robot_lock_ lock    /* ROBOT_LOCK() */
 115 #define __robot_lock_ref_pos            lock_ref_pos
 116 #define __robot_lock_est_pos            lock_est_pos
 117 #define __robot_lock_joy_data           lock_joy_data
 118 #define __robot_lock_meas_angles        lock_meas_angles
 119 #define __robot_lock_drives             lock
 120 #define __robot_lock_sharps             lock
 121 #define __robot_lock_cmu                lock
 122 #define __robot_lock_bumper             lock
 123 #define __robot_lock_drives             lock
 124 #define __robot_lock_disp               lock_disp
 125
 126 /* robot's main data structure */
 127 struct robot_eb2008 {
 128         long long cnt;
 129         unsigned char   mode;
 130         pthread_mutex_t lock;
 131         pthread_mutex_t lock_ref_pos;
 132         pthread_mutex_t lock_est_pos;
 133         pthread_mutex_t lock_meas_angles;
 134         pthread_mutex_t lock_joy_data;
 135         pthread_mutex_t lock_disp;
 136
 137         /* competition parameters */
 138         unsigned char team_color;
 139         unsigned char beacon_color;
 140
 141         #define CAROUSEL_SIZE 1
 142         #define BALL_TO_COLLECT 1
 143         enum ball_color carousel[CAROUSEL_SIZE];
 144         unsigned char carousel_cnt; /* The number of balls in carousel */
 145         unsigned char carousel_pos; /* required position of the carousel */
 146
 147         /** Temporary storage for new trajectory. After the trajectory creation is
 148          * finished, this trajectory is submitted to fsmmove. */
 149         void *new_trajectory;
 150
 151         unsigned char isTrajectory;
 152         unsigned char start;
 153
 154         /* partial FSMs */
 155         struct robo_fsm fsm[FSM_CNT];
 156
 157         /* actual position */
 158         struct ref_pos_type ref_pos;
 159         /* estimated position */
 160         struct est_pos_type est_pos;
 161
 162         struct motion_irc_type odometry;
 163         struct joy_data_type joy_data;
 164         struct drives_type drives;
 165
 166         /* orte */
 167         struct eb2008_orte_data orte;
 168         struct generic_orte_data gorte;
 169
 170         /* sensors */
 171         struct sharps_type sharps;
 172         struct bumper_type bumper;
 173         struct cmu_type cmu;
 174
 175         /* mcl */
 176         struct mcl_model *mcl;  /* Pointer to the generic MCL model */
 177         struct mcl_laser laser; /* MCL implementation for laser */
 178
 179         struct map *map;        /* Map for pathplanning (no locking) */
 180
 181         struct sercom_data *sercom; /* Sercom for display */
 182
 183         /* information about HW units status - for display */
 184         uint8_t hw_status[6];
 185
 186         /* contains name of current state of main fsm, for display*/
 187         char act_fsm_state_name[FSM_STATE_NAME_MAX_LEN];
 188
 189         bool obstacle_avoidance_enabled;
 190 }; /* robot_eb2008 */
 191
 192 extern struct robot_eb2008 robot;
 193
 194 #ifdef __cplusplus
 195 extern "C" {
 196 #endif
 197
 198 int robot_init();
 199 int robot_start();
 200 int robot_exit();
 201 int robot_wait();
 202 int robot_destroy();
 203
 204 /* Hack to easily disable display if it is not configured */
 205 void serial_comm(int status);
 206
 207
 208
 209 FSM_STATE_FULL_DECL(main, init);
 210 FSM_STATE_FULL_DECL(motion, init);
 211 FSM_STATE_FULL_DECL(loc, init);
 212 FSM_STATE_FULL_DECL(joy, init);
 213 FSM_STATE_FULL_DECL(disp, init);
 214
 215 #ifdef __cplusplus
 216 }
 217 #endif
 218
 219 /*Thread priorities*/
 220 #define THREAD_PRIO_TRAJ_FOLLOWER 255   /* As high as possible */
 221 #define THREAD_PRIO_MOTION      30
 222 #define THREAD_PRIO_MAIN        25
 223 #define THREAD_PRIO_LOC         20
 224 #define THREAD_PRIO_TRAJ_RECLAC 18
 225 #define OBST_FORGETING_PRIO 17  /* Priority of the thread for forgeting detected obstacles. */
 226 #define THREAD_PRIO_DISP        15
 227 #define THREAD_PRIO_JOY         10
 228
 229
 230 #endif  /* ROBOT_EB2008_H */