Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / MotorControl.cpp @ 3a73516c

History | View | Annotate | Download (9.28 KB)

1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
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
 * @file MotorControl.cpp
28
 * @brief Contains motor declarations and functions
29
 * 
30
 * @defgroup motorcontrol MotorControl
31
 * @brief Functions which a behavior can use to control the motors.
32
 * @ingroup behavior
33
 *
34
 * @author Colony Project, CMU Robotics Club
35
 * @author Ben Wasserman
36
 * @author Alex Zirbel
37
 *
38
 * @{
39
 **/
40

    
41
#include "MotorControl.h"
42

    
43
using namespace std;
44

    
45
/**
46
 * @brief Initialize the motors module of libscout.
47
 *
48
 * Initialize the libscout node as a publisher of set_motors and a client of
49
 * query_motors.
50
 */
51
MotorControl::MotorControl(const ros::NodeHandle& libscout_node,
52
                           string scoutname)
53
    : node(libscout_node)
54
{
55
    set_motors_pub =
56
        node.advertise< ::messages::set_motors>(scoutname + "/set_motors",
57
                                           QUEUE_SIZE, true);
58
    query_motors_client =
59
        node.serviceClient< ::messages::query_motors>(scoutname + "/query_motors");
60
}
61

    
62
MotorControl::~MotorControl()
63
{
64
    set_sides(0, 0, MOTOR_ABSOLUTE);
65
}
66

    
67
/**
68
 * @brief Sets all the speeds according to the specificiation of which motors
69
 *        to set.
70
 *
71
 * @param which A bitmask of which motors need to be set.
72
 * @param speed The value to set the motors to.
73
 * @param units The units the speed is expressed in.
74
 * @return Function status
75
 */
76
void MotorControl::set(int which, float speed, char units)
77
{
78
    float speed_fl, speed_fr, speed_bl, speed_br;
79
    speed_fl = speed_fr = speed_bl = speed_br = speed;
80

    
81
    if(which & MOTOR_FL_REV)
82
    {
83
        speed_fl = -speed;
84
    }
85
    if(which & MOTOR_FR_REV)
86
    {
87
        speed_fr = -speed;
88
    }
89
    if(which & MOTOR_BL_REV)
90
    {
91
        speed_bl = -speed;
92
    }
93
    if(which & MOTOR_BR_REV)
94
    {
95
        speed_br = -speed;
96
    }
97

    
98
    set_each(which, speed_fl, speed_fr, speed_bl, speed_br, units);
99
}
100

    
101
/**
102
 * @brief Sets the left and right sides of scout to different speeds.
103
 *
104
 * @param speed_l The speed of both left motors.
105
 * @param speed_r The speed of both right motors.
106
 * @param units The units to set to.
107
 * @return Function status
108
 */
109
void MotorControl::set_sides(float speed_l, float speed_r, char units)
110
{
111
    set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r, units);
112
}
113

    
114
/**
115
 * @brief Set motor speeds
116
 * Sets the speeds of the motors as a percentage of top speed. Can selectively
117
 * select which motors to set, and which to keep at previous speed.
118
 *
119
 * @param which A bitmask of which motors should be set.
120
 * @param speed The speed the motors should be set to.
121
 * @param units Optional units for the speeds.
122
 * @return Function status
123
 */
124
void MotorControl::set_each(int which,
125
                            float speed_fl, float speed_fr,
126
                            float speed_bl, float speed_br,
127
                            char units)
128
{
129
    check_which_ok(which);
130

    
131
    ::messages::set_motors msg;
132

    
133

    
134
    /* Tell the motors node which motors need to be updated */
135
    msg.fl_set = msg.fr_set = msg.bl_set = msg.br_set = false;
136
    if(which & (MOTOR_FL | MOTOR_FL_REV))
137
    {
138
        msg.fl_set = true;
139
        motor_fl_speed = rel_to_abs(speed_fl, units);
140
    }
141
    if(which & (MOTOR_FR | MOTOR_FR_REV))
142
    {
143
        msg.fr_set = true;
144
        motor_fr_speed = rel_to_abs(speed_fr, units);
145
    }
146
    if(which & (MOTOR_BL | MOTOR_BL_REV))
147
    {
148
        msg.bl_set = true;
149
        motor_bl_speed = rel_to_abs(speed_bl, units);
150
    }
151
    if(which & (MOTOR_BR | MOTOR_BR_REV))
152
    {
153
        msg.br_set = true;
154
        motor_br_speed = rel_to_abs(speed_br, units);
155
    }
156

    
157
    /* Set all the speeds (the booleans in the set_motors message determine
158
     * which ones will be used) */
159
    msg.fl_speed = trim_speed("FL", (int) motor_fl_speed);
160
    msg.fr_speed = trim_speed("FR", (int) motor_fr_speed);
161
    msg.bl_speed = trim_speed("BL", (int) motor_bl_speed);
162
    msg.br_speed = trim_speed("BR", (int) motor_br_speed);
163

    
164
    /* Publishes message to set_motors topic */
165
    set_motors_pub.publish(msg);
166
    ros::spinOnce();
167
}
168

    
169
/**
170
 * Double-checks the which variable to make sure the front and back
171
 * bits are not both set for a single motor.
172
 *
173
 * @param which The which motor specfication to check.
174
 */
