scoutos / scout / libscout / src / MotorControl.cpp @ c492be62
History | View | Annotate | Download (8.22 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 | c384dc7e | Alex Zirbel | node.advertise<motors::set_motors>(scoutname + "/set_motors",
|
54 | QUEUE_SIZE); |
||
55 | 2814387f | Alex Zirbel | query_motors_client = |
56 | c384dc7e | Alex Zirbel | node.serviceClient<motors::query_motors>(scoutname + "/query_motors");
|
57 | c406f16b | Ben | } |
58 | |||
59 | 2814387f | Alex Zirbel | /**
|
60 | * @brief Sets all the speeds according to the specificiation of which motors
|
||
61 | * to set.
|
||
62 | *
|
||
63 | * @param which A bitmask of which motors need to be set.
|
||
64 | * @param speed The value to set the motors to.
|
||
65 | * @param units The units the speed is expressed in.
|
||
66 | * @return Function status
|
||
67 | */
|
||
68 | a8480867 | Alex Zirbel | void MotorControl::set(int which, float speed, char units) |
69 | 2814387f | Alex Zirbel | { |
70 | float speed_fl, speed_fr, speed_bl, speed_br;
|
||
71 | speed_fl = speed_fr = speed_bl = speed_br = speed; |
||
72 | |||
73 | if(which & MOTOR_FL_REV)
|
||
74 | { |
||
75 | speed_fl = -speed; |
||
76 | } |
||
77 | if(which & MOTOR_FR_REV)
|
||
78 | { |
||
79 | speed_fr = -speed; |
||
80 | } |
||
81 | if(which & MOTOR_BL_REV)
|
||
82 | { |
||
83 | speed_bl = -speed; |
||
84 | } |
||
85 | if(which & MOTOR_BR_REV)
|
||
86 | { |
||
87 | speed_br = -speed; |
||
88 | } |
||
89 | |||
90 | a8480867 | Alex Zirbel | set_each(which, speed_fl, speed_fr, speed_bl, speed_br, units); |
91 | 2814387f | Alex Zirbel | } |
92 | |||
93 | /**
|
||
94 | * @brief Sets the left and right sides of scout to different speeds.
|
||
95 | *
|
||
96 | * @param speed_l The speed of both left motors.
|
||
97 | * @param speed_r The speed of both right motors.
|
||
98 | * @param units The units to set to.
|
||
99 | * @return Function status
|
||
100 | */
|
||
101 | a8480867 | Alex Zirbel | void MotorControl::set_sides(float speed_l, float speed_r, char units) |
102 | 2814387f | Alex Zirbel | { |
103 | a8480867 | Alex Zirbel | set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r, units); |
104 | 2814387f | Alex Zirbel | } |
105 | |||
106 | /**
|
||
107 | * @brief Set motor speeds
|
||
108 | 0121ead7 | bwasserm | * Sets the speeds of the motors as a percentage of top speed. Can selectively
|
109 | a8480867 | Alex Zirbel | * select which motors to set, and which to keep at previous speed.
|
110 | 2814387f | Alex Zirbel | *
|
111 | * @param which A bitmask of which motors should be set.
|
||
112 | * @param speed The speed the motors should be set to.
|
||
113 | * @param units Optional units for the speeds.
|
||
114 | * @return Function status
|
||
115 | 0121ead7 | bwasserm | */
|
116 | a8480867 | Alex Zirbel | void MotorControl::set_each(int which, |
117 | float speed_fl, float speed_fr, |
||
118 | float speed_bl, float speed_br, |
||
119 | char units)
|
||
120 | 4589a2a9 | Ben Wasserman | { |
121 | 2814387f | Alex Zirbel | check_which_ok(which); |
122 | |||
123 | motors::set_motors msg; |
||
124 | |||
125 | |||
126 | /* Tell the motors node which motors need to be updated */
|
||
127 | msg.fl_set = msg.fr_set = msg.bl_set = msg.br_set = false;
|
||
128 | if(which & (MOTOR_FL | MOTOR_FL_REV))
|
||
129 | { |
||
130 | msg.fl_set = true;
|
||
131 | a8480867 | Alex Zirbel | motor_fl_speed = rel_to_abs(speed_fl, units); |
132 | 2814387f | Alex Zirbel | } |
133 | if(which & (MOTOR_FR | MOTOR_FR_REV))
|
||
134 | { |
||
135 | msg.fr_set = true;
|
||
136 | a8480867 | Alex Zirbel | motor_fr_speed = rel_to_abs(speed_fr, units); |
137 | 2814387f | Alex Zirbel | } |
138 | if(which & (MOTOR_BL | MOTOR_BL_REV))
|
||
139 | { |
||
140 | msg.bl_set = true;
|
||
141 | a8480867 | Alex Zirbel | motor_bl_speed = rel_to_abs(speed_bl, units); |
142 | 2814387f | Alex Zirbel | } |
143 | if(which & (MOTOR_BR | MOTOR_BR_REV))
|
||
144 | { |
||
145 | msg.br_set = true;
|
||
146 | a8480867 | Alex Zirbel | motor_br_speed = rel_to_abs(speed_br, units); |
147 | 2814387f | Alex Zirbel | } |
148 | |||
149 | a8480867 | Alex Zirbel | /* Set all the speeds (the booleans in the set_motors message determine
|
150 | * which ones will be used) */
|
||
151 | msg.fl_speed = (int) motor_fl_speed;
|
||
152 | msg.fr_speed = (int) motor_fr_speed;
|
||
153 | msg.bl_speed = (int) motor_bl_speed;
|
||
154 | msg.br_speed = (int) motor_br_speed;
|
||
155 | 2814387f | Alex Zirbel | |
156 | /* Publishes message to set_motors topic */
|
||
157 | a8480867 | Alex Zirbel | set_motors_pub.publish(msg); |
158 | 2814387f | Alex Zirbel | ros::spinOnce(); |
159 | c406f16b | Ben | } |
160 | |||
161 | 2814387f | Alex Zirbel | /**
|
162 | * Double-checks the which variable to make sure the front and back
|
||
163 | * bits are not both set for a single motor.
|
||
164 | *
|
||
165 | * @param which The which motor specfication to check.
|
||
166 | */
|
||
167 | a8480867 | Alex Zirbel | void MotorControl::check_which_ok(int which) |
168 | 2814387f | Alex Zirbel | { |
169 | a8480867 | Alex Zirbel | ROS_ERROR_COND( ((which & MOTOR_FL) && (which & MOTOR_FL_REV)), |
170 | 2814387f | Alex Zirbel | "FL Motor set in both directions!");
|
171 | a8480867 | Alex Zirbel | ROS_ERROR_COND( ((which & MOTOR_FR) && (which & MOTOR_FR_REV)), |
172 | 2814387f | Alex Zirbel | "FR Motor set in both directions!");
|
173 | a8480867 | Alex Zirbel | ROS_ERROR_COND( ((which & MOTOR_BL) && (which & MOTOR_BL_REV)), |
174 | 2814387f | Alex Zirbel | "BL Motor set in both directions!");
|
175 | a8480867 | Alex Zirbel | ROS_ERROR_COND( ((which & MOTOR_BR) && (which & MOTOR_BR_REV)), |
176 | 2814387f | Alex Zirbel | "BR Motor set in both directions!");
|
177 | } |
||
178 | |||
179 | /**
|
||
180 | * @brief Query the current speeds of the motors
|
||
181 | 0121ead7 | bwasserm | *
|
182 | * Sends a request to the query_motors service which will reply with the
|
||
183 | * current speed of each motor.
|
||
184 | *
|
||
185 | a8480867 | Alex Zirbel | * @TODO Change so we can get multiple motor speeds with one call
|
186 | 2814387f | Alex Zirbel | *
|
187 | * @param which A bitmask that will specify which motor speed should be
|
||
188 | 0121ead7 | bwasserm | * returned
|
189 | cc558a8d | Alex Zirbel | * @return The speed of the selected motor
|
190 | 0121ead7 | bwasserm | */
|
191 | a8480867 | Alex Zirbel | float MotorControl::query(int which) |
192 | 4589a2a9 | Ben Wasserman | { |
193 | 2814387f | Alex Zirbel | motors::query_motors srv; |
194 | if(query_motors_client.call(srv))
|
||
195 | { |
||
196 | switch(which)
|
||
197 | { |
||
198 | case MOTOR_FL:
|
||
199 | return srv.response.fl_speed;
|
||
200 | case MOTOR_FR:
|
||
201 | return srv.response.fr_speed;
|
||
202 | case MOTOR_BL:
|
||
203 | return srv.response.bl_speed;
|
||
204 | case MOTOR_BR:
|
||
205 | return srv.response.br_speed;
|
||
206 | default:
|
||
207 | cc558a8d | Alex Zirbel | /// @todo: I hate this. Let's fix it soon.
|
208 | ROS_FATAL("Bad WHICH in motors_query.");
|
||
209 | 2814387f | Alex Zirbel | } |
210 | } |
||
211 | else
|
||
212 | { |
||
213 | cc558a8d | Alex Zirbel | ROS_FATAL("Failed to call service query_motors");
|
214 | c406f16b | Ben | } |
215 | 2814387f | Alex Zirbel | |
216 | return 0; |
||
217 | c406f16b | Ben | } |
218 | a8480867 | Alex Zirbel | |
219 | /**
|
||
220 | * @brief Converts set speeds (of various units) to absolute speeds.
|
||
221 | *
|
||
222 | * @param speed The speed expressed in the desired units
|
||
223 | * @param units The units the desired speed is measured in
|
||
224 | * @return The absolute speed of the motor
|
||
225 | **/
|
||
226 | float MotorControl::rel_to_abs(float rel_speed, int units) |
||
227 | { |
||
228 | switch(units)
|
||
229 | { |
||
230 | case MOTOR_ABSOLUTE:/* Speed given as absolute */ |
||
231 | return rel_speed;
|
||
232 | case MOTOR_PERCENT:/* Convert from percentage */ |
||
233 | return rel_speed * MAXSPEED / 100; |
||
234 | case MOTOR_MMS:/* Convert from mm/s */ |
||
235 | /** @todo Make math to do this conversion **/
|
||
236 | return rel_speed;
|
||
237 | case MOTOR_CMS:/* Convert from cm/s */ |
||
238 | /** @todo Make math to do this conversion **/
|
||
239 | return rel_speed;
|
||
240 | default: /* The units aren't recognized */ |
||
241 | /** @todo Decide on default case. Either percent or absolute. **/
|
||
242 | return rel_speed;
|
||
243 | } |
||
244 | } |
||
245 | |||
246 | /**
|
||
247 | * @brief Convert absolute speeds to speeds of various units.
|
||
248 | *
|
||
249 | * @param speed The speed expressed in absolute units.
|
||
250 | * @param units The units the desired speed is measured in.
|
||
251 | * @return The relative speed of the motor in desired units.
|
||
252 | **/
|
||
253 | float MotorControl::abs_to_rel(float abs_speed, int units) |
||
254 | { |
||
255 | switch(units)
|
||
256 | { |
||
257 | case MOTOR_ABSOLUTE:/* Speed given as absolute */ |
||
258 | return abs_speed;
|
||
259 | case MOTOR_PERCENT:/* Convert from percentage */ |
||
260 | return abs_speed * 100 / MAXSPEED; |
||
261 | case MOTOR_MMS:/* Convert from mm/s */ |
||
262 | /** @todo Make math to do this conversion **/
|
||
263 | return abs_speed;
|
||
264 | case MOTOR_CMS:/* Convert from cm/s */ |
||
265 | /** @todo Make math to do this conversion **/
|
||
266 | return abs_speed;
|
||
267 | default: /* The units aren't recognized */ |
||
268 | /** @todo Decide on default case. Either percent or absolute. **/
|
||
269 | return abs_speed;
|
||
270 | } |
||
271 | } |