root / scout / libscout / src / MotorControl.cpp @ 6ebee82c
History | View | Annotate | Download (9.2 KB)
1 | c406f16b | Ben | /**
|
---|---|---|---|
2 | 0121ead7 | bwasserm | * Copyright (c) 2011 Colony Project
|
3 | c406f16b | Ben | *
|
4 | * Permission is hereby granted, free of charge, to any person
|
||
5 | * obtaining a copy of this software and associated documentation
|
||
6 | * files (the "Software"), to deal in the Software without
|
||
7 | * restriction, including without limitation the rights to use,
|
||
8 | * copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
9 | * copies of the Software, and to permit persons to whom the
|
||
10 | * Software is furnished to do so, subject to the following
|
||
11 | * conditions:
|
||
12 | *
|
||
13 | * The above copyright notice and this permission notice shall be
|
||
14 | * included in all copies or substantial portions of the Software.
|
||
15 | *
|
||
16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
||
17 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||
18 | * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
||
19 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||
20 | * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||
21 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||
22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||
23 | * OTHER DEALINGS IN THE SOFTWARE.
|
||
24 | c492be62 | Alex Zirbel | */
|
25 | c406f16b | Ben | |
26 | /**
|
||
27 | a8480867 | Alex Zirbel | * @file MotorControl.cpp
|
28 | c406f16b | Ben | * @brief Contains motor declarations and functions
|
29 | *
|
||
30 | * Contains functions and definitions for the use of
|
||
31 | * motors
|
||
32 | *
|
||
33 | * @author Colony Project, CMU Robotics Club
|
||
34 | a8480867 | Alex Zirbel | * @author Ben Wasserman
|
35 | * @author Alex Zirbel
|
||
36 | c406f16b | Ben | **/
|
37 | |||
38 | a8480867 | Alex Zirbel | #include "MotorControl.h" |
39 | 0121ead7 | bwasserm | |
40 | c384dc7e | Alex Zirbel | using namespace std; |
41 | |||
42 | 2814387f | Alex Zirbel | /**
|
43 | * @brief Initialize the motors module of libscout.
|
||
44 | 0121ead7 | bwasserm | *
|
45 | * Initialize the libscout node as a publisher of set_motors and a client of
|
||
46 | * query_motors.
|
||
47 | */
|
||
48 | c384dc7e | Alex Zirbel | MotorControl::MotorControl(const ros::NodeHandle& libscout_node,
|
49 | string scoutname)
|
||
50 | a8480867 | Alex Zirbel | : node(libscout_node) |
51 | 4589a2a9 | Ben Wasserman | { |
52 | a8480867 | Alex Zirbel | set_motors_pub = |
53 | 6ebee82c | Alex | node.advertise< ::messages::set_motors>(scoutname + "/set_motors",
|
54 | cfdb3afa | Alex | QUEUE_SIZE, true);
|
55 | 2814387f | Alex Zirbel | query_motors_client = |
56 | 6ebee82c | Alex | node.serviceClient< ::messages::query_motors>(scoutname + "/query_motors");
|
57 | c406f16b | Ben | } |
58 | |||
59 | 6761a531 | Priya | MotorControl::~MotorControl() |
60 | { |
||
61 | set_sides(0, 0, MOTOR_ABSOLUTE); |
||
62 | } |
||
63 | |||
64 | 2814387f | Alex Zirbel | /**
|
65 | * @brief Sets all the speeds according to the specificiation of which motors
|
||
66 | * to set.
|
||
67 | *
|
||
68 | * @param which A bitmask of which motors need to be set.
|
||
69 | * @param speed The value to set the motors to.
|
||
70 | * @param units The units the speed is expressed in.
|
||
71 | * @return Function status
|
||
72 | */
|
||
73 | a8480867 | Alex Zirbel | void MotorControl::set(int which, float speed, char units) |
74 | 2814387f | Alex Zirbel | { |
75 | float speed_fl, speed_fr, speed_bl, speed_br;
|
||
76 | speed_fl = speed_fr = speed_bl = speed_br = speed; |
||
77 | |||
78 | if(which & MOTOR_FL_REV)
|
||
79 | { |
||
80 | speed_fl = -speed; |
||
81 | } |
||
82 | if(which & MOTOR_FR_REV)
|
||
83 | { |
||
84 | speed_fr = -speed; |
||
85 | } |
||
86 | if(which & MOTOR_BL_REV)
|
||
87 | { |
||
88 | speed_bl = -speed; |
||
89 | } |
||
90 | if(which & MOTOR_BR_REV)
|
||
91 | { |
||
92 | speed_br = -speed; |
||
93 | } |
||
94 | |||
95 | a8480867 | Alex Zirbel | set_each(which, speed_fl, speed_fr, speed_bl, speed_br, units); |
96 | 2814387f | Alex Zirbel | } |
97 | |||
98 | /**
|
||
99 | * @brief Sets the left and right sides of scout to different speeds.
|
||
100 | *
|
||
101 | * @param speed_l The speed of both left motors.
|
||
102 | * @param speed_r The speed of both right motors.
|
||
103 | * @param units The units to set to.
|
||
104 | * @return Function status
|
||
105 | */
|
||
106 | a8480867 | Alex Zirbel | void MotorControl::set_sides(float speed_l, float speed_r, char units) |
107 | 2814387f | Alex Zirbel | { |
108 | a8480867 | Alex Zirbel | set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r, units); |
109 | 2814387f | Alex Zirbel | } |
110 | |||
111 | /**
|
||
112 | * @brief Set motor speeds
|
||
113 | 0121ead7 | bwasserm | * Sets the speeds of the motors as a percentage of top speed. Can selectively
|
114 | a8480867 | Alex Zirbel | * select which motors to set, and which to keep at previous speed.
|
115 | 2814387f | Alex Zirbel | *
|
116 | * @param which A bitmask of which motors should be set.
|
||
117 | * @param speed The speed the motors should be set to.
|
||
118 | * @param units Optional units for the speeds.
|
||
119 | * @return Function status
|
||
120 | 0121ead7 | bwasserm | */
|
121 | a8480867 | Alex Zirbel | void MotorControl::set_each(int which, |
122 | float speed_fl, float speed_fr, |
||
123 | float speed_bl, float speed_br, |
||
124 | char units)
|
||
125 | 4589a2a9 | Ben Wasserman | { |
126 | 2814387f | Alex Zirbel | check_which_ok(which); |
127 | |||
128 | 6ebee82c | Alex | ::messages::set_motors msg; |
129 | 2814387f | Alex Zirbel | |
130 | |||
131 | /* Tell the motors node which motors need to be updated */
|
||
132 | msg.fl_set = msg.fr_set = msg.bl_set = msg.br_set = false;
|
||
133 | if(which & (MOTOR_FL | MOTOR_FL_REV))
|
||
134 | { |
||
135 | msg.fl_set = true;
|
||
136 | a8480867 | Alex Zirbel | motor_fl_speed = rel_to_abs(speed_fl, units); |
137 | 2814387f | Alex Zirbel | } |
138 | if(which & (MOTOR_FR | MOTOR_FR_REV))
|
||
139 | { |
||
140 | msg.fr_set = true;
|
||
141 | a8480867 | Alex Zirbel | motor_fr_speed = rel_to_abs(speed_fr, units); |
142 | 2814387f | Alex Zirbel | } |
143 | if(which & (MOTOR_BL | MOTOR_BL_REV))
|
||
144 | { |
||
145 | msg.bl_set = true;
|
||
146 | a8480867 | Alex Zirbel | motor_bl_speed = rel_to_abs(speed_bl, units); |
147 | 2814387f | Alex Zirbel | } |
148 | if(which & (MOTOR_BR | MOTOR_BR_REV))
|
||
149 | { |
||
150 | msg.br_set = true;
|
||
151 | a8480867 | Alex Zirbel | motor_br_speed = rel_to_abs(speed_br, units); |
152 | 2814387f | Alex Zirbel | } |
153 | |||
154 | a8480867 | Alex Zirbel | /* Set all the speeds (the booleans in the set_motors message determine
|
155 | * which ones will be used) */
|
||
156 | f325b893 | Alex | msg.fl_speed = trim_speed("FL", (int) motor_fl_speed); |
157 | msg.fr_speed = trim_speed("FR", (int) motor_fr_speed); |
||
158 | msg.bl_speed = trim_speed("BL", (int) motor_bl_speed); |
||
159 | msg.br_speed = trim_speed("BR", (int) motor_br_speed); |
||
160 | 2814387f | Alex Zirbel | |
161 | /* Publishes message to set_motors topic */
|
||
162 | a8480867 | Alex Zirbel | set_motors_pub.publish(msg); |
163 | 2814387f | Alex Zirbel | ros::spinOnce(); |
164 | c406f16b | Ben | } |
165 | |||
166 | 2814387f | Alex Zirbel | /**
|
167 | * Double-checks the which variable to make sure the front and back
|
||
168 | * bits are not both set for a single motor.
|
||
169 | *
|
||
170 | * @param which The which motor specfication to check.
|
||
171 | */
|
||
172 | a8480867 | Alex Zirbel | void MotorControl::check_which_ok(int which) |
173 | 2814387f | Alex Zirbel | { |
174 | a8480867 | Alex Zirbel | ROS_ERROR_COND( ((which & MOTOR_FL) && (which & MOTOR_FL_REV)), |
175 | 2814387f | Alex Zirbel | "FL Motor set in both directions!");
|
176 | a8480867 | Alex Zirbel | ROS_ERROR_COND( ((which & MOTOR_FR) && (which & MOTOR_FR_REV)), |
177 | 2814387f | Alex Zirbel | "FR Motor set in both directions!");
|
178 | a8480867 | Alex Zirbel | ROS_ERROR_COND( ((which & MOTOR_BL) && (which & MOTOR_BL_REV)), |
179 | 2814387f | Alex Zirbel | "BL Motor set in both directions!");
|
180 | a8480867 | Alex Zirbel | ROS_ERROR_COND( ((which & MOTOR_BR) && (which & MOTOR_BR_REV)), |
181 | 2814387f | Alex Zirbel | "BR Motor set in both directions!");
|
182 | } |
||
183 | |||
184 | /**
|
||
185 | f325b893 | Alex | * Trims an absolute speed to stay within motor maximum range.
|
186 | * Uses ROS_WARN to warn the user if a speed has been trimmed.
|
||
187 | *
|
||
188 | * @param which_str A description of which motor, to be used in the warning
|
||
189 | * message (for example, "FL")
|
||
190 | * @param speed The requested speed to be set to
|
||
191 | * @return The requested speed, trimmed to stay within limits.
|
||
192 | */
|
||
193 | int MotorControl::trim_speed(string which_str, int speed) |
||
194 | { |
||
195 | int trimmed_speed = speed;
|
||
196 | if (speed > MAXSPEED_ABS)
|
||
197 | { |
||
198 | trimmed_speed = (int) MAXSPEED_ABS;
|
||
199 | ROS_WARN("Motor (%s) speed trimmed to %d.", which_str.c_str(),
|
||
200 | (int) MAXSPEED_ABS);
|
||
201 | } |
||
202 | else if (speed < -MAXSPEED_ABS) |
||
203 | { |
||
204 | trimmed_speed = (int) -MAXSPEED_ABS;
|
||
205 | ROS_WARN("Motor (%s) speed trimmed to %d.", which_str.c_str(),
|
||
206 | (int) -MAXSPEED_ABS);
|
||
207 | } |
||
208 | |||
209 | return trimmed_speed;
|
||
210 | } |
||
211 | |||
212 | /**
|
||
213 | 2814387f | Alex Zirbel | * @brief Query the current speeds of the motors
|
214 | 0121ead7 | bwasserm | *
|
215 | * Sends a request to the query_motors service which will reply with the
|
||
216 | * current speed of each motor.
|
||
217 | *
|
||
218 | a8480867 | Alex Zirbel | * @TODO Change so we can get multiple motor speeds with one call
|
219 | 2814387f | Alex Zirbel | *
|
220 | * @param which A bitmask that will specify which motor speed should be
|
||
221 | 0121ead7 | bwasserm | * returned
|
222 | cc558a8d | Alex Zirbel | * @return The speed of the selected motor
|
223 | 0121ead7 | bwasserm | */
|
224 | a8480867 | Alex Zirbel | float MotorControl::query(int which) |
225 | 4589a2a9 | Ben Wasserman | { |
226 | 6ebee82c | Alex | ::messages::query_motors srv; |
227 | 2814387f | Alex Zirbel | if(query_motors_client.call(srv))
|
228 | { |
||
229 | switch(which)
|
||
230 | { |
||
231 | case MOTOR_FL:
|
||
232 | return srv.response.fl_speed;
|
||
233 | case MOTOR_FR:
|
||
234 | return srv.response.fr_speed;
|
||
235 | case MOTOR_BL:
|
||
236 | return srv.response.bl_speed;
|
||
237 | case MOTOR_BR:
|
||
238 | return srv.response.br_speed;
|
||
239 | default:
|
||
240 | cc558a8d | Alex Zirbel | /// @todo: I hate this. Let's fix it soon.
|
241 | ROS_FATAL("Bad WHICH in motors_query.");
|
||
242 | 2814387f | Alex Zirbel | } |
243 | } |
||
244 | else
|
||
245 | { |
||
246 | cc558a8d | Alex Zirbel | ROS_FATAL("Failed to call service query_motors");
|
247 | c406f16b | Ben | } |
248 | 2814387f | Alex Zirbel | |
249 | return 0; |
||
250 | c406f16b | Ben | } |
251 | a8480867 | Alex Zirbel | |
252 | /**
|
||
253 | * @brief Converts set speeds (of various units) to absolute speeds.
|
||
254 | *
|
||
255 | * @param speed The speed expressed in the desired units
|
||
256 | * @param units The units the desired speed is measured in
|
||
257 | * @return The absolute speed of the motor
|
||
258 | **/
|
||
259 | float MotorControl::rel_to_abs(float rel_speed, int units) |
||
260 | { |
||
261 | switch(units)
|
||
262 | { |
||
263 | case MOTOR_ABSOLUTE:/* Speed given as absolute */ |
||
264 | return rel_speed;
|
||
265 | case MOTOR_PERCENT:/* Convert from percentage */ |
||
266 | 4e393fcc | Alex | return rel_speed * MAXSPEED_ABS / 100.0; |
267 | a8480867 | Alex Zirbel | case MOTOR_MMS:/* Convert from mm/s */ |
268 | f325b893 | Alex | return rel_speed * MAXSPEED_ABS / (1000.0 * MAXSPEED_MPS); |
269 | a8480867 | Alex Zirbel | case MOTOR_CMS:/* Convert from cm/s */ |
270 | f325b893 | Alex | return rel_speed * MAXSPEED_ABS / (100.0 * MAXSPEED_MPS); |
271 | a8480867 | Alex Zirbel | default: /* The units aren't recognized */ |
272 | 4e393fcc | Alex | ROS_WARN("MotorControl::rel_to_abs used with default units.");
|
273 | a8480867 | Alex Zirbel | return rel_speed;
|
274 | } |
||
275 | } |
||
276 | |||
277 | /**
|
||
278 | * @brief Convert absolute speeds to speeds of various units.
|
||
279 | *
|
||
280 | * @param speed The speed expressed in absolute units.
|
||
281 | * @param units The units the desired speed is measured in.
|
||
282 | * @return The relative speed of the motor in desired units.
|
||
283 | **/
|
||
284 | float MotorControl::abs_to_rel(float abs_speed, int units) |
||
285 | { |
||
286 | switch(units)
|
||
287 | { |
||
288 | case MOTOR_ABSOLUTE:/* Speed given as absolute */ |
||
289 | return abs_speed;
|
||
290 | case MOTOR_PERCENT:/* Convert from percentage */ |
||
291 | 4e393fcc | Alex | return abs_speed * 100.0 / MAXSPEED_ABS; |
292 | a8480867 | Alex Zirbel | case MOTOR_MMS:/* Convert from mm/s */ |
293 | f325b893 | Alex | return abs_speed * MAXSPEED_MPS * 1000.0 / MAXSPEED_ABS; |
294 | a8480867 | Alex Zirbel | case MOTOR_CMS:/* Convert from cm/s */ |
295 | f325b893 | Alex | return abs_speed * MAXSPEED_MPS * 100.0 / MAXSPEED_ABS; |
296 | a8480867 | Alex Zirbel | default: /* The units aren't recognized */ |
297 | 4e393fcc | Alex | ROS_WARN("MotorControl::rel_to_abs used with default units.");
|
298 | a8480867 | Alex Zirbel | return abs_speed;
|
299 | } |
||
300 | } |