Project

General

Profile

Revision 7cb5de78

ID7cb5de78f535a69b8104606a6eac7994534f3f61

Added by Ben Wasserman over 12 years ago

Updated encoder node code with preliminary driver functionality.
Renamed encoder_state.msg to encoders_state.msg.
Added functions to encoders.cpp to send message of current encoder state.
Added function headers to encoders.h.
Added function to encoders.cpp to convert to different units.
Added additional global variables to store encoder state.
Updated encoder_state.msg and query_encoders.srv with additional fields for units.
Node is not completed. Still requires lowest level driver functionality. Needs constants for encoders and wheels. Needs initialization in main for encoders_state publisher. Needs implementation of encoders_set_mode, which should change what units the encoders node publishes the encoders_state message, and with which frequency.
I know this is bad practice, but I have not tried compiling my changes. I broke my computer's ability to compile ROS code, so this is untested. However, it should work. SORRY.

View differences:

scout/encoders/msg/encoder_state.msg
1
Header header
2
int8 fl_speed
3
int8 fr_speed
4
int8 bl_speed
5
int8 br_speed
6
int8 units
scout/encoders/msg/encoders_state.msg
1
Header header
2
int16 fl_speed
3
int16 fr_speed
4
int16 bl_speed
5
int16 br_speed
6
int16 fl_index
7
int16 fr_index
8
int16 bl_index
9
int16 br_index
10
int16 fl_distance
11
int16 fr_distance
12
int16 bl_distance
13
int16 br_distance
14
int8 units
scout/encoders/src/encoders.cpp
31 31
 * Implementation of functions for encoder use.
32 32
 *
33 33
 * @author Colony Project, CMU Robotics Club
34
 * @author Benjamin Wasserman
35 34
 **/
36 35

  
37 36
#include "ros/ros.h"
......
47 46

  
48 47
/* Encoder state variables */
49 48
/** \todo Fix types: static */
50
int encoder_fl; /**< The current state of the front left encoder. */
51
int encoder_fr; /**< The current state of the front right encoder. */
52
int encoder_bl; /**< The current state of the back left encoder. */
53
int encoder_br; /**< The current state of the back right encoder. */
49
/** \todo Fix integer overflow possibility */
50
/* 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;
60
/* The current distance each encoder has traveled in ticks from last reset */
61
int encoder_fl_distance;
62
int encoder_fr_distance;
63
int encoder_bl_distance;
64
int encoder_br_distance;
65
/* The units that the encoder measurements will be published in */
66
char encoder_units;
67

  
54 68

  
55 69
/*!
56 70
 * \brief Encoders driver. This is a ROS node that controls encoders.
......
87 101
 *
88 102
 * Sets the encoder state based on subscription to the encoder_state topic.
89 103
 *
104
 * \todo Make this function set the state properly. Probably requires 
105
 * creating a new msg type for the mode set.
106
 *
90 107
 * \param msg The message from the encoder_state topic, containing encoder
91 108
 * configuration settings.
92 109
 */
93
void encoder_state(const encoders::encoder_state::ConstPtr& msg){ 
94
  encoder_fl = msg->fl_encoder;
95
  encoder_fr = msg->fr_encoder;
96
  encoder_bl = msg->bl_encoder;
97
  encoder_br = msg->br_encoder;
110
void encoder_set_mode(const encoders::encoder_state::ConstPtr& msg)
111
{ 
112
  
98 113
}
99 114

  
100 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
/*!
101 154
 * \brief Outputs current encoder data
102 155
 *
103 156
 * Serves the service query_encoders by responding to service requests with the
104 157
 * encoder values.
105
 * \param req The request. Should be empty.
158
 * \param req The request. The units that the response should be in.
106 159
 * \param res The response. The fields will be filled with values.
107 160
 */
108 161
bool encoders_query(encoders::query_encoders::Request &req,
109
    encoders::query_encoders::Response &res){
110
  
111
  res.fl_encoder = encoder_fl;
112
  res.fr_encoder = encoder_fr;
113
  res.bl_encoder = encoder_bl;
114
  res.br_encoder = encoder_br;
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);
115 179

  
116 180
  ROS_DEBUG("Encoder values queried");
117 181
  return true;
118 182
}
119 183

  
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
}
scout/encoders/src/encoders.h
1 1
/**
2
 *Copyright (c) 2007 Colony Project
2
 *Copyright (c) 2011 Colony Project
3 3
 * 
4 4
 * Permission is hereby granted, free of charge, to any person
5 5
 * obtaining a copy of this software and associated documentation
......
34 34
 * @author Colony Project, CMU Robotics Club
35 35
 **/
36 36

  
37
/* Authors: Ben Wasserman, Lalitha Ganesan
38
*/
39

  
40 37
#ifndef _ENCODERS_H_
41 38
#define _ENCODERS_H_
42 39

  
43 40
#include "encoders/query_encoders.h"
44
#include "encoders/encoder_state.h"
41
#include "encoders/encoders_state.h"
42

  
43
#define ENCODERS_TICKS 't'
44
#define ENCODERS_MM 'm'
45
#define ENCODERS_CM 'c'
46
#define ENCODERS_DEG 'd'
45 47

  
48
#define NUM_TICKS 100
49
#define WHEEL_RADIUS_MM 20
50
#define PI 3.14159265
46 51

  
47 52
/** @brief Initialize the encoders module and driver **/
48 53
int main(int argc, char **argv);
49 54
/** @brief Responds to topic to get encoder state **/
50
void encoder_state(const encoders::encoder_state::ConstPtr& msg);
55
void encoder_set_mode(const encoders::encoders_state::ConstPtr& msg);
56
/** @brief Publishes encoder state **/
57
void encoders_state_publish();
51 58
/** @brief Responds to service to query encoder data **/
52 59
bool encoders_query(encoders::query_encoders::Request &req,
53 60
    encoders::query_encoders::Response &res);
61
/** @brief Converts encoder values to selected units **/
62
int encoders_ticks_to_units(int ticks, char units);
54 63

  
55 64
#endif
scout/encoders/srv/query_encoders.srv
1
int8 units
1 2
---
2 3
int8 fl_speed
3 4
int8 fr_speed
4 5
int8 bl_speed
5 6
int8 br_speed
7
int8 fl_index
8
int8 fr_index
9
int8 bl_index
10
int8 br_index
11
int8 fl_distance
12
int8 fr_distance
13
int8 bl_distance
14
int8 br_distance

Also available in: Unified diff