Revision d7c3c222
ID | d7c3c222b98353458e9c1e5353c8a4c76f847dc9 |
Encoders node now actually reads from hardware
This compiles but hasn't been tested on the Scout yet.
scout/encoders/src/encoders.cpp | ||
---|---|---|
31 | 31 |
* |
32 | 32 |
* @author Colony Project, CMU Robotics Club |
33 | 33 |
* @author Alex Zirbel |
34 |
* @author Tom Mullins |
|
34 | 35 |
**/ |
35 | 36 |
|
36 | 37 |
#include <ros/ros.h> |
... | ... | |
44 | 45 |
* @{ |
45 | 46 |
*/ |
46 | 47 |
|
47 |
/* Encoder state variables */ |
|
48 |
/** @todo Fix types: static */ |
|
49 |
/** @todo Fix integer overflow possibility */ |
|
48 |
/* Encoder instances */ |
|
49 |
/* @todo make sure these are the correct numbers */ |
|
50 |
static Encoder encoder_fl(0); |
|
51 |
static Encoder encoder_fr(1); |
|
52 |
static Encoder encoder_bl(2); |
|
53 |
static Encoder encoder_br(3); |
|
50 | 54 |
|
51 |
/* The current position of each encoder, measured in encoder ticks */ |
|
52 |
int encoder_fl_index; |
|
53 |
int encoder_fr_index; |
|
54 |
int encoder_bl_index; |
|
55 |
int encoder_br_index; |
|
55 |
/** |
|
56 |
* @brief Encoder constructor |
|
57 |
* |
|
58 |
* Opens device file, which is read at every call to get_ticks |
|
59 |
* |
|
60 |
* @param n The encoder number to read, between 0 and 3 inclusive |
|
61 |
*/ |
|
62 |
Encoder::Encoder(int n) |
|
63 |
{ |
|
64 |
char buf[60]; |
|
56 | 65 |
|
57 |
/* The current distance each encoder has traveled in ticks from last reset */ |
|
58 |
int encoder_fl_distance; |
|
59 |
int encoder_fr_distance; |
|
60 |
int encoder_bl_distance; |
|
61 |
int encoder_br_distance; |
|
66 |
// open device file |
|
67 |
sprintf(buf, "/sys/class/encoder/enc%d/ticks", n); |
|
68 |
fticks.open(buf, std::ios::in); |
|
69 |
} |
|
70 |
|
|
71 |
/** |
|
72 |
* @brief Returns the current tick count |
|
73 |
* |
|
74 |
* This will actually read from the encoder ticks file to get the latest value |
|
75 |
* from the driver |
|
76 |
*/ |
|
77 |
int Encoder::get_ticks() |
|
78 |
{ |
|
79 |
int ticks; |
|
80 |
fticks >> ticks; |
|
81 |
return ticks; |
|
82 |
} |
|
62 | 83 |
|
63 | 84 |
/** |
64 | 85 |
* @brief Outputs current encoder data |
... | ... | |
72 | 93 |
encoders::query_encoders::Response &res) |
73 | 94 |
{ |
74 | 95 |
/* Put index, velocity, and distance data in message */ |
75 |
res.fl_distance = encoder_fl_distance;
|
|
76 |
res.fr_distance = encoder_fr_distance;
|
|
77 |
res.bl_distance = encoder_bl_distance;
|
|
78 |
res.br_distance = encoder_br_distance;
|
|
96 |
res.fl_distance = encoder_fl.get_ticks();
|
|
97 |
res.fr_distance = encoder_fr.get_ticks();
|
|
98 |
res.bl_distance = encoder_bl.get_ticks();
|
|
99 |
res.br_distance = encoder_br.get_ticks();
|
|
79 | 100 |
|
80 |
ROS_DEBUG("Encoder values queried [Unimplemented]"); |
|
101 |
/* @todo maybe return value based on whether reads succeeded */ |
|
102 |
|
|
103 |
ROS_DEBUG("Encoder values queried"); |
|
81 | 104 |
return true; |
82 | 105 |
} |
83 | 106 |
|
... | ... | |
99 | 122 |
n.advertiseService("query_encoders", |
100 | 123 |
handle_encoders_query); |
101 | 124 |
|
102 |
/* Initialize hardware for motors */ |
|
103 |
// Hardware init functions here |
|
104 |
|
|
105 | 125 |
ROS_INFO("Ready to set encoders."); |
106 | 126 |
|
107 |
// Acutally measure encoders and update values here |
|
108 |
|
|
109 | 127 |
ros::spin(); |
110 | 128 |
|
111 | 129 |
return 0; |
Also available in: Unified diff