Project

General

Profile

Revision 8de28e68

ID8de28e688f1c3ce392a9bd55ea7c4c6a6cd8592d

Added by Priya over 12 years ago

Created structure for buttons node. Compiles. However event_button function is not written.

View differences:

scout/buttons/CMakeLists.txt
29 29
#rosbuild_add_executable(example examples/example.cpp)
30 30
#target_link_libraries(example ${PROJECT_NAME})
31 31

  
32
rosbuild_add_executable(motors_node src/motors.cpp)
32
rosbuild_add_executable(buttons_node src/buttons.cpp)
scout/buttons/src/buttons.cpp
50 50
static int button2_pressed; /**< Whether or not button 2 is pressed. */
51 51

  
52 52
/**
53
 * @brief Sets motor speed
53
 * @brief Handles button events
54 54
 *
55
 * Sets the motor speeds based on subscription to the set_motors topic.
55
 * Handles button events based on subscription to the button_event topic.
56 56
 *
57
 * @param msg The message from the set_motors topic, containing speeds and
58
 *  motor configuration settings.
57
 * @param msg The message from the button_event topic, containing buttons
58
 * statuses.
59 59
 */
60
void motors_set(const motors::set_motors::ConstPtr& msg)
60
void event_button(const buttons::button_event::ConstPtr& msg)
61 61
{
62
    /** @todo Edit to only set requested motors, not all */
63
    int which = msg->which;
64
    if(which & MOTOR_FL_REV)
65
    {
66
      motor_fl_speed = -1 * motors_rel_to_abs(msg->fl_speed, msg->units);
67
    }
68
    if(which & MOTOR_FR_REV)
69
    {
70
      motor_fr_speed = -1 * motors_rel_to_abs(msg->fr_speed, msg->units);
71
    }
72
    if(which & MOTOR_BL_REV)
73
    {
74
      motor_bl_speed = -1 * motors_rel_to_abs(msg->bl_speed, msg->units);
75
    }
76
    if(which & MOTOR_BR_REV)
77
    {
78
      motor_br_speed = -1 * motors_rel_to_abs(msg->br_speed, msg->units);
79
    }
80
    if(which & MOTOR_FL)
81
    {
82
      motor_fl_speed = motors_rel_to_abs(msg->fl_speed, msg->units);
83
    }
84
    if(which & MOTOR_FR)
85
    {
86
      motor_fr_speed = motors_rel_to_abs(msg->fr_speed, msg->units);
87
    }
88
    if(which & MOTOR_BL)
89
    {
90
      motor_bl_speed = motors_rel_to_abs(msg->bl_speed, msg->units);
91
    }
92
    if(which & MOTOR_BR)
93
    {
94
      motor_br_speed = motors_rel_to_abs(msg->br_speed, msg->units);
95
    }
96

  
97
    /* Write speeds to hardware */
98
    /** @todo Add code to write speeds to hardware */
62
    //TODO: What goes here?
99 63
}
100 64

  
65

  
101 66
/**
102
 * @brief Outputs current motor speeds
67
 * @brief Outputs whether or not the buttons are pressed.
103 68
 *
104
 * Serves the service query_motors by responding to service requests with the
105
 * speeds of the motors.
69
 * Serves the service query_buttons by responding to service requests 
70
 * with the status of the buttons
106 71
 * @param req The request. The only field is the units requested.
107 72
 * @param res The response. The fields will be filled with values.
108 73
 */
109
bool motors_query(motors::query_motors::Request &req,
110
                  motors::query_motors::Response &res)
74
bool buttons_query(buttons::query_buttons::Request &req,
75
                  buttons::query_buttons::Response &res)
111 76
{
112 77
    int units = req.units;
113
    res.fl_speed = motors_abs_to_rel(motor_fl_speed, units);
114
    res.fr_speed = motors_abs_to_rel(motor_fr_speed, units);
115
    res.bl_speed = motors_abs_to_rel(motor_bl_speed, units);
116
    res.br_speed = motors_abs_to_rel(motor_br_speed, units);
78
    res.button1_pressed = button1_pressed;
79
    res.button2_pressed = button2_pressed;
117 80

  
118
    ROS_DEBUG("Motor speeds queried");
81
    ROS_DEBUG("Button status queried");
119 82
    return true;
120 83
}
121 84

  
122
/**
123
 * @brief Converts set speeds (of various units) to absolute speeds.
124
 *
125
 * @param speed The speed expressed in the desired units
126
 * @param units The units the desired speed is measured in
127
 * @return The absolute speed of the motor
128
 **/
