Statistics
| Branch: | Revision:

root / scout / libscout / src / test_behaviors / Odometry_new.h @ bb64f5e5

History | View | Annotate | Download (1.45 KB)

1
// 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
#include "../helper_classes/ScoutPosition.h"
10

    
11
#define WHEEL_R 0.025 //this is a virtual unit now... use real values
12
#define WHEEL_B 0.115  // this is a virtual unit now... use real values
13
#define TICKS_PER_REV 48
14
#define RAD_PER_TICK (2*M_PI/TICKS_PER_REV)
15
#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
    pos* scout_pos;
54

    
55
};
56

    
57
#endif