Revision 4589a2a9
Deleted compiler output files that were accidentally included in analog.
| scout/libscout/src/libmotors.cpp | ||
|---|---|---|
| 34 | 34 |
* @author Colony Project, CMU Robotics Club |
| 35 | 35 |
**/ |
| 36 | 36 |
|
| 37 |
/* Author: Ben Wasserman |
|
| 38 |
*/ |
|
| 39 |
|
|
| 40 | 37 |
#include "libmotors.h" |
| 41 | 38 |
|
| 42 | 39 |
/* ROS node created in libscout.cpp */ |
| ... | ... | |
| 52 | 49 |
* Initialize the libscout node as a publisher of set_motors and a client of |
| 53 | 50 |
* query_motors. |
| 54 | 51 |
*/ |
| 55 |
void libmotors_init(){
|
|
| 52 |
void libmotors_init() |
|
| 53 |
{
|
|
| 56 | 54 |
set_motors_publisher = libscout_node.advertise<motors::set_motors>("libmotors", 10);
|
| 57 | 55 |
query_motors_client = libscout_node.serviceClient<motors::query_motors>("libmotors");
|
| 58 | 56 |
} |
| ... | ... | |
| 65 | 63 |
* \param which A bitmask of which motors should be set |
| 66 | 64 |
* \return Function status |
| 67 | 65 |
*/ |
| 68 |
int motors_set(int speed, int which, char units=MOTOR_PERCENT){
|
|
| 66 |
int motors_set(int speed, int which, char units=MOTOR_PERCENT) |
|
| 67 |
{
|
|
| 69 | 68 |
/** \todo Set fields of the message based on params */ |
| 70 | 69 |
|
| 71 |
if(!ros::ok()){
|
|
| 70 |
motors::set_motors msg; |
|
| 71 |
|
|
| 72 |
if(!ros::ok()) |
|
| 73 |
{
|
|
| 72 | 74 |
return LIB_ERROR; |
| 73 | 75 |
} |
| 74 | 76 |
|
| 75 | 77 |
/* Set the speed for each motor according to the which bitmask*/ |
| 76 |
motors::set_motors msg; |
|
| 77 |
if(which & MOTOR_FL_REV){
|
|
| 78 |
|
|
| 79 |
if(which & MOTOR_FL_REV) |
|
| 80 |
{
|
|
| 78 | 81 |
msg.fl_speed = -1 * speed; |
| 79 | 82 |
} |
| 80 |
if(which & MOTOR_FR_REV){
|
|
| 83 |
if(which & MOTOR_FR_REV) |
|
| 84 |
{
|
|
| 81 | 85 |
msg.fr_speed = -1 * speed; |
| 82 | 86 |
} |
| 83 |
if(which & MOTOR_BL_REV){
|
|
| 87 |
if(which & MOTOR_BL_REV) |
|
| 88 |
{
|
|
| 84 | 89 |
msg.bl_speed = -1 * speed; |
| 85 | 90 |
} |
| 86 |
if(which & MOTOR_BR_REV){
|
|
| 91 |
if(which & MOTOR_BR_REV) |
|
| 92 |
{
|
|
| 87 | 93 |
msg.br_speed = -1 * speed; |
| 88 |
}if(which & MOTOR_FL){
|
|
| 94 |
}if(which & MOTOR_FL) |
|
| 95 |
{
|
|
| 89 | 96 |
msg.fl_speed = speed; |
| 90 | 97 |
} |
| 91 |
if(which & MOTOR_FR){
|
|
| 98 |
if(which & MOTOR_FR) |
|
| 99 |
{
|
|
| 92 | 100 |
msg.fr_speed = speed; |
| 93 | 101 |
} |
| 94 |
if(which & MOTOR_BL){
|
|
| 102 |
if(which & MOTOR_BL) |
|
| 103 |
{
|
|
| 95 | 104 |
msg.bl_speed = speed; |
| 96 | 105 |
} |
| 97 |
if(which & MOTOR_BR){
|
|
| 106 |
if(which & MOTOR_BR) |
|
| 107 |
{
|
|
| 98 | 108 |
msg.br_speed = speed; |
| 99 | 109 |
} |
| 100 | 110 |
|
| ... | ... | |
| 119 | 129 |
* \return The speed of the selected motor, or LIB_ERR if no motor selected |
| 120 | 130 |
*/ |
| 121 | 131 |
|
| 122 |
int motors_query(int which){
|
|
| 132 |
int motors_query(int which) |
|
| 133 |
{
|
|
| 123 | 134 |
motors::query_motors srv; |
| 124 |
if(query_motors_client.call(srv)){
|
|
| 125 |
switch(which){
|
|
| 135 |
if(query_motors_client.call(srv)) |
|
| 136 |
{
|
|
| 137 |
switch(which) |
|
| 138 |
{
|
|
| 126 | 139 |
case MOTOR_FL: |
| 127 | 140 |
return srv.response.fl_speed; |
| 128 | 141 |
case MOTOR_FR: |
| ... | ... | |
| 135 | 148 |
ROS_WARN("Bad WHICH in motors_query.");
|
| 136 | 149 |
return LIB_ERROR; |
| 137 | 150 |
} |
| 138 |
}else{
|
|
| 151 |
} |
|
| 152 |
else |
|
| 153 |
{
|
|
| 139 | 154 |
ROS_ERROR("Failed to call service query_motors");
|
| 140 | 155 |
return LIB_ERROR; |
| 141 | 156 |
} |
Also available in: Unified diff