Statistics
| Branch: | Revision:

root / scout / libscout / src / libmotors.cpp @ 4589a2a9

History | View | Annotate | Download (3.98 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
/**
28
 * @file libmotors.cpp
29
 * @brief Contains motor declarations and functions
30
 * 
31
 * Contains functions and definitions for the use of
32
 * motors
33
 *
34
 * @author Colony Project, CMU Robotics Club
35
 **/
36

    
37
#include "libmotors.h"
38

    
39
/* ROS node created in libscout.cpp */
40
extern ros::NodeHandle libscout_node;
41

    
42
/** ROS publisher and client declaration */
43
ros::Publisher set_motors_publisher;
44
ros::ServiceClient query_motors_client;
45

    
46
/*!
47
 * \brief Initialize the motors module of libscout.
48
 *
49
 * Initialize the libscout node as a publisher of set_motors and a client of
50
 * query_motors.
51
 */
52
void libmotors_init()
53
{
54
  set_motors_publisher = libscout_node.advertise<motors::set_motors>("libmotors", 10);
55
  query_motors_client = libscout_node.serviceClient<motors::query_motors>("libmotors");
56
}
57

    
58
/*!
59
 * \brief Set motor speeds
60
 * Sets the speeds of the motors as a percentage of top speed. Can selectively
61
 * select which motors to set, and which to remain at previous speed.
62
 * \param speed The speed the motors should be set to
63
 * \param which A bitmask of which motors should be set
64
 * \return Function status
65
 */
66
int motors_set(int speed, int which, char units=MOTOR_PERCENT)
67
{
68
  /** \todo Set fields of the message based on params */
69
  
70
        motors::set_motors msg;
71
        
72
  if(!ros::ok())
73
        {
74
    return LIB_ERROR;
75
  }
76
 
77
  /* Set the speed for each motor according to the which bitmask*/
78
  
79
  if(which & MOTOR_FL_REV)
80
        {
81
    msg.fl_speed = -1 * speed;
82
  }
83
  if(which & MOTOR_FR_REV)
84
        {
85
    msg.fr_speed = -1 * speed;
86
  }
87
  if(which & MOTOR_BL_REV)
88
        {
89
    msg.bl_speed = -1 * speed;
90
  }
91
  if(which & MOTOR_BR_REV)
92
        {
93
    msg.br_speed = -1 * speed;
94
  }if(which & MOTOR_FL)
95
        {
96
    msg.fl_speed = speed;
97
  }
98
  if(which & MOTOR_FR)
99
        {
100
    msg.fr_speed = speed;
101
  }
102
  if(which & MOTOR_BL)
103
        {
104
    msg.bl_speed = speed;
105
  }
106
  if(which & MOTOR_BR)
107
        {
108
    msg.br_speed = speed;
109
  }
110

    
111
  /* Specify which units the speeds are given in */
112
  msg.units = units;
113

    
114
  /* Publishes message to set_motors topic */
115
  set_motors_publisher.publish(msg);
116
  ros::spinOnce();
117

    
118
  return LIB_OK;
119
}
120

    
121
/*!
122
 * \brief Query the current speeds of the motors
123
 *
124
 * Sends a request to the query_motors service which will reply with the
125
 *  current speed of each motor.
126
 *
127
 * \param which A bitmask that will specify which motor speed should be
128
 *  returned
129
 * \return The speed of the selected motor, or LIB_ERR if no motor selected
130
 */
131

    
132
int motors_query(int which)
133
{
134
  motors::query_motors srv;
135
  if(query_motors_client.call(srv))
136
        {
137
    switch(which)
138
                {
139
      case MOTOR_FL:
140
        return srv.response.fl_speed;
141
      case MOTOR_FR:
142
        return srv.response.fr_speed;
143
      case MOTOR_BL:
144
        return srv.response.bl_speed;
145
      case MOTOR_BR:
146
        return srv.response.br_speed;
147
      default:
148
        ROS_WARN("Bad WHICH in motors_query.");
149
        return LIB_ERROR;
150
    }
151
  }
152
        else
153
        {
154
    ROS_ERROR("Failed to call service query_motors");
155
    return LIB_ERROR;
156
  }
157

    
158
  return 0;
159
}