root / scout / libscout / src / libmotors.cpp @ 4589a2a9
History | View | Annotate | Download (3.98 KB)
| 1 | c406f16b | Ben | /**
|
|---|---|---|---|
| 2 | 0121ead7 | bwasserm | * Copyright (c) 2011 Colony Project
|
| 3 | c406f16b | Ben | *
|
| 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 | 0121ead7 | bwasserm | /* 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 | 4589a2a9 | Ben Wasserman | void libmotors_init()
|
| 53 | {
|
||
| 54 | c406f16b | Ben | 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 | 0121ead7 | bwasserm | /*!
|
| 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 | 4589a2a9 | Ben Wasserman | int motors_set(int speed, int which, char units=MOTOR_PERCENT) |
| 67 | {
|
||
| 68 | c406f16b | Ben | /** \todo Set fields of the message based on params */
|
| 69 | |||
| 70 | 4589a2a9 | Ben Wasserman | motors::set_motors msg; |
| 71 | |||
| 72 | if(!ros::ok())
|
||
| 73 | {
|
||
| 74 | c406f16b | Ben | return LIB_ERROR;
|
| 75 | } |
||
| 76 | 0121ead7 | bwasserm | |
| 77 | /* Set the speed for each motor according to the which bitmask*/
|
||
| 78 | 4589a2a9 | Ben Wasserman | |
| 79 | if(which & MOTOR_FL_REV)
|
||
| 80 | {
|
||
| 81 | 0121ead7 | bwasserm | msg.fl_speed = -1 * speed;
|
| 82 | } |
||
| 83 | 4589a2a9 | Ben Wasserman | if(which & MOTOR_FR_REV)
|
| 84 | {
|
||
| 85 | 0121ead7 | bwasserm | msg.fr_speed = -1 * speed;
|
| 86 | } |
||
| 87 | 4589a2a9 | Ben Wasserman | if(which & MOTOR_BL_REV)
|
| 88 | {
|
||
| 89 | 0121ead7 | bwasserm | msg.bl_speed = -1 * speed;
|
| 90 | } |
||
| 91 | 4589a2a9 | Ben Wasserman | if(which & MOTOR_BR_REV)
|
| 92 | {
|
||
| 93 | 0121ead7 | bwasserm | msg.br_speed = -1 * speed;
|
| 94 | 4589a2a9 | Ben Wasserman | }if(which & MOTOR_FL)
|
| 95 | {
|
||
| 96 | 0121ead7 | bwasserm | msg.fl_speed = speed; |
| 97 | } |
||
| 98 | 4589a2a9 | Ben Wasserman | if(which & MOTOR_FR)
|
| 99 | {
|
||
| 100 | 0121ead7 | bwasserm | msg.fr_speed = speed; |
| 101 | } |
||
| 102 | 4589a2a9 | Ben Wasserman | if(which & MOTOR_BL)
|
| 103 | {
|
||
| 104 | 0121ead7 | bwasserm | msg.bl_speed = speed; |
| 105 | } |
||
| 106 | 4589a2a9 | Ben Wasserman | if(which & MOTOR_BR)
|
| 107 | {
|
||
| 108 | 0121ead7 | bwasserm | msg.br_speed = speed; |
| 109 | } |
||
| 110 | |||
| 111 | /* Specify which units the speeds are given in */
|
||
| 112 | msg.units = units; |
||
| 113 | c406f16b | Ben | |
| 114 | 0121ead7 | bwasserm | /* Publishes message to set_motors topic */
|
| 115 | set_motors_publisher.publish(msg); |
||
| 116 | c406f16b | Ben | ros::spinOnce(); |
| 117 | |||
| 118 | return LIB_OK;
|
||
| 119 | } |
||
| 120 | |||
| 121 | 0121ead7 | bwasserm | /*!
|
| 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 | c406f16b | Ben | |
| 132 | 4589a2a9 | Ben Wasserman | int motors_query(int which) |
| 133 | {
|
||
| 134 | c406f16b | Ben | motors::query_motors srv; |
| 135 | 4589a2a9 | Ben Wasserman | if(query_motors_client.call(srv))
|
| 136 | {
|
||
| 137 | switch(which)
|
||
| 138 | {
|
||
| 139 | c406f16b | Ben | 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 | 0121ead7 | bwasserm | default:
|
| 148 | c406f16b | Ben | ROS_WARN("Bad WHICH in motors_query.");
|
| 149 | return LIB_ERROR;
|
||
| 150 | } |
||
| 151 | 4589a2a9 | Ben Wasserman | } |
| 152 | else
|
||
| 153 | {
|
||
| 154 | c406f16b | Ben | ROS_ERROR("Failed to call service query_motors");
|
| 155 | return LIB_ERROR;
|
||
| 156 | } |
||
| 157 | |||
| 158 | return 0; |
||
| 159 | } |