Project

General

Profile

Revision ce559b91

IDce559b91dc2723f09fa0c381cd4ebff86d4f8e10

Added by Alex Zirbel over 6 years ago

Fixed the encoders node.

Simplified the node to only advertise a single service - a query allows other nodes to find out, simply, the total distance traveled. The node also only handles plain encoder ticks, because distance and such computation should be done on the library end.

View differences:

scout/encoders/src/encoders.cpp
23 23
 * OTHER DEALINGS IN THE SOFTWARE.
24 24
 */
25 25

  
26

  
27 26
/**
28 27
 * @file encoders.cpp
29 28
 * @brief Encoders
......
31 30
 * Implementation of functions for encoder use.
32 31
 *
33 32
 * @author Colony Project, CMU Robotics Club
33
 * @author Alex Zirbel
34 34
 **/
35 35

  
36
#include "ros/ros.h"
36
#include <ros/ros.h>
37

  
37 38
#include "encoders.h"
38
//#include "libscout/src/constants.h"
39
#include <cstdlib>
40 39

  
41 40
/**
42 41
 * @defgroup encoders Encoders
43 42
 * @brief Functions for using the encoders
44 43
 *
45
 **/
44
 * @{
45
 */
46 46

  
47 47
/* Encoder state variables */
48
/** \todo Fix types: static */
49
/** \todo Fix integer overflow possibility */
48
/** @todo Fix types: static */
49
/** @todo Fix integer overflow possibility */
50

  
50 51
/* The current position of each encoder, measured in encoder ticks */
51
int encoder_fl_index; /**< The current state of the front left encoder. */
52
int encoder_fr_index; /**< The current state of the front right encoder. */
53
int encoder_bl_index; /**< The current state of the back left encoder. */
54
int encoder_br_index; /**< The current state of the back right encoder. */
55
/* The current velocity of each encoder, measured in ticks per second */
56
int encoder_fl_velocity;
57
int encoder_fr_velocity;
58
int encoder_bl_velocity;
59
int encoder_br_velocity;
52
int encoder_fl_index;
53
int encoder_fr_index;
54
int encoder_bl_index;
55
int encoder_br_index;
56

  
60 57
/* The current distance each encoder has traveled in ticks from last reset */
61 58
int encoder_fl_distance;
62 59
int encoder_fr_distance;
63 60
int encoder_bl_distance;
64 61
int encoder_br_distance;
65
/* The units that the encoder measurements will be published in */
66
char encoder_units;
67 62

  
63
/**
64
 * @brief Outputs current encoder data
65
 *
66
 * Serves the service query_encoders by responding to service requests with the
67
 * encoder values.
68
 * @param req The request. The units that the response should be in.
69
 * @param res The response. The fields will be filled with values.
70
 */
71
bool handle_encoders_query(encoders::query_encoders::Request  &req,
72
                           encoders::query_encoders::Response &res)
73
{
74
    /* Put index, velocity, and distance data in message */
75
    res.fl_distance = encoder_fl_distance;
76
    res.fr_distance = encoder_fr_distance;
77
    res.bl_distance = encoder_bl_distance;
78
    res.br_distance = encoder_br_distance;
79

  
80
    ROS_DEBUG("Encoder values queried [Unimplemented]");
81
    return true;
82
}
68 83

  
69
/*!
70
 * \brief Encoders driver. This is a ROS node that controls encoders.
84
/**
85
 * @brief Encoders driver. This is a ROS node that controls encoders.
71 86
 *
72 87
 * This is the main function for the encoders node. It is run when the node
73 88
 * starts and initializes the encoders. It then publishes to the
74 89
 * encoder_state topic and advertises the query_encoders service.
75
 * 
76
 * \param argc The number of command line arguments (should be 1)
77
 * \param argv The array of command line arguments
78
 **/
79
int main(int argc, char **argv){
80
  /* Initialize in ROS the encoders driver node */
81
  ros::init(argc, argv, "encoders_driver");
82
  
83
  /* Advertise that this serves the query_encoders service */
84
  ros::NodeHandle n;
85
  ros::ServiceServer service = n.advertiseService("query_encoders", encoders_query);
90
 */
91
int main(int argc, char **argv)
92
{
93
    /* Initialize in ROS the encoders driver node */
94
    ros::init(argc, argv, "encoders_driver");
86 95

  
87
  /* Subscribe to the encoder_state topic */
88
  ros::Subscriber sub0 = n.subscribe("encoder_state", 4, encoder_state);
96
    /* Advertise that this serves the query_encoders service */
97
    ros::NodeHandle n;
98
    ros::ServiceServer service =
99
        n.advertiseService("query_encoders",
100
                           handle_encoders_query);
89 101

  
90
  /* Initialize hardware for motors */
91
  // Hardware init functions here
102
    /* Initialize hardware for motors */
103
    // Hardware init functions here
92 104

  
93
  ROS_INFO("Ready to set encoders.");
94
  ros::spin();
105
    ROS_INFO("Ready to set encoders.");
95 106

  
96
  return 0;
97
}
107
    // Acutally measure encoders and update values here
98 108

  
99
/*!
100
 * \brief Gets encoder state
101
 *
102
 * Sets the encoder state based on subscription to the encoder_state topic.
103
 *
104
 * \todo Make this function set the state properly. Probably requires 
105
 * creating a new msg type for the mode set.
106
 *
107
 * \param msg The message from the encoder_state topic, containing encoder
108
 * configuration settings.
109
 */
110
void encoder_set_mode(const encoders::encoder_state::ConstPtr& msg)
111
{ 
112
  
113
}
109
    ros::spin();
114 110

  
115
/*!
116
 * \brief Publishes encoder state
117
 *
118
 * Publishes the current encoders position, velocty, and distance from last
119
 * reset.
120
 */
121
void encoders_state_publish()
122
{
123
	encoders::encoders_state msg;
124
	
125
	if(!ros::ok())
126
	{
127
    return LIB_ERROR;
128
  }
129
	
130
	/* Put index, velocity, and distance data in message */
131
	msg.fl_index = encoders_ticks_to_units(encoder_fl_index, encoder_units);
132
	msg.fr_index = encoders_ticks_to_units(encoder_fr_index, encoder_units);
133
	msg.bl_index = encoders_ticks_to_units(encoder_bl_index, encoder_units);
134
	msg.br_index = encoders_ticks_to_units(encoder_br_index, encoder_units);
135
	msg.fl_speed = encoders_ticks_to_units(encoder_fl_velocity, encoder_units);
136
	msg.fr_speed = encoders_ticks_to_units(encoder_fr_velocity, encoder_units);
137
	msg.bl_speed = encoders_ticks_to_units(encoder_bl_velocity, encoder_units);
138
	msg.br_speed = encoders_ticks_to_units(encoder_br_velocity, encoder_units);
139
	msg.fl_distance = encoders_ticks_to_units(encoder_fl_distance, encoder_units);
140
	msg.fr_distance = encoders_ticks_to_units(encoder_fr_distance, encoder_units);
141
	msg.bl_distance = encoders_ticks_to_units(encoder_bl_distance, encoder_units);
142
	msg.br_distance = encoders_ticks_to_units(encoder_br_distance, encoder_units);
143
	/* Specifies which units the encoder values are given in */
144
	msg.units = encoder_units;
145
	
146
	/* Publishes message to encoders_state */
147
	encoders_state_publisher.publish(msg);
148
	ros::spinOnce();
149
	
150
	return LIB_OK;
151
}	
152

  
153
/*!
154
 * \brief Outputs current encoder data
155
 *
156
 * Serves the service query_encoders by responding to service requests with the
157
 * encoder values.
158
 * \param req The request. The units that the response should be in.
159
 * \param res The response. The fields will be filled with values.
160
 */
161
bool encoders_query(encoders::query_encoders::Request &req,
162
    encoders::query_encoders::Response &res)
163
{
164
  int units = req.units;	
165
	
166
  /* Put index, velocity, and distance data in message */
167
	res.fl_index = encoders_ticks_to_units(encoder_fl_index, units);
168
	res.fr_index = encoders_ticks_to_units(encoder_fr_index, units);
169
	res.bl_index = encoders_ticks_to_units(encoder_bl_index, units);
170
	res.br_index = encoders_ticks_to_units(encoder_br_index, units);
171
	res.fl_speed = encoders_ticks_to_units(encoder_fl_velocity, units);
172
	res.fr_speed = encoders_ticks_to_units(encoder_fr_velocity, units);
173
	res.bl_speed = encoders_ticks_to_units(encoder_bl_velocity, units);
174
	res.br_speed = encoders_ticks_to_units(encoder_br_velocity, units);
175
	res.fl_distance = encoders_ticks_to_units(encoder_fl_distance, units);
176
	res.fr_distance = encoders_ticks_to_units(encoder_fr_distance, units);
177
	res.bl_distance = encoders_ticks_to_units(encoder_bl_distance, units);
178
	res.br_distance = encoders_ticks_to_units(encoder_br_distance, units);
179

  
180
  ROS_DEBUG("Encoder values queried");
181
  return true;
111
    return 0;
182 112
}
183 113

  
184
int encoders_ticks_to_units(int ticks, char units)
185
{
186
	switch(encoder_units)
187
	{
188
		case ENCODER_TICKS: /* Just return ticks */
189
			return ticks;
190
		case ENCODER_MM: /* Return mm of linear travel by the wheel */
191
			return ticks * (2 * PI * WHEEL_RADIUS_MM / NUM_TICKS);
192
		case ENCODER_CM: /* Return cm of linear travel by the wheel */
193
			return ticks * (2 * PI * WHEEL_RADIUS_MM / NUM_TICKS) / 10;
194
		case ENCODER_DEG: /* Return degrees of travel from last reset */
195
			return ticks * (360 / NUM_TICKS);
196
		default: /* The units aren't recognized. Return ticks. */
197
			return ticks;
198
	}
199
}
114
/** @} */

Also available in: Unified diff