Statistics
| Branch: | Revision:

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

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
#define WHEEL_B 0.105  // 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