129
int motors_rel_to_abs(int rel_speed, int units)
130
{
131
    switch(units)
132
    {
133
      case MOTOR_ABSOLUTE:/* Speed given as absolute */
134
        return rel_speed;
135
      case MOTOR_PERCENT:/* Convert from percentage */
136
        return rel_speed * MAXSPEED / 100;
137
      case MOTOR_MMS:/* Convert from mm/s */
138
        /** @todo Make math to do this conversion **/
139
        return rel_speed;
140
      case MOTOR_CMS:/* Convert from cm/s */
141
        /** @todo Make math to do this conversion **/
142
        return rel_speed;
143
      default: /* The units aren't recognized */
144
        /** @todo Decide on default case. Either percent or absolute. **/
145
        return rel_speed;
146
    }
147
}
148

  
149
/**
150
 * @brief Convert absolute speeds to speeds of various units.
151
 *
152
 * @param speed The speed expressed in absolute units.
153
 * @param units The units the desired speed is measured in.
154
 * @return The relative speed of the motor in desired units.
155
 **/
156
int motors_abs_to_rel(int abs_speed, int units)
157
{
158
    switch(units)
159
    {
160
      case MOTOR_ABSOLUTE:/* Speed given as absolute */
161
        return abs_speed;
162
      case MOTOR_PERCENT:/* Convert from percentage */
163
        return abs_speed * 100 / MAXSPEED;
164
      case MOTOR_MMS:/* Convert from mm/s */
165
        /** @todo Make math to do this conversion **/
166
        return abs_speed;
167
      case MOTOR_CMS:/* Convert from cm/s */
168
        /** @todo Make math to do this conversion **/
169
        return abs_speed;
170
      default: /* The units aren't recognized */
171
        /** @todo Decide on default case. Either percent or absolute. **/
172
        return abs_speed;
173
    }
174
}
175 85

  
176 86
/**
177
 * @brief Motors driver. This is a ROS node that controls motor speeds.
87
 * @brief Buttons driver. This is a ROS node that queries button status.
178 88
 *
179
 * This is the main function for the motors node. It is run when the node
180
 * starts and initializes the motors. It then subscribes to the
181
 * set_motors, and set_motor_speeds topics, and advertises the
182
 * query_motors service.
89
 * This is the main function for the buttons node. It is run
90
 * when the node starts and initializes the buttons. It then 
91
 * subscribes to the event_button topics, and advertises the 
92
 * query_buttons service.
183 93
 * 
184 94
 * @param argc The number of command line arguments (should be 1)
185 95
 * @param argv The array of command line arguments
186 96
 **/
187 97
int main(int argc, char **argv)
188 98
{
189
    /* Initialize in ROS the motors driver node */
190
    ros::init(argc, argv, "motors_driver");
99
    /* Initialize in ROS the buttons driver node */
100
    ros::init(argc, argv, "buttons_driver");
191 101

  
192
    /* Advertise that this serves the query_motors service */
102
    /* Advertise that this serves the query_buttons service */
193 103
    ros::NodeHandle n;
194
    ros::ServiceServer service = n.advertiseService("query_motors",
195
                                                    motors_query);
104
    ros::ServiceServer service = n.advertiseService("query_buttons",
105
                                                    buttons_query);
196 106

  
197
    /* Subscribe to the set_motors topic */
198
    ros::Subscriber sub0 = n.subscribe("set_motors", 4, motors_set);
107
    /* Subscribe to the button_event topic */
108
    ros::Subscriber sub0 = n.subscribe("button_event", 4, event_button);
199 109

  
200
    /* Initialize hardware for motors */
110
    /* Initialize hardware for buttons */
201 111
    // Hardware init functions here
202 112

  
203
    ROS_INFO("Ready to set motors.");
113
    ROS_INFO("Ready to query buttons.");
204 114
    ros::spin();
205 115

  
206 116
    return 0;

Also available in: Unified diff