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