Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / MotorControl.cpp @ 4e393fcc

History | View | Annotate | Download (8.24 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<motors::set_motors>(scoutname + "/set_motors",
54
                                           QUEUE_SIZE);
55
    query_motors_client =
56
        node.serviceClient<motors::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
    motors::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 = (int) motor_fl_speed;
157
    msg.fr_speed = (int) motor_fr_speed;
158
    msg.bl_speed = (int) motor_bl_speed;
159
    msg.br_speed = (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
 * @brief Query the current speeds of the motors
186
 *
187
 * Sends a request to the query_motors service which will reply with the
188
 *  current speed of each motor.
189
 *
190
 * @TODO Change so we can get multiple motor speeds with one call
191
 *
192
 * @param which A bitmask that will specify which motor speed should be
193
 *  returned
194
 * @return The speed of the selected motor
195
 */
196
float MotorControl::query(int which)
197
{
198
    motors::query_motors srv;
199
    if(query_motors_client.call(srv))
200
    {
201
        switch(which)
202
        {
203
            case MOTOR_FL:
204
                return srv.response.fl_speed;
205
            case MOTOR_FR:
206
                return srv.response.fr_speed;
207
            case MOTOR_BL:
208
                return srv.response.bl_speed;
209
            case MOTOR_BR:
210
                return srv.response.br_speed;
211
            default:
212
                /// @todo: I hate this. Let's fix it soon.
213
                ROS_FATAL("Bad WHICH in motors_query.");
214
        }
215
    }
216
    else
217
    {
218
        ROS_FATAL("Failed to call service query_motors");
219
    }
220

    
221
    return 0;
222
}
223

    
224
/**
225
 * @brief Converts set speeds (of various units) to absolute speeds.
226
 *
227
 * @param speed The speed expressed in the desired units
228
 * @param units The units the desired speed is measured in
229
 * @return The absolute speed of the motor
230
 **/
231
float MotorControl::rel_to_abs(float rel_speed, int units)
232
{
233
    switch(units)
234
    {
235
      case MOTOR_ABSOLUTE:/* Speed given as absolute */
236
        return rel_speed;
237
      case MOTOR_PERCENT:/* Convert from percentage */
238
        return rel_speed * MAXSPEED_ABS / 100.0;
239
      case MOTOR_MMS:/* Convert from mm/s */
240
        return rel_speed * MAXSPEED_ABS * 100.0 / MAXSPEED_MPS;
241
      case MOTOR_CMS:/* Convert from cm/s */
242
        return rel_speed * MAXSPEED_ABS * 10.0 / MAXSPEED_MPS;
243
      default: /* The units aren't recognized */
244
        ROS_WARN("MotorControl::rel_to_abs used with default units.");
245
        return rel_speed;
246
    }
247
}
248

    
249
/**
250
 * @brief Convert absolute speeds to speeds of various units.
251
 *
252
 * @param speed The speed expressed in absolute units.
253
 * @param units The units the desired speed is measured in.
254
 * @return The relative speed of the motor in desired units.
255
 **/
256
float MotorControl::abs_to_rel(float abs_speed, int units)
257
{
258
    switch(units)
259
    {
260
      case MOTOR_ABSOLUTE:/* Speed given as absolute */
261
        return abs_speed;
262
      case MOTOR_PERCENT:/* Convert from percentage */
263
        return abs_speed * 100.0 / MAXSPEED_ABS;
264
      case MOTOR_MMS:/* Convert from mm/s */
265
        return abs_speed * MAXSPEED_MPS / (MAXSPEED_ABS * 100.0);
266
      case MOTOR_CMS:/* Convert from cm/s */
267
        return abs_speed * MAXSPEED_MPS / (MAXSPEED_ABS * 10.0);
268
      default: /* The units aren't recognized */
269
        ROS_WARN("MotorControl::rel_to_abs used with default units.");
270
        return abs_speed;
271
    }
272
}