src/boostFSM/robot.h

   1 #ifndef ROBOT_H
   2 #define ROBOT_H
   3 #include <stdint.h>
   4 #include <stdio.h>
   5 #include <trgenconstr.h>
   6 #include <robottype.h>
   7 #include <robottype.h>
   8 #include <roboorte_robottype.h>
   9 #include <robodim.h>
  10 #include <roboevent.h>
  11 #include <fsm.h>
  12 #include <robot_config.h>
  13 #include <actuators.h>
  14 #include <semaphore.h>
  15 #include "movehelper.h"
  16 #include "scheduler.hpp"
  17 //#include <ul_log.h>
  18
  19 //UL_LOG_CUST(ulogd_robot_h); /* Log domain name = ulogd + name of the file */
  20 #include <guard.hpp>
  21 /**
  22  * Competition parameters
  23  */
  24 enum team_colour {
  25         VIOLET = 0,             /* Coordinate transformation does not apply */
  26         RED                     /* Coordinate transformation applies (in *_trans() functions) */
  27 };
  28
  29 enum robot_start_state {
  30         POWER_ON = 0,
  31         START_PLUGGED_IN,
  32         COMPETITION_STARTED,
  33 };
  34
  35 enum robot_status {
  36         STATUS_OK,
  37         STATUS_WARNING,
  38         STATUS_FAILED,
  39 };
  40
  41 enum robot_component {
  42         COMPONENT_MOTOR,
  43         COMPONENT_ODOMETRY,
  44         COMPONENT_CAMERA,
  45         COMPONENT_POWER,
  46         COMPONENT_HOKUYO,
  47         COMPONENT_START,
  48         COMPONENT_JAWS,
  49         COMPONENT_LIFT,
  50
  51         ROBOT_COMPONENT_NUMBER
  52 };
  53
  54 enum FSM {
  55         MAIN,
  56         MOT
  57 };
  58
  59 class Robot
  60 {
  61         void int_handler(int sig);
  62         void block_signals();
  63         //static void trans_callback(struct robo_fsm *fsm) publication stringnames of states
  64     public:
  65         pthread_mutex_t lock;
  66         pthread_mutex_t lock_ref_pos;
  67         pthread_mutex_t lock_est_pos_odo;
  68         pthread_mutex_t lock_est_pos_indep_odo;
  69         pthread_mutex_t lock_meas_angles;
  70         pthread_mutex_t lock_joy_data;
  71         pthread_mutex_t lock_disp;
  72         pthread_mutex_t lock_camera;
  73         /* competition parameters */
  74         team_colour team_color;
  75
  76         /** State variable used for controlling the robot by pluggin
  77          * in and out start connector. */
  78         robot_start_state start_state;
  79
  80         /* actual position */
  81         robot_pos_type ref_pos;
  82         /* estimated position */
  83         robot_pos_type est_pos_odo;
  84         robot_pos_type est_pos_indep_odo;
  85         /** True if est_pos_odo is updated according to reception of motion_irc */
  86         bool odometry_works;
  87         /** True if est_pos_indep_odo is updated according to reception of motion_indep_odo */
  88         bool indep_odometry_works;
  89
  90         bool use_back_bumpers;
  91         bool use_left_bumper;
  92         bool use_right_bumper;
  93
  94         /** True iff at least one wheel rotates backward */
  95         bool moves_backward;
  96
  97         /*
  98          * sometimes H8S does not send queried odometry
  99          * following flag has been added for EKF estimator,
 100          * since is has been hardly disturbed by missing measurement
 101          * (taken as sudden zero velocities)
 102          */
 103         bool motion_irc_received;
 104
 105         /* orte */
 106         struct robottype_orte_data orte;
 107
 108         /* sensors */
 109         struct motion_irc_type motion_irc;      /* motor odometry */
 110         struct odo_data_type odo_data;          /* independent odometry */
 111         struct corr_distances_type corr_distances;      /* ultrasound */
 112
 113         hokuyo_scan_type hokuyo;
 114         bool ignore_hokuyo;
 115
 116         //struct map *map;      /* Map for pathplanning (no locking) */
 117
 118         robot_status status[ROBOT_COMPONENT_NUMBER];
 119
 120         char corns_conf_center;
 121         char corns_conf_side;
 122         struct corns_group *corns;
 123
 124         bool obstacle_avoidance_enabled;
 125
 126         /** is set to true in separate thread when there is short time left */
 127         bool short_time_to_end;
 128         bool check_turn_safety;
 129
 130         float odo_cal_b;
 131         float odo_cal_a;
 132         float odo_distance_a;
 133         float odo_distance_b;
 134         Actuators act;
 135         Scheduler sched;
 136         /* State machines */
 137         Scheduler::processor_handle MAIN;
 138         Scheduler::processor_handle MOTION;
 139         /* Class methods */
 140         Robot();
 141         ~Robot();
 142         int init() __attribute__ ((warn_unused_result));
 143         int start() __attribute__ ((warn_unused_result));
 144         void robot_exit();
 145         void destroy();
 146         void set_est_pos_trans(double x, double y, double phi){set_est_pos(trans_x(x), trans_y(y), trans_angle(phi));}
 147         void set_est_pos(double x, double y, double phi);
 148         void get_est_pos_trans(double &x, double &y, double &phi);
 149         void get_est_pos(double &x, double &y, double &phi);
 150 };
 151
 152 extern Robot robot;
 153
 154 #endif