Revision 7cb5de78
Updated encoder node code with preliminary driver functionality.
Renamed encoder_state.msg to encoders_state.msg.
Added functions to encoders.cpp to send message of current encoder state.
Added function headers to encoders.h.
Added function to encoders.cpp to convert to different units.
Added additional global variables to store encoder state.
Updated encoder_state.msg and query_encoders.srv with additional fields for units.
Node is not completed. Still requires lowest level driver functionality. Needs constants for encoders and wheels. Needs initialization in main for encoders_state publisher. Needs implementation of encoders_set_mode, which should change what units the encoders node publishes the encoders_state message, and with which frequency.
I know this is bad practice, but I have not tried compiling my changes. I broke my computer's ability to compile ROS code, so this is untested. However, it should work. SORRY.
scout/encoders/src/encoders.cpp | ||
---|---|---|
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 |
} |
Also available in: Unified diff