Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / behaviors / Odometry.h @ 3db79f25

History | View | Annotate | Download (1.07 KB)

1
#ifndef _ODOMETRY_H_
2
#define _ODOMETRY_H_
3

    
4
#include "../Behavior.h"
5

    
6

    
7
#define WHEEL_RADIUS  2
8
#define WHEEL_CIRCUM  (2*M_PI*WHEEL_RADIUS)
9
#define WHEEL_BASE    5
10
#define ENCODER_COUNT (4*1200*M_PI/5)
11
#define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT)
12

    
13
typedef struct{
14
  float x;
15
  float y;
16
  float theta;
17
} pos;
18

    
19
class Odometry : Behavior{
20

    
21
  public:
22

    
23
  /** Set up the odometry node and prepare communcations over ROS */
24
  Odometry(std::string scoutname);
25

    
26
  /** Query encoders and estimate position based on encoder reading */
27
  void get_position();
28

    
29
  /** Gets scout position and prints to screen */
30
  void run();
31
  
32
  private:
33

    
34
    /** ROS publisher and client declaration */
35
    ros::NodeHandle node;
36
    //TODO: ros::Publisher scout_position;
37
    ros::ServiceClient query_encoders_client;
38

    
39
    float msg_time_in;
40

    
41
    int motor_fl_dist;
42
    int motor_fr_dist;
43
    int motor_bl_dist;
44
    int motor_br_dist;
45

    
46
    unsigned int motor_fl_ticks;
47
    unsigned int motor_fr_ticks;
48
    unsigned int motor_bl_ticks;
49
    unsigned int motor_br_ticks;
50
    float scout_theta;
51

    
52
    pos* scout_pos;
53
};
54

    
55
#endif