root / scout / libscout / src / test_behaviors / Odometry_new.h @ bb64f5e5
History | View | Annotate | Download (1.45 KB)
1 | 36701fcd | Yuyang Guo | // a smarter Odometry the use RK4 integration
|
---|---|---|---|
2 | |||
3 | #ifndef _ODOMETRY_NEW_H_
|
||
4 | #define _ODOMETRY_NEW_H_
|
||
5 | |||
6 | #include "../Behavior.h" |
||
7 | #include "../Sensors.h" |
||
8 | #include "messages/ScoutPosition.h" |
||
9 | bb64f5e5 | Priya | #include "../helper_classes/ScoutPosition.h" |
10 | 36701fcd | Yuyang Guo | |
11 | beaf9201 | Colony Scout | #define WHEEL_R 0.025 //this is a virtual unit now... use real values |
12 | 259aaff8 | Colony Scout | #define WHEEL_B 0.115 // this is a virtual unit now... use real values |
13 | 36701fcd | Yuyang Guo | #define TICKS_PER_REV 48 |
14 | beaf9201 | Colony Scout | #define RAD_PER_TICK (2*M_PI/TICKS_PER_REV) |
15 | 36701fcd | Yuyang Guo | #define LOOP_RATE 10 // Hz, integrate 10 times per second |
16 | #define LOOP_TIME (1.0/LOOP_RATE) // secs |
||
17 | |||
18 | class Odometry_new : Behavior{ |
||
19 | |||
20 | public:
|
||
21 | |||
22 | /** Set up the odometry node and prepare communications over ROS */
|
||
23 | Odometry_new(std::string scoutname, Sensors* sensors); |
||
24 | |||
25 | /** Query encoders and estimate position based on encoder reading */
|
||
26 | void get_position(double loop_time); |
||
27 | |||
28 | /** Gets scout position and prints to screen */
|
||
29 | void run();
|
||
30 | |||
31 | private:
|
||
32 | |||
33 | /** ROS publisher and client declaration */
|
||
34 | ros::NodeHandle node; |
||
35 | ros::Publisher scout_position; |
||
36 | ros::ServiceClient query_encoders_client; |
||
37 | messages::ScoutPosition position; |
||
38 | |||
39 | std::string name; |
||
40 | |||
41 | // FIXME: not sure whether these would overflow....
|
||
42 | int motor_fl_ticks_iter;
|
||
43 | int motor_fr_ticks_iter;
|
||
44 | int motor_bl_ticks_iter;
|
||
45 | int motor_br_ticks_iter;
|
||
46 | |||
47 | int motor_fl_ticks_last;
|
||
48 | int motor_fr_ticks_last;
|
||
49 | int motor_bl_ticks_last;
|
||
50 | int motor_br_ticks_last;
|
||
51 | int scout_theta;
|
||
52 | |||
53 | bb64f5e5 | Priya | pos* scout_pos; |
54 | 36701fcd | Yuyang Guo | |
55 | }; |
||
56 | |||
57 | #endif |