commit 7cb5de78f535a69b8104606a6eac7994534f3f61
Author: Ben Wasserman <bwasserm@andrew.cmu.edu>
Date:   Wed Nov 2 18:23:58 2011 -0400

    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.

diff --git a/scout/encoders/msg/encoder_state.msg b/scout/encoders/msg/encoder_state.msg
deleted file mode 100644
index 9a2ca5c..0000000
--- a/scout/encoders/msg/encoder_state.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-Header header
-int8 fl_speed
-int8 fr_speed
-int8 bl_speed
-int8 br_speed
-int8 units
diff --git a/scout/encoders/msg/encoders_state.msg b/scout/encoders/msg/encoders_state.msg
new file mode 100644
index 0000000..6f0f2b2
--- /dev/null
+++ b/scout/encoders/msg/encoders_state.msg
@@ -0,0 +1,14 @@
+Header header
+int16 fl_speed
+int16 fr_speed
+int16 bl_speed
+int16 br_speed
+int16 fl_index
+int16 fr_index
+int16 bl_index
+int16 br_index
+int16 fl_distance
+int16 fr_distance
+int16 bl_distance
+int16 br_distance
+int8 units
diff --git a/scout/encoders/src/encoders.cpp b/scout/encoders/src/encoders.cpp
index 8afd4b7..1ab2685 100644
--- a/scout/encoders/src/encoders.cpp
+++ b/scout/encoders/src/encoders.cpp
@@ -31,7 +31,6 @@
  * Implementation of functions for encoder use.
  *
  * @author Colony Project, CMU Robotics Club
- * @author Benjamin Wasserman
  **/
 
 #include "ros/ros.h"
@@ -47,10 +46,25 @@
 
 /* Encoder state variables */
 /** \todo Fix types: static */
-int encoder_fl; /**< The current state of the front left encoder. */
-int encoder_fr; /**< The current state of the front right encoder. */
-int encoder_bl; /**< The current state of the back left encoder. */
-int encoder_br; /**< The current state of the back right encoder. */
+/** \todo Fix integer overflow possibility */
+/* The current position of each encoder, measured in encoder ticks */
+int encoder_fl_index; /**< The current state of the front left encoder. */
+int encoder_fr_index; /**< The current state of the front right encoder. */
+int encoder_bl_index; /**< The current state of the back left encoder. */
+int encoder_br_index; /**< The current state of the back right encoder. */
+/* The current velocity of each encoder, measured in ticks per second */
+int encoder_fl_velocity;
+int encoder_fr_velocity;
+int encoder_bl_velocity;
+int encoder_br_velocity;
+/* The current distance each encoder has traveled in ticks from last reset */
+int encoder_fl_distance;
+int encoder_fr_distance;
+int encoder_bl_distance;
+int encoder_br_distance;
+/* The units that the encoder measurements will be published in */
+char encoder_units;
+
 
 /*!
  * \brief Encoders driver. This is a ROS node that controls encoders.
@@ -87,33 +101,99 @@ int main(int argc, char **argv){
  *
  * Sets the encoder state based on subscription to the encoder_state topic.
  *
+ * \todo Make this function set the state properly. Probably requires 
+ * creating a new msg type for the mode set.
+ *
  * \param msg The message from the encoder_state topic, containing encoder
  * configuration settings.
  */
