Statistics
| Branch: | Revision:

root / scout / libscout / src / test_behaviors / Odometry_new.h @ 36701fcd

History | View | Annotate | Download (1.49 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

    
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