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