Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / test_behaviors / Odometry_new.h @ beaf9201

History | View | Annotate | Download (1.46 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 0.025 //this is a virtual unit now... use real values
11
#define WHEEL_B 0.105  // this is a virtual unit now... use real values
12
#define TICKS_PER_REV 48
13
#define RAD_PER_TICK (2*M_PI/TICKS_PER_REV)
14
#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