Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (1.09 KB)

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