31 |
31 |
* Implementation of functions for encoder use.
|
32 |
32 |
*
|
33 |
33 |
* @author Colony Project, CMU Robotics Club
|
34 |
|
* @author Benjamin Wasserman
|
35 |
34 |
**/
|
36 |
35 |
|
37 |
36 |
#include "ros/ros.h"
|
... | ... | |
47 |
46 |
|
48 |
47 |
/* Encoder state variables */
|
49 |
48 |
/** \todo Fix types: static */
|
50 |
|
int encoder_fl; /**< The current state of the front left encoder. */
|
51 |
|
int encoder_fr; /**< The current state of the front right encoder. */
|
52 |
|
int encoder_bl; /**< The current state of the back left encoder. */
|
53 |
|
int encoder_br; /**< The current state of the back right encoder. */
|
|
49 |
/** \todo Fix integer overflow possibility */
|
|
50 |
/* 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;
|
|
60 |
/* The current distance each encoder has traveled in ticks from last reset */
|
|
61 |
int encoder_fl_distance;
|
|
62 |
int encoder_fr_distance;
|
|
63 |
int encoder_bl_distance;
|
|
64 |
int encoder_br_distance;
|
|
65 |
/* The units that the encoder measurements will be published in */
|
|
66 |
char encoder_units;
|
|
67 |
|
54 |
68 |
|
55 |
69 |
/*!
|
56 |
70 |
* \brief Encoders driver. This is a ROS node that controls encoders.
|
... | ... | |
87 |
101 |
*
|
88 |
102 |
* Sets the encoder state based on subscription to the encoder_state topic.
|
89 |
103 |
*
|
|
104 |
* \todo Make this function set the state properly. Probably requires
|
|
105 |
* creating a new msg type for the mode set.
|
|
106 |
*
|
90 |
107 |
* \param msg The message from the encoder_state topic, containing encoder
|
91 |
108 |
* configuration settings.
|
92 |
109 |
*/
|
93 |
|
void encoder_state(const encoders::encoder_state::ConstPtr& msg){
|
94 |
|
encoder_fl = msg->fl_encoder;
|
95 |
|
encoder_fr = msg->fr_encoder;
|
96 |
|
encoder_bl = msg->bl_encoder;
|
97 |
|
encoder_br = msg->br_encoder;
|
|
110 |
void encoder_set_mode(const encoders::encoder_state::ConstPtr& msg)
|
|
111 |
{
|
|
112 |
|
98 |
113 |
}
|
99 |
114 |
|
100 |
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 |
/*!
|
101 |
154 |
* \brief Outputs current encoder data
|
102 |
155 |
*
|
103 |
156 |
* Serves the service query_encoders by responding to service requests with the
|
104 |
157 |
* encoder values.
|
105 |
|
* \param req The request. Should be empty.
|
|
158 |
* \param req The request. The units that the response should be in.
|
106 |
159 |
* \param res The response. The fields will be filled with values.
|
107 |
160 |
*/
|
108 |
161 |
bool encoders_query(encoders::query_encoders::Request &req,
|
109 |
|
encoders::query_encoders::Response &res){
|
110 |
|
|
111 |
|
res.fl_encoder = encoder_fl;
|
112 |
|
res.fr_encoder = encoder_fr;
|
113 |
|
res.bl_encoder = encoder_bl;
|
114 |
|
res.br_encoder = encoder_br;
|
|
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);
|
115 |
179 |
|
116 |
180 |
ROS_DEBUG("Encoder values queried");
|
117 |
181 |
return true;
|
118 |
182 |
}
|
119 |
183 |
|
|
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 |
}
|