root / scout / libscout / src / test_behaviors / Odometry_new.h @ 36701fcd
History | View | Annotate | Download (1.49 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 | |||
10 | #define WHEEL_R 1 //this is a virtual unit now... use real values |
||
11 | #define WHEEL_B 1 // this is a virtual unit now... use real values |
||
12 | #define TICKS_PER_REV 48 |
||
13 | #define PI 3.1415926 |
||
14 | #define RAD_PER_TICK ((TICKS_PER_REV)/360.0 * PI / 180.0) |
||
15 | #define LOOP_RATE 10 // Hz, integrate 10 times per second |
||
16 | #define LOOP_TIME (1.0/LOOP_RATE) // secs |
||
17 | |||
18 | typedef struct{ |
||
19 | float x;
|
||
20 | float y;
|
||
21 | float theta;
|
||
22 | } posi; |
||
23 | |||
24 | class Odometry_new : Behavior{ |
||
25 | |||
26 | public:
|
||
27 | |||
28 | /** Set up the odometry node and prepare communications over ROS */
|
||
29 | Odometry_new(std::string scoutname, Sensors* sensors); |
||
30 | |||
31 | /** Query encoders and estimate position based on encoder reading */
|
||
32 | void get_position(double loop_time); |
||
33 | |||
34 | /** Gets scout position and prints to screen */
|
||
35 | void run();
|
||
36 | |||
37 | private:
|
||
38 | |||
39 | /** ROS publisher and client declaration */
|
||
40 | ros::NodeHandle node; |
||
41 | ros::Publisher scout_position; |
||
42 | ros::ServiceClient query_encoders_client; |
||
43 | messages::ScoutPosition position; |
||
44 | |||
45 | std::string name; |
||
46 | |||
47 | // FIXME: not sure whether these would overflow....
|
||
48 | int motor_fl_ticks_iter;
|
||
49 | int motor_fr_ticks_iter;
|
||
50 | int motor_bl_ticks_iter;
|
||
51 | int motor_br_ticks_iter;
|
||
52 | |||
53 | int motor_fl_ticks_last;
|
||
54 | int motor_fr_ticks_last;
|
||
55 | int motor_bl_ticks_last;
|
||
56 | int motor_br_ticks_last;
|
||
57 | int scout_theta;
|
||
58 | |||
59 | posi* scout_pos; |
||
60 | |||
61 | }; |
||
62 | |||
63 | #endif |