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