-void encoder_state(const encoders::encoder_state::ConstPtr& msg){ 
-  encoder_fl = msg->fl_encoder;
-  encoder_fr = msg->fr_encoder;
-  encoder_bl = msg->bl_encoder;
-  encoder_br = msg->br_encoder;
+void encoder_set_mode(const encoders::encoder_state::ConstPtr& msg)
+{ 
+  
 }
 
 /*!
+ * \brief Publishes encoder state
+ *
+ * Publishes the current encoders position, velocty, and distance from last
+ * reset.
+ */
+void encoders_state_publish()
+{
+	encoders::encoders_state msg;
+	
+	if(!ros::ok())
+	{
+    return LIB_ERROR;
+  }
+	
+	/* Put index, velocity, and distance data in message */
+	msg.fl_index = encoders_ticks_to_units(encoder_fl_index, encoder_units);
+	msg.fr_index = encoders_ticks_to_units(encoder_fr_index, encoder_units);
+	msg.bl_index = encoders_ticks_to_units(encoder_bl_index, encoder_units);
+	msg.br_index = encoders_ticks_to_units(encoder_br_index, encoder_units);
+	msg.fl_speed = encoders_ticks_to_units(encoder_fl_velocity, encoder_units);
+	msg.fr_speed = encoders_ticks_to_units(encoder_fr_velocity, encoder_units);
+	msg.bl_speed = encoders_ticks_to_units(encoder_bl_velocity, encoder_units);
+	msg.br_speed = encoders_ticks_to_units(encoder_br_velocity, encoder_units);
+	msg.fl_distance = encoders_ticks_to_units(encoder_fl_distance, encoder_units);
+	msg.fr_distance = encoders_ticks_to_units(encoder_fr_distance, encoder_units);
+	msg.bl_distance = encoders_ticks_to_units(encoder_bl_distance, encoder_units);
+	msg.br_distance = encoders_ticks_to_units(encoder_br_distance, encoder_units);
+	/* Specifies which units the encoder values are given in */
+	msg.units = encoder_units;
+	
+	/* Publishes message to encoders_state */
+	encoders_state_publisher.publish(msg);
+	ros::spinOnce();
+	
+	return LIB_OK;
+}	
+
+/*!
  * \brief Outputs current encoder data
  *
  * Serves the service query_encoders by responding to service requests with the
  * encoder values.
- * \param req The request. Should be empty.
+ * \param req The request. The units that the response should be in.
  * \param res The response. The fields will be filled with values.
  */
 bool encoders_query(encoders::query_encoders::Request &req,
-    encoders::query_encoders::Response &res){
-  
-  res.fl_encoder = encoder_fl;
-  res.fr_encoder = encoder_fr;
-  res.bl_encoder = encoder_bl;
-  res.br_encoder = encoder_br;
+    encoders::query_encoders::Response &res)
+{
+  int units = req.units;	
+	
+  /* Put index, velocity, and distance data in message */
+	res.fl_index = encoders_ticks_to_units(encoder_fl_index, units);
+	res.fr_index = encoders_ticks_to_units(encoder_fr_index, units);
+	res.bl_index = encoders_ticks_to_units(encoder_bl_index, units);
+	res.br_index = encoders_ticks_to_units(encoder_br_index, units);
+	res.fl_speed = encoders_ticks_to_units(encoder_fl_velocity, units);
+	res.fr_speed = encoders_ticks_to_units(encoder_fr_velocity, units);
+	res.bl_speed = encoders_ticks_to_units(encoder_bl_velocity, units);
+	res.br_speed = encoders_ticks_to_units(encoder_br_velocity, units);
+	res.fl_distance = encoders_ticks_to_units(encoder_fl_distance, units);
+	res.fr_distance = encoders_ticks_to_units(encoder_fr_distance, units);
+	res.bl_distance = encoders_ticks_to_units(encoder_bl_distance, units);
+	res.br_distance = encoders_ticks_to_units(encoder_br_distance, units);
 
   ROS_DEBUG("Encoder values queried");
   return true;
 }
 
+int encoders_ticks_to_units(int ticks, char units)
+{
+	switch(encoder_units)
+	{
+		case ENCODER_TICKS: /* Just return ticks */
+			return ticks;
+		case ENCODER_MM: /* Return mm of linear travel by the wheel */
+			return ticks * (2 * PI * WHEEL_RADIUS_MM / NUM_TICKS);
+		case ENCODER_CM: /* Return cm of linear travel by the wheel */
+			return ticks * (2 * PI * WHEEL_RADIUS_MM / NUM_TICKS) / 10;
+		case ENCODER_DEG: /* Return degrees of travel from last reset */
+			return ticks * (360 / NUM_TICKS);
+		default: /* The units aren't recognized. Return ticks. */
+			return ticks;
+	}
+}
diff --git a/scout/encoders/src/encoders.h b/scout/encoders/src/encoders.h
index 82515cc..628943c 100644
--- a/scout/encoders/src/encoders.h
+++ b/scout/encoders/src/encoders.h
@@ -1,5 +1,5 @@
 /**
- *Copyright (c) 2007 Colony Project
+ *Copyright (c) 2011 Colony Project
  * 
  * Permission is hereby granted, free of charge, to any person
  * obtaining a copy of this software and associated documentation
@@ -34,22 +34,31 @@
  * @author Colony Project, CMU Robotics Club
  **/
 
-/* Authors: Ben Wasserman, Lalitha Ganesan
-*/
-
 #ifndef _ENCODERS_H_
 #define _ENCODERS_H_
 
 #include "encoders/query_encoders.h"
-#include "encoders/encoder_state.h"
+#include "encoders/encoders_state.h"
+
+#define ENCODERS_TICKS 't'
+#define ENCODERS_MM 'm'
+#define ENCODERS_CM 'c'
+#define ENCODERS_DEG 'd'
 
+#define NUM_TICKS 100
+#define WHEEL_RADIUS_MM 20
+#define PI 3.14159265
 
 /** @brief Initialize the encoders module and driver **/
 int main(int argc, char **argv);
 /** @brief Responds to topic to get encoder state **/
-void encoder_state(const encoders::encoder_state::ConstPtr& msg);
+void encoder_set_mode(const encoders::encoders_state::ConstPtr& msg);
+/** @brief Publishes encoder state **/
+void encoders_state_publish();
 /** @brief Responds to service to query encoder data **/
 bool encoders_query(encoders::query_encoders::Request &req,
     encoders::query_encoders::Response &res);
+/** @brief Converts encoder values to selected units **/
+int encoders_ticks_to_units(int ticks, char units);
 
 #endif
diff --git a/scout/encoders/srv/query_encoders.srv b/scout/encoders/srv/query_encoders.srv
index 6db1530..919ba19 100644
--- a/scout/encoders/srv/query_encoders.srv
+++ b/scout/encoders/srv/query_encoders.srv
@@ -1,5 +1,14 @@
+int8 units
 ---
 int8 fl_speed
 int8 fr_speed
 int8 bl_speed
 int8 br_speed
+int8 fl_index
+int8 fr_index
+int8 bl_index
+int8 br_index
+int8 fl_distance
+int8 fr_distance
+int8 bl_distance
+int8 br_distance
