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