Statistics
| Branch: | Revision:

root / scout / libscout / src / behaviors / Odometry.h @ ddfeb111

History | View | Annotate | Download (1.22 KB)

1 f79fbce2 Priya
#ifndef _ODOMETRY_H_
2
#define _ODOMETRY_H_
3
4
#include "../Behavior.h"
5 d140fd71 Yuyang
#include "../Sensors.h"
6 30a3768e Priya
#include "messages/ScoutPosition.h"
7 f79fbce2 Priya
8
9 ddfeb111 Priya
#define WHEEL_RADIUS  .2
10 f79fbce2 Priya
#define WHEEL_CIRCUM  (2*M_PI*WHEEL_RADIUS)
11 ddfeb111 Priya
#define WHEEL_BASE    1.2
12
#define ENCODER_COUNT (2*WHEEL_RADIUS*350*M_PI/WHEEL_BASE)
13 f79fbce2 Priya
#define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT)
14
15
typedef struct{
16
  float x;
17
  float y;
18 31be19a6 Priya
  float theta;
19 f79fbce2 Priya
} pos;
20
21
class Odometry : Behavior{
22
23
  public:
24
25
  /** Set up the odometry node and prepare communcations over ROS */
26 d140fd71 Yuyang
  Odometry(std::string scoutname, Sensors* sensors);
27 f79fbce2 Priya
28
  /** Query encoders and estimate position based on encoder reading */
29
  void get_position();
30
31
  /** Gets scout position and prints to screen */
32
  void run();
33
  
34
  private:
35
36
    /** ROS publisher and client declaration */
37
    ros::NodeHandle node;
38 30a3768e Priya
    ros::Publisher scout_position;
39 f79fbce2 Priya
    ros::ServiceClient query_encoders_client;
40 30a3768e Priya
    messages::ScoutPosition position;
41
42
    std::string name;
43 f79fbce2 Priya
44
    float msg_time_in;
45 31be19a6 Priya
46 3db79f25 Priya
    int motor_fl_dist;
47
    int motor_fr_dist;
48
    int motor_bl_dist;
49
    int motor_br_dist;
50 f79fbce2 Priya
51 3db79f25 Priya
    unsigned int motor_fl_ticks;
52
    unsigned int motor_fr_ticks;
53
    unsigned int motor_bl_ticks;
54
    unsigned int motor_br_ticks;
55 31be19a6 Priya
    float scout_theta;
56
57 f79fbce2 Priya
    pos* scout_pos;
58
};
59
60
#endif