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; |
| scout/encoders/src/encoders.h | ||
|---|---|---|
| 29 | 29 |
* |
| 30 | 30 |
* @author Colony Project, CMU Robotics Club |
| 31 | 31 |
* @author Alex Zirbel |
| 32 |
* @author Tom Mullins |
|
| 32 | 33 |
**/ |
| 33 | 34 |
|
| 34 | 35 |
#ifndef _ENCODERS_H_ |
| 35 | 36 |
#define _ENCODERS_H_ |
| 36 | 37 |
|
| 38 |
#include <fstream> |
|
| 37 | 39 |
#include "encoders/query_encoders.h" |
| 38 | 40 |
|
| 41 |
class Encoder |
|
| 42 |
{
|
|
| 43 |
public: |
|
| 44 |
Encoder(int n); |
|
| 45 |
int get_ticks(); |
|
| 46 |
private: |
|
| 47 |
std::fstream fticks; /**< The file in sysfs containing tick count */ |
|
| 48 |
}; |
|
| 49 |
|
|
| 39 | 50 |
/** @brief Initialize the encoders module and driver **/ |
| 40 | 51 |
int main(int argc, char **argv); |
| 41 | 52 |
|
Also available in: Unified diff