Statistics
| Branch: | Revision:

root / scout / libscout / src / behaviors / Odometry.h @ 3db79f25

History | View | Annotate | Download (1.07 KB)

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