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 |
/** @} */
|