Statistics
| Branch: | Revision:

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
}