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