root / scout / libscout / src / behaviors / Odometry.h @ 16d4e150
History | View | Annotate | Download (1.11 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 | 3db79f25 | Priya | #define WHEEL_RADIUS 2 |
9 | f79fbce2 | Priya | #define WHEEL_CIRCUM (2*M_PI*WHEEL_RADIUS) |
10 | 3db79f25 | Priya | #define WHEEL_BASE 5 |
11 | #define ENCODER_COUNT (4*1200*M_PI/5) |
||
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 | 3db79f25 | Priya | int motor_fl_dist;
|
43 | int motor_fr_dist;
|
||
44 | int motor_bl_dist;
|
||
45 | int motor_br_dist;
|
||
46 | f79fbce2 | Priya | |
47 | 3db79f25 | Priya | unsigned int motor_fl_ticks; |
48 | unsigned int motor_fr_ticks; |
||
49 | unsigned int motor_bl_ticks; |
||
50 | unsigned int motor_br_ticks; |
||
51 | 31be19a6 | Priya | float scout_theta;
|
52 | |||
53 | f79fbce2 | Priya | pos* scout_pos; |
54 | }; |
||
55 | |||
56 | #endif |