Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / MotorControl.cpp @ 7db6cf9f

History | View | Annotate | Download (9.2 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
 * Contains functions and definitions for the use of
31
 * motors
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 * @author Ben Wasserman
35
 * @author Alex Zirbel
36
 **/
37

    
38
#include "MotorControl.h"
39

    
40
using namespace std;
41

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

    
59
MotorControl::~MotorControl()
60
{
61
    set_sides(0, 0, MOTOR_ABSOLUTE);
62
}
63

    
64
/**
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
void MotorControl::set(int which, float speed, char units)
74
{
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
    set_each(which, speed_fl, speed_fr, speed_bl, speed_br, units);
96
}
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
void MotorControl::set_sides(float speed_l, float speed_r, char units)
107
{
108
    set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r, units);
109
}
110

    
111
/**
112
 * @brief Set motor speeds
113
 * Sets the speeds of the motors as a percentage of top speed. Can selectively
114
 * select which motors to set, and which to keep at previous speed.
115
 *
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
 */
121
void MotorControl::set_each(int which,
122
                            float speed_fl, float speed_fr,
123
                            float speed_bl, float speed_br,
124
                            char units)
125
{
126
    check_which_ok(which);
127

    
128
    ::messages::set_motors msg;
129

    
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
        motor_fl_speed = rel_to_abs(speed_fl, units);
137
    }
138
    if(which & (MOTOR_FR | MOTOR_FR_REV))
139
    {
140
        msg.fr_set = true;
141
        motor_fr_speed = rel_to_abs(speed_fr, units);
142
    }
143
    if(which & (MOTOR_BL | MOTOR_BL_REV))
144
    {
145
        msg.bl_set = true;
146
        motor_bl_speed = rel_to_abs(speed_bl, units);
147
    }
148
    if(which & (MOTOR_BR | MOTOR_BR_REV))
149
    {
150
        msg.br_set = true;
151
        motor_br_speed = rel_to_abs(speed_br, units);
152
    }
153

    
154
    /* Set all the speeds (the booleans in the set_motors message determine
155
     * which ones will be used) */
156
    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

    
161
    /* Publishes message to set_motors topic */
162
    set_motors_pub.publish(msg);
163
    ros::spinOnce();
164
}
165

    
166
/**
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
void MotorControl::check_which_ok(int which)
173
{
174
    ROS_ERROR_COND( ((which & MOTOR_FL) && (which & MOTOR_FL_REV)),
175
        "FL Motor set in both directions!");
176
    ROS_ERROR_COND( ((which & MOTOR_FR) && (which & MOTOR_FR_REV)),
177
        "FR Motor set in both directions!");
178
    ROS_ERROR_COND( ((which & MOTOR_BL) && (which & MOTOR_BL_REV)),
179
        "BL Motor set in both directions!");
180
    ROS_ERROR_COND( ((which & MOTOR_BR) && (which & MOTOR_BR_REV)),
181
        "BR Motor set in both directions!");
182
}
183

    
184
/**
185
 * 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
 * @brief Query the current speeds of the motors
214
 *
215
 * Sends a request to the query_motors service which will reply with the
216
 *  current speed of each motor.
217
 *
218
 * @todo Change so we can get multiple motor speeds with one call
219
 *
220
 * @param which A bitmask that will specify which motor speed should be
221
 *  returned
222
 * @return The speed of the selected motor
223
 */
224
float MotorControl::query(int which)
225
{
226
    ::messages::query_motors srv;
227
    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
                /// @todo: I hate this. Let's fix it soon.
241
                ROS_FATAL("Bad WHICH in motors_query.");
242
        }
243
    }
244
    else
245
    {
246
        ROS_FATAL("Failed to call service query_motors");
247
    }
248

    
249
    return 0;
250
}
251

    
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
        return rel_speed * MAXSPEED_ABS / 100.0;
267
      case MOTOR_MMS:/* Convert from mm/s */
268
        return rel_speed * MAXSPEED_ABS / (1000.0 * MAXSPEED_MPS);
269
      case MOTOR_CMS:/* Convert from cm/s */
270
        return rel_speed * MAXSPEED_ABS / (100.0 * MAXSPEED_MPS);
271
      default: /* The units aren't recognized */
272
        ROS_WARN("MotorControl::rel_to_abs used with default units.");
273
        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
        return abs_speed * 100.0 / MAXSPEED_ABS;
292
      case MOTOR_MMS:/* Convert from mm/s */
293
        return abs_speed * MAXSPEED_MPS * 1000.0 / MAXSPEED_ABS;
294
      case MOTOR_CMS:/* Convert from cm/s */
295
        return abs_speed * MAXSPEED_MPS * 100.0 / MAXSPEED_ABS;
296
      default: /* The units aren't recognized */
297
        ROS_WARN("MotorControl::rel_to_abs used with default units.");
298
        return abs_speed;
299
    }
300
}