| 23 |
23 |
* OTHER DEALINGS IN THE SOFTWARE.
|
| 24 |
24 |
*/
|
| 25 |
25 |
|
| 26 |
|
|
| 27 |
26 |
/**
|
| 28 |
27 |
* @file encoders.cpp
|
| 29 |
28 |
* @brief Encoders
|
| ... | ... | |
| 31 |
30 |
* Implementation of functions for encoder use.
|
| 32 |
31 |
*
|
| 33 |
32 |
* @author Colony Project, CMU Robotics Club
|
|
33 |
* @author Alex Zirbel
|
| 34 |
34 |
**/
|
| 35 |
35 |
|
| 36 |
|
#include "ros/ros.h"
|
|
36 |
#include <ros/ros.h>
|
|
37 |
|
| 37 |
38 |
#include "encoders.h"
|
| 38 |
|
//#include "libscout/src/constants.h"
|
| 39 |
|
#include <cstdlib>
|
| 40 |
39 |
|
| 41 |
40 |
/**
|
| 42 |
41 |
* @defgroup encoders Encoders
|
| 43 |
42 |
* @brief Functions for using the encoders
|
| 44 |
43 |
*
|
| 45 |
|
**/
|
|
44 |
* @{
|
|
45 |
*/
|
| 46 |
46 |
|
| 47 |
47 |
/* Encoder state variables */
|
| 48 |
|
/** \todo Fix types: static */
|
| 49 |
|
/** \todo Fix integer overflow possibility */
|
|
48 |
/** @todo Fix types: static */
|
|
49 |
/** @todo Fix integer overflow possibility */
|
|
50 |
|
| 50 |
51 |
/* The current position of each encoder, measured in encoder ticks */
|
| 51 |
|
int encoder_fl_index; /**< The current state of the front left encoder. */
|
| 52 |
|
int encoder_fr_index; /**< The current state of the front right encoder. */
|
| 53 |
|
int encoder_bl_index; /**< The current state of the back left encoder. */
|
| 54 |
|
int encoder_br_index; /**< The current state of the back right encoder. */
|
| 55 |
|
/* The current velocity of each encoder, measured in ticks per second */
|
| 56 |
|
int encoder_fl_velocity;
|
| 57 |
|
int encoder_fr_velocity;
|
| 58 |
|
int encoder_bl_velocity;
|
| 59 |
|
int encoder_br_velocity;
|
|
52 |
int encoder_fl_index;
|
|
53 |
int encoder_fr_index;
|
|
54 |
int encoder_bl_index;
|
|
55 |
int encoder_br_index;
|
|
56 |
|
| 60 |
57 |
/* The current distance each encoder has traveled in ticks from last reset */
|
| 61 |
58 |
int encoder_fl_distance;
|
| 62 |
59 |
int encoder_fr_distance;
|
| 63 |
60 |
int encoder_bl_distance;
|
| 64 |
61 |
int encoder_br_distance;
|
| 65 |
|
/* The units that the encoder measurements will be published in */
|
| 66 |
|
char encoder_units;
|
| 67 |
62 |
|
|
63 |
/**
|
|
64 |
* @brief Outputs current encoder data
|
|
65 |
*
|
|
66 |
* Serves the service query_encoders by responding to service requests with the
|
|
67 |
* encoder values.
|
|
68 |
* @param req The request. The units that the response should be in.
|
|
69 |
* @param res The response. The fields will be filled with values.
|
|
70 |
*/
|
|
71 |
bool handle_encoders_query(encoders::query_encoders::Request &req,
|
|
72 |
encoders::query_encoders::Response &res)
|
|
73 |
{
|
|
74 |
/* 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;
|
|
79 |
|
|
80 |
ROS_DEBUG("Encoder values queried [Unimplemented]");
|
|
81 |
return true;
|
|
82 |
}
|
| 68 |
83 |
|
| 69 |
|
/*!
|
| 70 |
|
* \brief Encoders driver. This is a ROS node that controls encoders.
|
|
84 |
/**
|
|
85 |
* @brief Encoders driver. This is a ROS node that controls encoders.
|
| 71 |
86 |
*
|
| 72 |
87 |
* This is the main function for the encoders node. It is run when the node
|
| 73 |
88 |
* starts and initializes the encoders. It then publishes to the
|
| 74 |
89 |
* encoder_state topic and advertises the query_encoders service.
|
| 75 |
|
*
|
| 76 |
|
* \param argc The number of command line arguments (should be 1)
|
| 77 |
|
* \param argv The array of command line arguments
|
| 78 |
|
**/
|
| 79 |
|
int main(int argc, char **argv){
|
| 80 |
|
/* Initialize in ROS the encoders driver node */
|
| 81 |
|
ros::init(argc, argv, "encoders_driver");
|
| 82 |
|
|
| 83 |
|
/* Advertise that this serves the query_encoders service */
|
| 84 |
|
ros::NodeHandle n;
|
| 85 |
|
ros::ServiceServer service = n.advertiseService("query_encoders", encoders_query);
|
|
90 |
*/
|
|
91 |
int main(int argc, char **argv)
|
|
92 |
{
|
|
93 |
/* Initialize in ROS the encoders driver node */
|
|
94 |
ros::init(argc, argv, "encoders_driver");
|
| 86 |
95 |
|
| 87 |
|
/* Subscribe to the encoder_state topic */
|
| 88 |
|
ros::Subscriber sub0 = n.subscribe("encoder_state", 4, encoder_state);
|
|
96 |
/* Advertise that this serves the query_encoders service */
|
|
97 |
ros::NodeHandle n;
|
|
98 |
ros::ServiceServer service =
|
|
99 |
n.advertiseService("query_encoders",
|
|
100 |
handle_encoders_query);
|
| 89 |
101 |
|
| 90 |
|
/* Initialize hardware for motors */
|
| 91 |
|
// Hardware init functions here
|
|
102 |
/* Initialize hardware for motors */
|
|
103 |
// Hardware init functions here
|
| 92 |
104 |
|
| 93 |
|
ROS_INFO("Ready to set encoders.");
|
| 94 |
|
ros::spin();
|
|
105 |
ROS_INFO("Ready to set encoders.");
|
| 95 |
106 |
|
| 96 |
|
return 0;
|
| 97 |
|
}
|
|
107 |
// Acutally measure encoders and update values here
|
| 98 |
108 |
|
| 99 |
|
/*!
|
| 100 |
|
* \brief Gets encoder state
|
| 101 |
|
*
|
| 102 |
|
* Sets the encoder state based on subscription to the encoder_state topic.
|
| 103 |
|
*
|
| 104 |
|
* \todo Make this function set the state properly. Probably requires
|
| 105 |
|
* creating a new msg type for the mode set.
|
| 106 |
|
*
|
| 107 |
|
* \param msg The message from the encoder_state topic, containing encoder
|
| 108 |
|
* configuration settings.
|
| 109 |
|
*/
|
| 110 |
|
void encoder_set_mode(const encoders::encoder_state::ConstPtr& msg)
|
| 111 |
|
{
|
| 112 |
|
|
| 113 |
|
}
|
|
109 |
ros::spin();
|
| 114 |
110 |
|
| 115 |
|
/*!
|
| 116 |
|
* \brief Publishes encoder state
|
| 117 |
|
*
|
| 118 |
|
* Publishes the current encoders position, velocty, and distance from last
|
| 119 |
|
* reset.
|
| 120 |
|
*/
|
| 121 |
|
void encoders_state_publish()
|
| 122 |
|
{
|
| 123 |
|
encoders::encoders_state msg;
|
| 124 |
|
|
| 125 |
|
if(!ros::ok())
|
| 126 |
|
{
|
| 127 |
|
return LIB_ERROR;
|
| 128 |
|
}
|
| 129 |
|
|
| 130 |
|
/* Put index, velocity, and distance data in message */
|
| 131 |
|
msg.fl_index = encoders_ticks_to_units(encoder_fl_index, encoder_units);
|
| 132 |
|
msg.fr_index = encoders_ticks_to_units(encoder_fr_index, encoder_units);
|
| 133 |
|
msg.bl_index = encoders_ticks_to_units(encoder_bl_index, encoder_units);
|
| 134 |
|
msg.br_index = encoders_ticks_to_units(encoder_br_index, encoder_units);
|
| 135 |
|
msg.fl_speed = encoders_ticks_to_units(encoder_fl_velocity, encoder_units);
|
| 136 |
|
msg.fr_speed = encoders_ticks_to_units(encoder_fr_velocity, encoder_units);
|
| 137 |
|
msg.bl_speed = encoders_ticks_to_units(encoder_bl_velocity, encoder_units);
|
| 138 |
|
msg.br_speed = encoders_ticks_to_units(encoder_br_velocity, encoder_units);
|
| 139 |
|
msg.fl_distance = encoders_ticks_to_units(encoder_fl_distance, encoder_units);
|
| 140 |
|
msg.fr_distance = encoders_ticks_to_units(encoder_fr_distance, encoder_units);
|
| 141 |
|
msg.bl_distance = encoders_ticks_to_units(encoder_bl_distance, encoder_units);
|
| 142 |
|
msg.br_distance = encoders_ticks_to_units(encoder_br_distance, encoder_units);
|
| 143 |
|
/* Specifies which units the encoder values are given in */
|
| 144 |
|
msg.units = encoder_units;
|
| 145 |
|
|
| 146 |
|
/* Publishes message to encoders_state */
|
| 147 |
|
encoders_state_publisher.publish(msg);
|
| 148 |
|
ros::spinOnce();
|
| 149 |
|
|
| 150 |
|
return LIB_OK;
|
| 151 |
|
}
|
| 152 |
|
|
| 153 |
|
/*!
|
| 154 |
|
* \brief Outputs current encoder data
|
| 155 |
|
*
|
| 156 |
|
* Serves the service query_encoders by responding to service requests with the
|
| 157 |
|
* encoder values.
|
| 158 |
|
* \param req The request. The units that the response should be in.
|
| 159 |
|
* \param res The response. The fields will be filled with values.
|
| 160 |
|
*/
|
| 161 |
|
bool encoders_query(encoders::query_encoders::Request &req,
|
| 162 |
|
encoders::query_encoders::Response &res)
|
| 163 |
|
{
|
| 164 |
|
int units = req.units;
|
| 165 |
|
|
| 166 |
|
/* Put index, velocity, and distance data in message */
|
| 167 |
|
res.fl_index = encoders_ticks_to_units(encoder_fl_index, units);
|
| 168 |
|
res.fr_index = encoders_ticks_to_units(encoder_fr_index, units);
|
| 169 |
|
res.bl_index = encoders_ticks_to_units(encoder_bl_index, units);
|
| 170 |
|
res.br_index = encoders_ticks_to_units(encoder_br_index, units);
|
| 171 |
|
res.fl_speed = encoders_ticks_to_units(encoder_fl_velocity, units);
|
| 172 |
|
res.fr_speed = encoders_ticks_to_units(encoder_fr_velocity, units);
|
| 173 |
|
res.bl_speed = encoders_ticks_to_units(encoder_bl_velocity, units);
|
| 174 |
|
res.br_speed = encoders_ticks_to_units(encoder_br_velocity, units);
|
| 175 |
|
res.fl_distance = encoders_ticks_to_units(encoder_fl_distance, units);
|
| 176 |
|
res.fr_distance = encoders_ticks_to_units(encoder_fr_distance, units);
|
| 177 |
|
res.bl_distance = encoders_ticks_to_units(encoder_bl_distance, units);
|
| 178 |
|
res.br_distance = encoders_ticks_to_units(encoder_br_distance, units);
|
| 179 |
|
|
| 180 |
|
ROS_DEBUG("Encoder values queried");
|
| 181 |
|
return true;
|
|
111 |
return 0;
|
| 182 |
112 |
}
|
| 183 |
113 |
|
| 184 |
|
int encoders_ticks_to_units(int ticks, char units)
|
| 185 |
|
{
|
| 186 |
|
switch(encoder_units)
|
| 187 |
|
{
|
| 188 |
|
case ENCODER_TICKS: /* Just return ticks */
|
| 189 |
|
return ticks;
|
| 190 |
|
case ENCODER_MM: /* Return mm of linear travel by the wheel */
|
| 191 |
|
return ticks * (2 * PI * WHEEL_RADIUS_MM / NUM_TICKS);
|
| 192 |
|
case ENCODER_CM: /* Return cm of linear travel by the wheel */
|
| 193 |
|
return ticks * (2 * PI * WHEEL_RADIUS_MM / NUM_TICKS) / 10;
|
| 194 |
|
case ENCODER_DEG: /* Return degrees of travel from last reset */
|
| 195 |
|
return ticks * (360 / NUM_TICKS);
|
| 196 |
|
default: /* The units aren't recognized. Return ticks. */
|
| 197 |
|
return ticks;
|
| 198 |
|
}
|
| 199 |
|
}
|
|
114 |
/** @} */
|