Revision f358d0b2
Tested encoders on scout. Made hacky changes so that it works. Tom needs to clean this up.
scout/encoders/src/encoders.cpp | ||
---|---|---|
61 | 61 |
*/ |
62 | 62 |
Encoder::Encoder(int n) |
63 | 63 |
{ |
64 |
char buf[60]; |
|
65 |
|
|
66 |
// open device file |
|
67 |
sprintf(buf, "/sys/class/encoder/enc%d/ticks", n); |
|
68 |
fticks.open(buf, std::ios::in); |
|
64 |
sprintf(filename, "/sys/class/encoder/enc%d/ticks", n); |
|
69 | 65 |
} |
70 | 66 |
|
71 | 67 |
/** |
... | ... | |
76 | 72 |
*/ |
77 | 73 |
int Encoder::get_ticks() |
78 | 74 |
{ |
75 |
// open device file |
|
76 |
fticks.open(filename, std::ios::in); |
|
77 |
|
|
79 | 78 |
int ticks; |
80 | 79 |
fticks >> ticks; |
80 |
|
|
81 |
fticks.close(); |
|
81 | 82 |
return ticks; |
82 | 83 |
} |
83 | 84 |
|
... | ... | |
101 | 102 |
/* @todo maybe return value based on whether reads succeeded */ |
102 | 103 |
|
103 | 104 |
ROS_DEBUG("Encoder values queried"); |
105 |
ROS_INFO("Encoder values queried %d %d %d %d", res.fl_distance, res.fr_distance, res.bl_distance, res.br_distance); |
|
104 | 106 |
return true; |
105 | 107 |
} |
106 | 108 |
|
scout/encoders/src/encoders.h | ||
---|---|---|
44 | 44 |
Encoder(int n); |
45 | 45 |
int get_ticks(); |
46 | 46 |
private: |
47 |
char filename[60]; |
|
47 | 48 |
std::fstream fticks; /**< The file in sysfs containing tick count */ |
48 | 49 |
}; |
49 | 50 |
|
Also available in: Unified diff