Revision 4589a2a9 scout/libscout/src/libmotors.cpp

View differences:

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