175
void MotorControl::check_which_ok(int which)
176
{
177
    ROS_ERROR_COND( ((which & MOTOR_FL) && (which & MOTOR_FL_REV)),
178
        "FL Motor set in both directions!");
179
    ROS_ERROR_COND( ((which & MOTOR_FR) && (which & MOTOR_FR_REV)),
180
        "FR Motor set in both directions!");
181
    ROS_ERROR_COND( ((which & MOTOR_BL) && (which & MOTOR_BL_REV)),
182
        "BL Motor set in both directions!");
183
    ROS_ERROR_COND( ((which & MOTOR_BR) && (which & MOTOR_BR_REV)),
184
        "BR Motor set in both directions!");
185
}
186

    
187
/**
188
 * Trims an absolute speed to stay within motor maximum range.
189
 * Uses ROS_WARN to warn the user if a speed has been trimmed.
190
 *
191
 * @param which_str A description of which motor, to be used in the warning
192
 *                  message (for example, "FL")
193
 * @param speed The requested speed to be set to
194
 * @return The requested speed, trimmed to stay within limits.
195
 */
196
int MotorControl::trim_speed(string which_str, int speed)
197
{
198
    int trimmed_speed = speed;
199
    if (speed > MAXSPEED_ABS)
200
    {
201
        trimmed_speed = (int) MAXSPEED_ABS;
202
        ROS_WARN("Motor (%s) speed trimmed to %d.", which_str.c_str(),
203
            (int) MAXSPEED_ABS);
204
    }
205
    else if (speed < -MAXSPEED_ABS)
206
    {
207
        trimmed_speed = (int) -MAXSPEED_ABS;
208
        ROS_WARN("Motor (%s) speed trimmed to %d.", which_str.c_str(),
209
            (int) -MAXSPEED_ABS);
210
    }
211

    
212
    return trimmed_speed;
213
}
214

    
215
/**
216
 * @brief Query the current speeds of the motors
217
 *
218
 * Sends a request to the query_motors service which will reply with the
219
 *  current speed of each motor.
220
 *
221
 * @TODO Change so we can get multiple motor speeds with one call
222
 *
223
 * @param which A bitmask that will specify which motor speed should be
224
 *  returned
225
 * @return The speed of the selected motor
226
 */
227
float MotorControl::query(int which)
228
{
229
    ::messages::query_motors srv;
230
    if(query_motors_client.call(srv))
231
    {
232
        switch(which)
233
        {
234
            case MOTOR_FL:
235
                return srv.response.fl_speed;
236
            case MOTOR_FR:
237
                return srv.response.fr_speed;
238
            case MOTOR_BL:
239
                return srv.response.bl_speed;
240
            case MOTOR_BR:
241
                return srv.response.br_speed;
242
            default:
243
                /// @todo: I hate this. Let's fix it soon.
244
                ROS_FATAL("Bad WHICH in motors_query.");
245
        }
246
    }
247
    else
248
    {
249
        ROS_FATAL("Failed to call service query_motors");
250
    }
251

    
252
    return 0;
253
}
254

    
255
/**
256
 * @brief Converts set speeds (of various units) to absolute speeds.
257
 *
258
 * @param speed The speed expressed in the desired units
259
 * @param units The units the desired speed is measured in
260
 * @return The absolute speed of the motor
261
 **/
262
float MotorControl::rel_to_abs(float rel_speed, int units)
263
{
264
    switch(units)
265
    {
266
      case MOTOR_ABSOLUTE:/* Speed given as absolute */
267
        return rel_speed;
268
      case MOTOR_PERCENT:/* Convert from percentage */
269
        return rel_speed * MAXSPEED_ABS / 100.0;
270
      case MOTOR_MMS:/* Convert from mm/s */
271
        return rel_speed * MAXSPEED_ABS / (1000.0 * MAXSPEED_MPS);
272
      case MOTOR_CMS:/* Convert from cm/s */
273
        return rel_speed * MAXSPEED_ABS / (100.0 * MAXSPEED_MPS);
274
      default: /* The units aren't recognized */
275
        ROS_WARN("MotorControl::rel_to_abs used with default units.");
276
        return rel_speed;
277
    }
278
}
279

    
280
/**
281
 * @brief Convert absolute speeds to speeds of various units.
282
 *
283
 * @param speed The speed expressed in absolute units.
284
 * @param units The units the desired speed is measured in.
285
 * @return The relative speed of the motor in desired units.
286
 **/
287
float MotorControl::abs_to_rel(float abs_speed, int units)
288
{
289
    switch(units)
290
    {
291
      case MOTOR_ABSOLUTE:/* Speed given as absolute */
292
        return abs_speed;
293
      case MOTOR_PERCENT:/* Convert from percentage */
294
        return abs_speed * 100.0 / MAXSPEED_ABS;
295
      case MOTOR_MMS:/* Convert from mm/s */
296
        return abs_speed * MAXSPEED_MPS * 1000.0 / MAXSPEED_ABS;
297
      case MOTOR_CMS:/* Convert from cm/s */
298
        return abs_speed * MAXSPEED_MPS * 100.0 / MAXSPEED_ABS;
299
      default: /* The units aren't recognized */
300
        ROS_WARN("MotorControl::rel_to_abs used with default units.");
301
        return abs_speed;
302
    }
303
}
304

    
305
/** @} */