Project

General

Profile

Statistics
| Branch: | Revision:

root / scout / libscout / src / MotorControl.cpp @ c492be62

History | View | Annotate | Download (8.22 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
/**
60
 * @brief Sets all the speeds according to the specificiation of which motors
61
 *        to set.
62
 *
63
 * @param which A bitmask of which motors need to be set.
64
 * @param speed The value to set the motors to.
65
 * @param units The units the speed is expressed in.
66
 * @return Function status
67
 */
68
void MotorControl::set(int which, float speed, char units)
69
{
70
    float speed_fl, speed_fr, speed_bl, speed_br;
71
    speed_fl = speed_fr = speed_bl = speed_br = speed;
72

    
73
    if(which & MOTOR_FL_REV)
74
    {
75
        speed_fl = -speed;
76
    }
77
    if(which & MOTOR_FR_REV)
78
    {
79
        speed_fr = -speed;
80
    }
81
    if(which & MOTOR_BL_REV)
82
    {
83
        speed_bl = -speed;
84
    }
85
    if(which & MOTOR_BR_REV)
86
    {
87
        speed_br = -speed;
88
    }
89

    
90
    set_each(which, speed_fl, speed_fr, speed_bl, speed_br, units);
91
}
92

    
93
/**
94
 * @brief Sets the left and right sides of scout to different speeds.
95
 *
96
 * @param speed_l The speed of both left motors.
97
 * @param speed_r The speed of both right motors.
98
 * @param units The units to set to.
99
 * @return Function status
100
 */
101
void MotorControl::set_sides(float speed_l, float speed_r, char units)
102
{
103
    set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r, units);
104
}
105

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

    
123
    motors::set_motors msg;
124

    
125

    
126
    /* Tell the motors node which motors need to be updated */
127
    msg.fl_set = msg.fr_set = msg.bl_set = msg.br_set = false;
128
    if(which & (MOTOR_FL | MOTOR_FL_REV))
129
    {
130
        msg.fl_set = true;
131
        motor_fl_speed = rel_to_abs(speed_fl, units);
132
    }
133
    if(which & (MOTOR_FR | MOTOR_FR_REV))
134
    {
135
        msg.fr_set = true;
136
        motor_fr_speed = rel_to_abs(speed_fr, units);
137
    }
138
    if(which & (MOTOR_BL | MOTOR_BL_REV))
139
    {
140
        msg.bl_set = true;
141
        motor_bl_speed = rel_to_abs(speed_bl, units);
142
    }
143
    if(which & (MOTOR_BR | MOTOR_BR_REV))
144
    {
145
        msg.br_set = true;
146
        motor_br_speed = rel_to_abs(speed_br, units);
147
    }
148

    
149
    /* Set all the speeds (the booleans in the set_motors message determine
150
     * which ones will be used) */
151
    msg.fl_speed = (int) motor_fl_speed;
152
    msg.fr_speed = (int) motor_fr_speed;
153
    msg.bl_speed = (int) motor_bl_speed;
154
    msg.br_speed = (int) motor_br_speed;
155

    
156
    /* Publishes message to set_motors topic */
157
    set_motors_pub.publish(msg);
158
    ros::spinOnce();
159
}
160

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

    
179
/**
180
 * @brief Query the current speeds of the motors
181
 *
182
 * Sends a request to the query_motors service which will reply with the
183
 *  current speed of each motor.
184
 *
185
 * @TODO Change so we can get multiple motor speeds with one call
186
 *
187
 * @param which A bitmask that will specify which motor speed should be
188
 *  returned
189
 * @return The speed of the selected motor
190
 */
191
float MotorControl::query(int which)
192
{
193
    motors::query_motors srv;
194
    if(query_motors_client.call(srv))
195
    {
196
        switch(which)
197
        {
198
            case MOTOR_FL:
199
                return srv.response.fl_speed;
200
            case MOTOR_FR:
201
                return srv.response.fr_speed;
202
            case MOTOR_BL:
203
                return srv.response.bl_speed;
204
            case MOTOR_BR:
205
                return srv.response.br_speed;
206
            default:
207
                /// @todo: I hate this. Let's fix it soon.
208
                ROS_FATAL("Bad WHICH in motors_query.");
209
        }
210
    }
211
    else
212
    {
213
        ROS_FATAL("Failed to call service query_motors");
214
    }
215

    
216
    return 0;
217
}
218

    
219
/**
220
 * @brief Converts set speeds (of various units) to absolute speeds.
221
 *
222
 * @param speed The speed expressed in the desired units
223
 * @param units The units the desired speed is measured in
224
 * @return The absolute speed of the motor
225
 **/
226
float MotorControl::rel_to_abs(float rel_speed, int units)
227
{
228
    switch(units)
229
    {
230
      case MOTOR_ABSOLUTE:/* Speed given as absolute */
231
        return rel_speed;
232
      case MOTOR_PERCENT:/* Convert from percentage */
233
        return rel_speed * MAXSPEED / 100;
234
      case MOTOR_MMS:/* Convert from mm/s */
235
        /** @todo Make math to do this conversion **/
236
        return rel_speed;
237
      case MOTOR_CMS:/* Convert from cm/s */
238
        /** @todo Make math to do this conversion **/
239
        return rel_speed;
240
      default: /* The units aren't recognized */
241
        /** @todo Decide on default case. Either percent or absolute. **/
242
        return rel_speed;
243
    }
244
}
245

    
246
/**
247
 * @brief Convert absolute speeds to speeds of various units.
248
 *
249
 * @param speed The speed expressed in absolute units.
250
 * @param units The units the desired speed is measured in.
251
 * @return The relative speed of the motor in desired units.
252
 **/
253
float MotorControl::abs_to_rel(float abs_speed, int units)
254
{
255
    switch(units)
256
    {
257
      case MOTOR_ABSOLUTE:/* Speed given as absolute */
258
        return abs_speed;
259
      case MOTOR_PERCENT:/* Convert from percentage */
260
        return abs_speed * 100 / MAXSPEED;
261
      case MOTOR_MMS:/* Convert from mm/s */
262
        /** @todo Make math to do this conversion **/
263
        return abs_speed;
264
      case MOTOR_CMS:/* Convert from cm/s */
265
        /** @todo Make math to do this conversion **/
266
        return abs_speed;
267
      default: /* The units aren't recognized */
268
        /** @todo Decide on default case. Either percent or absolute. **/
269
        return abs_speed;
270
    }
271
}