commit a84808670408804bf5ce08741a3b59130351928f
Author: Alex Zirbel <azirbel@andrew.cmu.edu>
Date:   Sun Dec 4 01:17:08 2011 -0500

    Made a lot of changes to the general structure.
    
    Applied object-orienting techniques to the code, cleaning it up considerably. Major design changes are as follows:
    
    * All separate node code (ie motors) are meant to only interface with hardware - doing as little logic as possible.
    * Instead, the logic is moved to a class (ie MotorControl) in libscout. PID and other tools should be located here. This significantly reduces dependencies.
    * Files can be included cross-package by being placed in /include/packagename, but this should be avoided to reduce dependencies.
    
    A few formatting changes:
    
    * Comments should end with */ always, not **/
    * Use @ instead of \
    * No double newlines.
    
    The motors node is revamped and pretty much correct at this point.

diff --git a/scout/libscout/CMakeLists.txt b/scout/libscout/CMakeLists.txt
index 822d6c3..251693e 100644
--- a/scout/libscout/CMakeLists.txt
+++ b/scout/libscout/CMakeLists.txt
@@ -29,4 +29,5 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 #rosbuild_add_executable(example examples/example.cpp)
 #target_link_libraries(example ${PROJECT_NAME})
 
-rosbuild_add_executable(libscout_node src/behavior.cpp src/libscout.cpp src/libmotors.cpp src/libheadlights.cpp src/libbuttons.cpp)
+#rosbuild_add_executable(libscout_node src/behavior.cpp src/libscout.cpp src/libmotors.cpp src/libheadlights.cpp src/libbuttons.cpp)
+rosbuild_add_executable(alex_behavior src/alex_behavior.cpp src/libscout.h src/MotorControl.cpp src/HeadlightControl.cpp)
diff --git a/scout/libscout/include/libscout/constants.h b/scout/libscout/include/libscout/constants.h
new file mode 100644
index 0000000..779dcda
--- /dev/null
+++ b/scout/libscout/include/libscout/constants.h
@@ -0,0 +1,61 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ **/
+
+
+/**
+ * @file constants.h
+ * @brief Contains top level scout library constants
+ * 
+ * Contains constants for use in libscout
+ *
+ * @author Colony Project, CMU Robotics Club
+ * @author Ben Wasserman
+ **/
+
+#ifndef _CONSTANTS_H_
+#define _CONSTANTS_H_
+
+#include "ros/ros.h"
+
+/* ROS defines */
+#define QUEUE_SIZE 10
+
+/* Libscout Defines */
+
+/* Init defines */
+#define LIB_ALL -1
+#define LIB_MOTORS 0x1
+#define LIB_SONAR 0x2
+#define LIB_CLIFFSENSORS 0x4
+#define LIB_HEADLIGHTS 0x8
+#define LIB_BUTTONS 0x10
+
+/* Status defines */
+//TODO MAKE ENUMS
+#define LIB_ERROR -1
+#define LIB_OK 0
+
+
+#endif
diff --git a/scout/libscout/src/HeadlightControl.cpp b/scout/libscout/src/HeadlightControl.cpp
new file mode 100644
index 0000000..293d747
--- /dev/null
+++ b/scout/libscout/src/HeadlightControl.cpp
@@ -0,0 +1,115 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+/**
+ * @file HeadlightControl.cpp
+ * @brief Contains headlights declarations and functions
+ * 
+ * Contains functions and definitions for the use of
+ * headlights
+ *
+ * @author Colony Project, CMU Robotics Club
+ * @author Ben Wasserman
+ * @author Alex Zirbel
+ */
+
+#include "HeadlightControl.h"
+
+/** ROS publisher and client declaration */
+
+/**
+ * @brief Initialize the headlights module of libscout.
+ *
+ * Initialize the libscout node as a publisher of set_headlights.
+ */
+HeadlightControl::HeadlightControl(const ros::NodeHandle& libscout_node)
+    : node(libscout_node)
+{
+    set_headlights_pub = node.advertise<headlights::set_headlights>
+        ("set_headlights", QUEUE_SIZE);
+}
+
+/**
+ * @brief Set headlight colors as a single RGB value
+ *
+ * Sets the colors of the headlights as a web color value. Can selectively
+ * select which motors to set, and which to remain at previous color.
+ * @param color The color the headlights should be set to expressed as 0xRRGGBB
+ * @param which A bitmask of which headlights should be set
+ */
+void HeadlightControl::set(int color, int which)
+{
+    int red, green, blue;
+
+    red = (color & RED) >> REDSHIFT;
+    green = (color & GREEN) >> GREENSHIFT;
+    blue = (color & BLUE) >> BLUESHIFT;
+
+    set_rgb(red, green, blue, which);
+}
+
+/**
+ * @brief Set headlight colors as separate RGB values
+ *
+ * Sets the colors of the headlights as a web color value. Can selectively
+ * select which motors to set, and which to remain at previous color.
+ * @param red The red value the headlights should be set to
+ * @param green The green value the headlights should be set to
+ * @param blue The blue value the headlights should be set to
+ * @param which A bitmask of which headlights should be set
+ */
+void HeadlightControl::set_rgb(int red, int green, int blue, int which)
+{
+    headlights::set_headlights msg;
+
+    if(which & HL_LEFT)
+    {
+        msg.left_red = red;
+        msg.left_green = green;
+        msg.left_blue = blue;
+    }
+    else
+    {
+        msg.left_red = NO_SET;
+        msg.left_green = NO_SET;
+        msg.left_blue = NO_SET;
+    }
+    if(which & HL_RIGHT)
+    {
+        msg.right_red = red;
+        msg.right_green = green;
+        msg.right_blue = blue;
+    }
+    else
+    {
+        msg.right_red = NO_SET;
+        msg.right_green = NO_SET;
+        msg.right_blue = NO_SET;
+    }
+
+    /* Publishes message to set_motors topic */
+    set_headlights_pub.publish(msg);
+    ros::spinOnce();
+}
diff --git a/scout/libscout/src/HeadlightControl.h b/scout/libscout/src/HeadlightControl.h
new file mode 100644
index 0000000..b2dbdbd
--- /dev/null
+++ b/scout/libscout/src/HeadlightControl.h
@@ -0,0 +1,80 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+/**
+ * @file HeadlightControl.h
+ * @brief Contains headlights declarations and functions
+ * 
+ * Contains functions and definitions for the use of
+ * headlights
+ *
+ * @author Colony Project, CMU Robotics Club
+ * @author Ben Wasserman
+ * @author Alex Zirbel
+ */
+
+#ifndef _HEADLIGHT_CONTROL_H_
+#define _HEADLIGHT_CONTROL_H_
+
+#include <ros/ros.h>
+#include <libscout/constants.h>
+#include <headlights/set_headlights.h>
+
+/* Defines */
+#define WHITE		0xFFFFFF
+#define OFF			0x000000
+#define RED 		0xFF0000
+#define GREEN 	    0x00FF00
+#define BLUE		0x0000FF
+#define YELLOW	    0xFFFF00
+#define CYAN		0x00FFFF
+#define PINK		0xFF00FF
+
+#define HL_RIGHT	0x1
+#define HL_LEFT		0x2
+#define HL_BOTH		0x3
+
+#define NO_SET	-1
+
+#define REDSHIFT 		16
+#define GREENSHIFT 	    8
+#define BLUESHIFT		0
+
+class HeadlightControl
+{
+    public:
+        HeadlightControl(const ros::NodeHandle& libscout_node);
+
+        void set(int color, int which);
+        void set_rgb(int red, int green, int blue, int which);
+
+    private:
+        ros::NodeHandle node;
+
+        ros::Publisher set_headlights_pub;
+};
+
+#endif
+
diff --git a/scout/libscout/src/MotorControl.cpp b/scout/libscout/src/MotorControl.cpp
new file mode 100644
index 0000000..3f29277
--- /dev/null
+++ b/scout/libscout/src/MotorControl.cpp
@@ -0,0 +1,268 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ **/
+
+/**
+ * @file MotorControl.cpp
+ * @brief Contains motor declarations and functions
+ * 
+ * Contains functions and definitions for the use of
+ * motors
+ *
+ * @author Colony Project, CMU Robotics Club
+ * @author Ben Wasserman
+ * @author Alex Zirbel
+ **/
+
+#include "MotorControl.h"
+
+/**
+ * @brief Initialize the motors module of libscout.
+ *
+ * Initialize the libscout node as a publisher of set_motors and a client of
+ * query_motors.
+ */
+MotorControl::MotorControl(const ros::NodeHandle& libscout_node)
+    : node(libscout_node)
+{
+    set_motors_pub =
+        node.advertise<motors::set_motors>("set_motors", QUEUE_SIZE);
+    query_motors_client =
+        node.serviceClient<motors::query_motors>("query_motors");
+}
+
+/**
+ * @brief Sets all the speeds according to the specificiation of which motors
+ *        to set.
+ *
+ * @param which A bitmask of which motors need to be set.
+ * @param speed The value to set the motors to.
+ * @param units The units the speed is expressed in.
+ * @return Function status
+ */
+void MotorControl::set(int which, float speed, char units)
+{
+    float speed_fl, speed_fr, speed_bl, speed_br;
+    speed_fl = speed_fr = speed_bl = speed_br = speed;
+
+    if(which & MOTOR_FL_REV)
+    {
+        speed_fl = -speed;
+    }
+    if(which & MOTOR_FR_REV)
+    {
+        speed_fr = -speed;
+    }
+    if(which & MOTOR_BL_REV)
+    {
+        speed_bl = -speed;
+    }
+    if(which & MOTOR_BR_REV)
+    {
+        speed_br = -speed;
+    }
+
+    set_each(which, speed_fl, speed_fr, speed_bl, speed_br, units);
+}
+
+/**
+ * @brief Sets the left and right sides of scout to different speeds.
+ *
+ * @param speed_l The speed of both left motors.
+ * @param speed_r The speed of both right motors.
+ * @param units The units to set to.
+ * @return Function status
+ */
+void MotorControl::set_sides(float speed_l, float speed_r, char units)
+{
+    set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r, units);
+}
+
+/**
+ * @brief Set motor speeds
+ * Sets the speeds of the motors as a percentage of top speed. Can selectively
+ * select which motors to set, and which to keep at previous speed.
+ *
+ * @param which A bitmask of which motors should be set.
+ * @param speed The speed the motors should be set to.
+ * @param units Optional units for the speeds.
+ * @return Function status
+ */
+void MotorControl::set_each(int which,
+                            float speed_fl, float speed_fr,
+                            float speed_bl, float speed_br,
+                            char units)
+{
+    check_which_ok(which);
+
+    motors::set_motors msg;
+
+
+    /* Tell the motors node which motors need to be updated */
+    msg.fl_set = msg.fr_set = msg.bl_set = msg.br_set = false;
+    if(which & (MOTOR_FL | MOTOR_FL_REV))
+    {
+        msg.fl_set = true;
+        motor_fl_speed = rel_to_abs(speed_fl, units);
+    }
+    if(which & (MOTOR_FR | MOTOR_FR_REV))
+    {
+        msg.fr_set = true;
+        motor_fr_speed = rel_to_abs(speed_fr, units);
+    }
+    if(which & (MOTOR_BL | MOTOR_BL_REV))
+    {
+        msg.bl_set = true;
+        motor_bl_speed = rel_to_abs(speed_bl, units);
+    }
+    if(which & (MOTOR_BR | MOTOR_BR_REV))
+    {
+        msg.br_set = true;
+        motor_br_speed = rel_to_abs(speed_br, units);
+    }
+
+    /* Set all the speeds (the booleans in the set_motors message determine
+     * which ones will be used) */
+    msg.fl_speed = (int) motor_fl_speed;
+    msg.fr_speed = (int) motor_fr_speed;
+    msg.bl_speed = (int) motor_bl_speed;
+    msg.br_speed = (int) motor_br_speed;
+
+    /* Publishes message to set_motors topic */
+    set_motors_pub.publish(msg);
+    ros::spinOnce();
+}
+
+/**
+ * Double-checks the which variable to make sure the front and back
+ * bits are not both set for a single motor.
+ *
+ * @param which The which motor specfication to check.
+ */
+void MotorControl::check_which_ok(int which)
+{
+    ROS_ERROR_COND( ((which & MOTOR_FL) && (which & MOTOR_FL_REV)),
+        "FL Motor set in both directions!");
+    ROS_ERROR_COND( ((which & MOTOR_FR) && (which & MOTOR_FR_REV)),
+        "FR Motor set in both directions!");
+    ROS_ERROR_COND( ((which & MOTOR_BL) && (which & MOTOR_BL_REV)),
+        "BL Motor set in both directions!");
+    ROS_ERROR_COND( ((which & MOTOR_BR) && (which & MOTOR_BR_REV)),
+        "BR Motor set in both directions!");
+}
+
+/**
+ * @brief Query the current speeds of the motors
+ *
+ * Sends a request to the query_motors service which will reply with the
+ *  current speed of each motor.
+ *
+ * @TODO Change so we can get multiple motor speeds with one call
+ *
+ * @param which A bitmask that will specify which motor speed should be
+ *  returned
+ * @return The speed of the selected motor, or LIB_ERR if no motor selected
+ */
+float MotorControl::query(int which)
+{
+    motors::query_motors srv;
+    if(query_motors_client.call(srv))
+    {
+        switch(which)
+        {
+            case MOTOR_FL:
+                return srv.response.fl_speed;
+            case MOTOR_FR:
+                return srv.response.fr_speed;
+            case MOTOR_BL:
+                return srv.response.bl_speed;
+            case MOTOR_BR:
+                return srv.response.br_speed;
+            default:
+                ROS_WARN("Bad WHICH in motors_query.");
+                return LIB_ERROR;
+        }
+    }
+    else
+    {
+        ROS_ERROR("Failed to call service query_motors");
+        return LIB_ERROR;
+    }
+
+    return 0;
+}
+
+/**
+ * @brief Converts set speeds (of various units) to absolute speeds.
+ *
+ * @param speed The speed expressed in the desired units
+ * @param units The units the desired speed is measured in
+ * @return The absolute speed of the motor
+ **/
+float MotorControl::rel_to_abs(float rel_speed, int units)
+{
+    switch(units)
+    {
+      case MOTOR_ABSOLUTE:/* Speed given as absolute */
+        return rel_speed;
+      case MOTOR_PERCENT:/* Convert from percentage */
+        return rel_speed * MAXSPEED / 100;
+      case MOTOR_MMS:/* Convert from mm/s */
+        /** @todo Make math to do this conversion **/
+        return rel_speed;
+      case MOTOR_CMS:/* Convert from cm/s */
+        /** @todo Make math to do this conversion **/
+        return rel_speed;
+      default: /* The units aren't recognized */
+        /** @todo Decide on default case. Either percent or absolute. **/
+        return rel_speed;
+    }
+}
+
+/**
+ * @brief Convert absolute speeds to speeds of various units.
+ *
+ * @param speed The speed expressed in absolute units.
+ * @param units The units the desired speed is measured in.
+ * @return The relative speed of the motor in desired units.
+ **/
+float MotorControl::abs_to_rel(float abs_speed, int units)
+{
+    switch(units)
+    {
+      case MOTOR_ABSOLUTE:/* Speed given as absolute */
+        return abs_speed;
+      case MOTOR_PERCENT:/* Convert from percentage */
+        return abs_speed * 100 / MAXSPEED;
+      case MOTOR_MMS:/* Convert from mm/s */
+        /** @todo Make math to do this conversion **/
+        return abs_speed;
+      case MOTOR_CMS:/* Convert from cm/s */
+        /** @todo Make math to do this conversion **/
+        return abs_speed;
+      default: /* The units aren't recognized */
+        /** @todo Decide on default case. Either percent or absolute. **/
+        return abs_speed;
+    }
+}
diff --git a/scout/libscout/src/MotorControl.h b/scout/libscout/src/MotorControl.h
new file mode 100644
index 0000000..c12da74
--- /dev/null
+++ b/scout/libscout/src/MotorControl.h
@@ -0,0 +1,119 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ **/
+
+/**
+ * @file MotorControl.h
+ * @brief Contains motor declarations and functions
+ * 
+ * Contains functions and definitions for the use of
+ * motors
+ *
+ * @author Colony Project, CMU Robotics Club
+ * @author Ben Wasserman
+ * @author Alex Zirbel
+ **/
+
+#ifndef _MOTOR_CONTROL_H_
+#define _MOTOR_CONTROL_H_
+
+#include <ros/ros.h>
+/// @TODO maybe not so good! Then packages have to depend on libscout.
+#include <libscout/constants.h>
+#include <motors/query_motors.h>
+#include <motors/set_motors.h>
+
+/* Motor-specific defines */
+#define MOTOR_ALL 0xF
+#define MOTOR_ALL_REV 0xF0
+#define MOTOR_NONE 0x0
+#define MOTOR_FL 0x8
+#define MOTOR_FR 0x4
+#define MOTOR_BL 0x2
+#define MOTOR_BR 0x1
+#define MOTOR_FL_REV 0x80
+#define MOTOR_FR_REV 0x40
+#define MOTOR_BL_REV 0x20
+#define MOTOR_BR_REV 0x10
+#define MOTOR_FRONT MOTOR_FL | MOTOR_FR
+#define MOTOR_BACK MOTOR_BR | MOTOR_BR
+#define MOTOR_LEFT MOTOR_FL | MOTOR_BL
+#define MOTOR_RIGHT MOTOR_FR | MOTOR_BR
+#define MOTOR_LEFT_REV MOTOR_FL_REV | MOTOR_BL_REV
+#define MOTOR_RIGHT_REV MOTOR_FR_REV | MOTOR_BR_REV
+#define MOTOR_LEFT_SPIN MOTOR_LEFT_REV | MOTOR_RIGHT
+#define MOTOR_RIGHT_SPIN MOTOR_LEFT | MOTOR_RIGHT_REV
+
+#define MAXSPEED 255
+#define MOTOR_PERCENT 'p'
+#define MOTOR_MMS 'm'
+#define MOTOR_CMS 'c'
+#define MOTOR_ABSOLUTE 'a'
+#define MOTOR_DEFAULT_UNIT MOTOR_PERCENT
+
+class MotorControl
+{
+    public:
+        /** Set up the motor node and prepare to communicate over ROS */
+        MotorControl(const ros::NodeHandle& libscout_node);
+
+        /** Uses which to specify what to change, and sets all to same speed */
+        void set(int which, float speed, char units=MOTOR_DEFAULT_UNIT);
+
+        /** Sets each side to a different speed */
+        void set_sides(float speed_l, float speed_r,
+                     char units=MOTOR_DEFAULT_UNIT);
+
+        /** Sets each motor speed individually */
+        void set_each(int which,
+                    float speed_fl, float speed_fr,
+                    float speed_bl, float speed_br,
+                    char units=MOTOR_DEFAULT_UNIT);
+        
+        /** Requests the current motor speeds */
+        float query(int which);
+    
+    private:
+        /** Error if which sets a motor to both forward and backward */
+        void check_which_ok(int which);
+
+        float rel_to_abs(float rel_speed, int units);
+        float abs_to_rel(float abs_speed, int units);
+
+        /* Absolute speeds, but allowing fractions. Useful for PID, etc. */
+        float motor_fl_speed;
+        float motor_fr_speed;
+        float motor_bl_speed;
+        float motor_br_speed;
+
+        /** ROS publisher and client declaration */
+        ros::Publisher set_motors_pub;
+        ros::ServiceClient query_motors_client;
+
+        ros::NodeHandle node;
+
+};
+
+#endif
+
diff --git a/scout/libscout/src/alex_behavior.cpp b/scout/libscout/src/alex_behavior.cpp
new file mode 100644
index 0000000..2701bc4
--- /dev/null
+++ b/scout/libscout/src/alex_behavior.cpp
@@ -0,0 +1,61 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+/**
+ * @file alex_behavior.cpp
+ * @brief A simple robot behavior, smart-runaround style.
+ * 
+ * @author Colony Project, CMU Robotics Club
+ * @author Alex Zirbel
+ */
+
+#include "libscout.h"
+
+/**
+ * @brief Main. The main function for the behavior.
+ * 
+ * This is the main function for libscout. It calls init() which initializes
+ * the clients and publishers/subscribers for the other parts of the library.
+ */
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "libscout");
+
+    ros::NodeHandle node;
+
+    MotorControl motors(node);
+
+    ros::Rate loop_rate(10);
+
+    while(ros::ok())
+    {
+        motors.set_sides(20, 80);
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    return 0;
+}
diff --git a/scout/libscout/src/behavior.cpp b/scout/libscout/src/behavior.cpp
index c74a94d..10f9001 100644
--- a/scout/libscout/src/behavior.cpp
+++ b/scout/libscout/src/behavior.cpp
@@ -71,7 +71,6 @@ int main(int argc, char **argv){
     msg.bl_speed = 125;
     msg.fr_speed = 125;
     msg.br_speed = 125;
-    msg.which = 0;
     msg.units = MOTOR_ABSOLUTE;
 
     /* publish */
diff --git a/scout/libscout/src/constants.h b/scout/libscout/src/constants.h
deleted file mode 100644
index 779dcda..0000000
--- a/scout/libscout/src/constants.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/**
- * Copyright (c) 2011 Colony Project
- * 
- * Permission is hereby granted, free of charge, to any person
- * obtaining a copy of this software and associated documentation
- * files (the "Software"), to deal in the Software without
- * restriction, including without limitation the rights to use,
- * copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following
- * conditions:
- * 
- * The above copyright notice and this permission notice shall be
- * included in all copies or substantial portions of the Software.
- * 
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
- * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
- * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
- * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- **/
-
-
-/**
- * @file constants.h
- * @brief Contains top level scout library constants
- * 
- * Contains constants for use in libscout
- *
- * @author Colony Project, CMU Robotics Club
- * @author Ben Wasserman
- **/
-
-#ifndef _CONSTANTS_H_
-#define _CONSTANTS_H_
-
-#include "ros/ros.h"
-
-/* ROS defines */
-#define QUEUE_SIZE 10
-
-/* Libscout Defines */
-
-/* Init defines */
-#define LIB_ALL -1
-#define LIB_MOTORS 0x1
-#define LIB_SONAR 0x2
-#define LIB_CLIFFSENSORS 0x4
-#define LIB_HEADLIGHTS 0x8
-#define LIB_BUTTONS 0x10
-
-/* Status defines */
-//TODO MAKE ENUMS
-#define LIB_ERROR -1
-#define LIB_OK 0
-
-
-#endif
diff --git a/scout/libscout/src/libheadlights.cpp b/scout/libscout/src/libheadlights.cpp
deleted file mode 100644
index 53f576c..0000000
--- a/scout/libscout/src/libheadlights.cpp
+++ /dev/null
@@ -1,127 +0,0 @@
-/**
- * Copyright (c) 2011 Colony Project
- * 
- * Permission is hereby granted, free of charge, to any person
- * obtaining a copy of this software and associated documentation
- * files (the "Software"), to deal in the Software without
- * restriction, including without limitation the rights to use,
- * copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following
- * conditions:
- * 
- * The above copyright notice and this permission notice shall be
- * included in all copies or substantial portions of the Software.
- * 
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
- * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
- * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
- * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- **/
-
-
-/**
- * @file libheadlights.cpp
- * @brief Contains headlights declarations and functions
- * 
- * Contains functions and definitions for the use of
- * headlights
- *
- * @author Colony Project, CMU Robotics Club
- * @author Ben Wasserman
- **/
-
-#include "libheadlights.h"
-
-/* ROS node created in libscout.cpp */
-extern ros::NodeHandle node;
-
-/** ROS publisher and client declaration */
-ros::Publisher set_headlights_publisher;
-
-/*!
- * \brief Initialize the headlights module of libscout.
- *
- * Initialize the libscout node as a publisher of set_headlights.
- */
-void libheadlights_init()
-{
-  set_headlights_publisher =
-		node.advertise<headlights::set_headlights>("libheadlights", 10);
-}
-
-/*!
- * \brief Set headlight colors as a single RGB value
- * Sets the colors of the headlights as a web color value. Can selectively
- * select which motors to set, and which to remain at previous color.
- * \param color The color the headlights should be set to expressed as 0xRRGGBB
- * \param which A bitmask of which headlights should be set
- * \return Function status
- */
-int headlights_set(int color, int which)
-{
-  int red, green, blue;
-	
-	red = (color & RED) >> REDSHIFT;
-	green = (color & GREEN) >> GREENSHIFT;
-	blue = (color & BLUE) >> BLUESHIFT;
-	
-	return headlights_set_rgb(red, green, blue, which);
-}
-
-/*!
- * \brief Set headlight colors as separate RGB values
- * Sets the colors of the headlights as a web color value. Can selectively
- * select which motors to set, and which to remain at previous color.
- * \param red The red value the headlights should be set to
- * \param green The green value the headlights should be set to
- * \param blue The blue value the headlights should be set to
- * \param which A bitmask of which headlights should be set
- * \return Function status
- */
-int headlights_set_rgb(int red, int green, int blue, int which)
-{
-  /** \todo Set fields of the message based on params */
-  
-	headlights::set_headlights msg;
-	
-  if(!ros::ok())
-	{
-    return LIB_ERROR;
-  }
- 
-	if(which & HL_LEFT)
-	{
-		msg.left_red = red;
-		msg.left_green = green;
-		msg.left_blue = blue;
-	}
-	else
-	{
-		msg.left_red = NO_SET;
-		msg.left_green = NO_SET;
-		msg.left_blue = NO_SET;
-	}
-	if(which & HL_RIGHT)
-	{
-		msg.right_red = red;
-		msg.right_green = green;
-		msg.right_blue = blue;
-	}
-	else
-	{
-		msg.right_red = NO_SET;
-		msg.right_green = NO_SET;
-		msg.right_blue = NO_SET;
-	}
-	
-  /* Publishes message to set_motors topic */
-  set_headlights_publisher.publish(msg);
-  ros::spinOnce();
-
-  return LIB_OK;
-}
diff --git a/scout/libscout/src/libheadlights.h b/scout/libscout/src/libheadlights.h
deleted file mode 100644
index 9406c00..0000000
--- a/scout/libscout/src/libheadlights.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/**
- * Copyright (c) 2011 Colony Project
- * 
- * Permission is hereby granted, free of charge, to any person
- * obtaining a copy of this software and associated documentation
- * files (the "Software"), to deal in the Software without
- * restriction, including without limitation the rights to use,
- * copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following
- * conditions:
- * 
- * The above copyright notice and this permission notice shall be
- * included in all copies or substantial portions of the Software.
- * 
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
- * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
- * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
- * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- **/
-
-
-/**
- * @file libheadlights.h
- * @brief Contains headlights declarations and functions
- * 
- * Contains functions and definitions for the use of
- * headlights
- *
- * @author Colony Project, CMU Robotics Club
- * @author Ben Wasserman
- **/
-
-#include "ros/ros.h"
-#include "constants.h"
-#include "headlights/set_headlights.h"
-
-#ifndef _LIBheadlights_H_
-#define _LIBheadlights_H_
-
-/* Defines */
-#define WHITE		0xFFFFFF
-#define OFF			0x000000
-#define RED 		0xFF0000
-#define GREEN 	0x00FF00
-#define BLUE		0x0000FF
-#define YELLOW	0xFFFF00
-#define CYAN		0x00FFFF
-#define PINK		0xFF00FF
-
-#define HL_RIGHT	0x1
-#define HL_LEFT		0x2
-#define HL_BOTH		0x3
-
-#define NO_SET	-1
-
-#define REDSHIFT 		16
-#define GREENSHIFT 	 8
-#define BLUESHIFT		 0
-
-void libheadlights_init();
-int headlights_set(int color, int which);
-int headlights_set_rgb(int red, int green, int blue, int which);
-
-#endif
-
diff --git a/scout/libscout/src/libmotors.cpp b/scout/libscout/src/libmotors.cpp
deleted file mode 100644
index 41d057b..0000000
--- a/scout/libscout/src/libmotors.cpp
+++ /dev/null
@@ -1,226 +0,0 @@
-/**
- * Copyright (c) 2011 Colony Project
- * 
- * Permission is hereby granted, free of charge, to any person
- * obtaining a copy of this software and associated documentation
- * files (the "Software"), to deal in the Software without
- * restriction, including without limitation the rights to use,
- * copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following
- * conditions:
- * 
- * The above copyright notice and this permission notice shall be
- * included in all copies or substantial portions of the Software.
- * 
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
- * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
- * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
- * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- **/
-
-
-/**
- * @file libmotors.cpp
- * @brief Contains motor declarations and functions
- * 
- * Contains functions and definitions for the use of
- * motors
- *
- * @author Colony Project, CMU Robotics Club
- **/
-
-#include "libmotors.h"
-
-/* ROS node created in libscout.cpp */
-extern ros::NodeHandle node;
-
-/** ROS publisher and client declaration */
-ros::Publisher set_motors_publisher;
-ros::ServiceClient query_motors_client;
-
-/**
- * @brief Initialize the motors module of libscout.
- *
- * Initialize the libscout node as a publisher of set_motors and a client of
- * query_motors.
- */
-void libmotors_init()
-{
-    set_motors_publisher =
-        node.advertise<motors::set_motors>("set_motors", QUEUE_SIZE);
-    query_motors_client =
-        node.serviceClient<motors::query_motors>("query_motors");
-}
-
-/**
- * @brief Sets all the speeds according to the specificiation of which motors
- *        to set.
- *
- * @param which A bitmask of which motors need to be set.
- * @param speed The value to set the motors to.
- * @param units The units the speed is expressed in.
- * @return Function status
- */
-int motors_set(int which, float speed, char units)
-{
-    float speed_fl, speed_fr, speed_bl, speed_br;
-    speed_fl = speed_fr = speed_bl = speed_br = speed;
-
-    if(which & MOTOR_FL_REV)
-    {
-        speed_fl = -speed;
-    }
-    if(which & MOTOR_FR_REV)
-    {
-        speed_fr = -speed;
-    }
-    if(which & MOTOR_BL_REV)
-    {
-        speed_bl = -speed;
-    }
-    if(which & MOTOR_BR_REV)
-    {
-        speed_br = -speed;
-    }
-
-    return motors_set_each(which, speed_fl, speed_fr, speed_bl, speed_br,
-                           units);
-}
-
-/**
- * @brief Sets the left and right sides of scout to different speeds.
- *
- * @param speed_l The speed of both left motors.
- * @param speed_r The speed of both right motors.
- * @param units The units to set to.
- * @return Function status
- */
-int motors_set_sides(float speed_l, float speed_r, char units)
-{
-    return motors_set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r,
-                               units);
-}
-
-/**
- * @brief Set motor speeds
- * Sets the speeds of the motors as a percentage of top speed. Can selectively
- * select which motors to set, and which to remain at previous speed.
- *
- * @param which A bitmask of which motors should be set.
- * @param speed The speed the motors should be set to.
- * @param units Optional units for the speeds.
- * @return Function status
- */
-int motors_set_each(int which,
-                    float speed_fl, float speed_fr,
-                    float speed_bl, float speed_br,
-                    char units)
-{
-    check_which_ok(which);
-
-    motors::set_motors msg;
-
-    if(!ros::ok())
-    {
-        return LIB_ERROR;
-    }
-
-    /* Set all the speeds (the booleans in the set_motors message determine
-     * which ones will be used) */
-    msg.fl_speed = speed_fl;
-    msg.fr_speed = speed_fr;
-    msg.bl_speed = speed_bl;
-    msg.br_speed = speed_br;
-
-    /* Tell the motors node which motors need to be updated */
-    msg.fl_set = msg.fr_set = msg.bl_set = msg.br_set = false;
-    if(which & (MOTOR_FL | MOTOR_FL_REV))
-    {
-        msg.fl_set = true;
-    }
-    if(which & (MOTOR_FR | MOTOR_FR_REV))
-    {
-        msg.fr_set = true;
-    }
-    if(which & (MOTOR_BL | MOTOR_BL_REV))
-    {
-        msg.bl_set = true;
-    }
-    if(which & (MOTOR_BR | MOTOR_BR_REV))
-    {
-        msg.br_set = true;
-    }
-
-    /* Specify which units the speeds are given in */
-    msg.units = units;
-
-    /* Publishes message to set_motors topic */
-    set_motors_publisher.publish(msg);
-    ros::spinOnce();
-
-    return LIB_OK;
-}
-
-/**
- * Double-checks the which variable to make sure the front and back
- * bits are not both set for a single motor.
- *
- * @param which The which motor specfication to check.
- */
-void check_which_ok(int which)
-{
-    ROS_WARN_COND( ((which & MOTOR_FL) && (which & MOTOR_FL_REV)),
-        "FL Motor set in both directions!");
-    ROS_WARN_COND( ((which & MOTOR_FR) && (which & MOTOR_FR_REV)),
-        "FR Motor set in both directions!");
-    ROS_WARN_COND( ((which & MOTOR_BL) && (which & MOTOR_BL_REV)),
-        "BL Motor set in both directions!");
-    ROS_WARN_COND( ((which & MOTOR_BR) && (which & MOTOR_BR_REV)),
-        "BR Motor set in both directions!");
-}
-
-/**
- * @brief Query the current speeds of the motors
- *
- * Sends a request to the query_motors service which will reply with the
- *  current speed of each motor.
- *
- * @todo Change so we can get multiple motor speeds with one call
- *
- * @param which A bitmask that will specify which motor speed should be
- *  returned
- * @return The speed of the selected motor, or LIB_ERR if no motor selected
- */
-float motors_query(int which)
-{
-    motors::query_motors srv;
-    if(query_motors_client.call(srv))
-    {
-        switch(which)
-        {
-            case MOTOR_FL:
-                return srv.response.fl_speed;
-            case MOTOR_FR:
-                return srv.response.fr_speed;
-            case MOTOR_BL:
-                return srv.response.bl_speed;
-            case MOTOR_BR:
-                return srv.response.br_speed;
-            default:
-                ROS_WARN("Bad WHICH in motors_query.");
-                return LIB_ERROR;
-        }
-    }
-    else
-    {
-        ROS_ERROR("Failed to call service query_motors");
-        return LIB_ERROR;
-    }
-
-    return 0;
-}
diff --git a/scout/libscout/src/libmotors.h b/scout/libscout/src/libmotors.h
deleted file mode 100644
index 8d0a987..0000000
--- a/scout/libscout/src/libmotors.h
+++ /dev/null
@@ -1,84 +0,0 @@
-/**
- * Copyright (c) 2011 Colony Project
- * 
- * Permission is hereby granted, free of charge, to any person
- * obtaining a copy of this software and associated documentation
- * files (the "Software"), to deal in the Software without
- * restriction, including without limitation the rights to use,
- * copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following
- * conditions:
- * 
- * The above copyright notice and this permission notice shall be
- * included in all copies or substantial portions of the Software.
- * 
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
- * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
- * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
- * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- **/
-
-
-/**
- * @file libmotors.h
- * @brief Contains motor declarations and functions
- * 
- * Contains functions and definitions for the use of
- * motors
- *
- * @author Colony Project, CMU Robotics Club
- **/
-
-#include "ros/ros.h"
-#include "constants.h"
-#include "motors/query_motors.h"
-#include "motors/set_motors.h"
-
-#ifndef _LIBMOTORS_H_
-#define _LIBMOTORS_H_
-
-/* Defines */
-#define MOTOR_ALL 0xF
-#define MOTOR_ALL_REV 0xF0
-#define MOTOR_NONE 0x0
-#define MOTOR_FL 0x8
-#define MOTOR_FR 0x4
-#define MOTOR_BL 0x2
-#define MOTOR_BR 0x1
-#define MOTOR_FL_REV 0x80
-#define MOTOR_FR_REV 0x40
-#define MOTOR_BL_REV 0x20
-#define MOTOR_BR_REV 0x10
-#define MOTOR_FRONT MOTOR_FL | MOTOR_FR
-#define MOTOR_BACK MOTOR_BR | MOTOR_BR
-#define MOTOR_LEFT MOTOR_FL | MOTOR_BL
-#define MOTOR_RIGHT MOTOR_FR | MOTOR_BR
-#define MOTOR_LEFT_REV MOTOR_FL_REV | MOTOR_BL_REV
-#define MOTOR_RIGHT_REV MOTOR_FR_REV | MOTOR_BR_REV
-#define MOTOR_LEFT_SPIN MOTOR_LEFT_REV | MOTOR_RIGHT
-#define MOTOR_RIGHT_SPIN MOTOR_LEFT | MOTOR_RIGHT_REV
-
-#define MOTOR_PERCENT 'p'
-#define MOTOR_MMS 'm'
-#define MOTOR_CMS 'c'
-#define MOTOR_ABSOLUTE 'a'
-#define MOTOR_DEFAULT_UNIT MOTOR_PERCENT
-
-void libmotors_init();
-int motors_set(int which, float speed, char units=MOTOR_DEFAULT_UNIT);
-int motors_set_sides(float speed_l, float speed_r,
-                     char units=MOTOR_DEFAULT_UNIT);
-int motors_set_each(int which,
-                    float speed_fl, float speed_fr,
-                    float speed_bl, float speed_br,
-                    char units=MOTOR_DEFAULT_UNIT);
-void check_which_ok(int which);
-float motors_query(int which);
-
-#endif
-
diff --git a/scout/libscout/src/libscout.cpp b/scout/libscout/src/libscout.cpp
index f8fa587..2465714 100644
--- a/scout/libscout/src/libscout.cpp
+++ b/scout/libscout/src/libscout.cpp
@@ -21,8 +21,7 @@
  * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
  * OTHER DEALINGS IN THE SOFTWARE.
- **/
-
+ */
 
 /**
  * @file libscout.cpp
@@ -33,46 +32,53 @@
  *
  * @author Colony Project, CMU Robotics Club
  * @author Ben Wasserman
- **/
+ */
 
 #include "libscout.h"
 
-/* Global objects */
-ros::NodeHandle node;
-
-/** \todo Decide how to call user behaviors
-  * I'm thinking that there be a function call in main that calls the "main"
-  fn in the user behavior. However, their "main" function will not be called 
-  main. However, this has to be able to point to different functions by 
-  changing as little code in this file as possible. Also, the user behaviors
-  should live in a different directory (possibly a different package).
-  However, this other package should not be executed as a separate node,
-  because that is the job of the libscout node/package to handle all the ROS 
-  stuff, and keep it under the lid. The user only has to call normal c 
-  functions. I also want to reduce the need to recompile this package when the 
-  user behavior, or any other package that works below the surface.
-**/
+/** @todo Decide how to call user behaviors
+ *
+ * I'm thinking that there be a function call in main that calls the "main"
+ fn in the user behavior. However, their "main" function will not be called 
+ main. However, this has to be able to point to different functions by 
+ changing as little code in this file as possible. Also, the user behaviors
+ should live in a different directory (possibly a different package).
+ However, this other package should not be executed as a separate node,
+ because that is the job of the libscout node/package to handle all the ROS 
+ stuff, and keep it under the lid. The user only has to call normal c 
+ functions. I also want to reduce the need to recompile this package when the 
+ user behavior, or any other package that works below the surface.
+ */
 
-/*!
- * \brief Initializes modules in the libscout.
+/**
+ * @brief Initializes modules in the libscout.
  * 
  * Calls init functions for each module in libscout.
- * \param modules A bitmask of the modules that will be initialized.
- **/
-int init(int modules, int argc, char **argv){
-	ros::init(argc, argv, "libscout");
+ * @param modules A bitmask of the modules that will be initialized.
+ */
+int init(int modules, int argc, char **argv)
+{
+    ros::init(argc, argv, "libscout");
+
+    ros::NodeHandle node;
+
+    MotorControl motors = new MotorControl(node);
+    HeadLightControl headlights = new HeadlightControl(node);
 
-  /** \todo Copy this if for each module that gets added to the library */
-  if(modules & LIB_MOTORS){
-    libmotors_init();
-  }
-	if(modules & LIB_HEADLIGHTS){
-    libheadlights_init();
-  }
-  if(modules & LIB_BUTTONS){
-    libbuttons_init();
-  }
-  /** \todo Add other lib inits **/
-  return 0;
+    /** @todo Copy this if for each module that gets added to the library */
+    /*if(modules & LIB_MOTORS)
+    {
+        libmotors_init();
+    }
+    if(modules & LIB_HEADLIGHTS)
+    {
+        libheadlights_init();
+    }
+    if(modules & LIB_BUTTONS)
+    {
+        libbuttons_init();
+    }*/
+    /** @todo Add other lib inits **/
+    return 0;
 }
 
diff --git a/scout/libscout/src/libscout.h b/scout/libscout/src/libscout.h
index f9a6c68..8b2e857 100644
--- a/scout/libscout/src/libscout.h
+++ b/scout/libscout/src/libscout.h
@@ -23,7 +23,6 @@
  * OTHER DEALINGS IN THE SOFTWARE.
  **/
 
-
 /**
  * @file libscout.h
  * @brief Contains top level scout library functions
@@ -41,11 +40,11 @@
 #ifndef _LIBSCOUT_H_
 #define _LIBSCOUT_H_
 
-#include "ros/ros.h"
-#include "constants.h"
-#include "libmotors.h"
-#include "libheadlights.h"
-#include "libbuttons.h"
+#include <ros/ros.h>
+#include <libscout/constants.h>
+
+#include "MotorControl.h"
+#include "HeadlightControl.h"
 
 /* Libscout functions */
 int init(int modules, int argc, char **argv);
diff --git a/scout/motors/msg/set_motors.msg b/scout/motors/msg/set_motors.msg
index 5371de7..9756229 100644
--- a/scout/motors/msg/set_motors.msg
+++ b/scout/motors/msg/set_motors.msg
@@ -1,21 +1,13 @@
 Header header
 
-# Keeps track of which motors need to be changed
-int8 which
-
 # Set these to true if the motor should be updated
 bool fl_set
 bool fr_set
 bool bl_set
 bool br_set
 
-# The motor speeds, from -255 to 255 if absolute
-# -100 to 100 if percentage
-# Decimal values if in measured units
-float32 fl_speed
-float32 fr_speed
-float32 bl_speed
-float32 br_speed
-
-# Allows motor speeds to be specified mm/s, cm/s, percent, or absolute
-int8 units
+# The absolute motor speeds to set
+int8 fl_speed
+int8 fr_speed
+int8 bl_speed
+int8 br_speed
diff --git a/scout/motors/src/motors.cpp b/scout/motors/src/motors.cpp
index 7709c36..369bb5f 100644
--- a/scout/motors/src/motors.cpp
+++ b/scout/motors/src/motors.cpp
@@ -33,11 +33,9 @@
  * @author Ben Wasserman
  **/
 
-#include "ros/ros.h"
-#include "motors_util.cpp"
-#include "motors.h"
-//#include "libscout/src/constants.h"
+#include <ros/ros.h>
 #include <cstdlib>
+#include "motors.h"
 
 /**
  * @defgroup motors Motors
@@ -46,8 +44,6 @@
  * @{
  **/
 
-
-
 /* Motor state variables
  * Speeds expressed as absolute speed out of max speed (0 - +-MAXSPEED)
  * Absolute speed is the speed written to the hardware to move the motors
@@ -69,22 +65,23 @@ void motors_set(const motors::set_motors::ConstPtr& msg)
 {
     if(msg->fl_set)
     {
-      motor_fl_speed = motors_rel_to_abs(msg->fl_speed, msg->units);
+      motor_fl_speed = msg->fl_speed;
     }
     if(msg->fr_set)
     {
-      motor_fr_speed = motors_rel_to_abs(msg->fr_speed, msg->units);
+      motor_fr_speed = msg->fr_speed;
     }
     if(msg->bl_set)
     {
-      motor_bl_speed = motors_rel_to_abs(msg->bl_speed, msg->units);
+      motor_bl_speed = msg->bl_speed;
     }
     if(msg->br_set)
     {
-      motor_br_speed = motors_rel_to_abs(msg->br_speed, msg->units);
+      motor_br_speed = msg->br_speed;
     }
 
     /** @todo Add code to write speeds to hardware */
+
     ROS_DEBUG("Motor speeds set");
 }
 
@@ -99,13 +96,15 @@ void motors_set(const motors::set_motors::ConstPtr& msg)
 bool motors_query(motors::query_motors::Request &req,
                   motors::query_motors::Response &res)
 {
-    int units = req.units;
-    res.fl_speed = motors_abs_to_rel(motor_fl_speed, units);
-    res.fr_speed = motors_abs_to_rel(motor_fr_speed, units);
-    res.bl_speed = motors_abs_to_rel(motor_bl_speed, units);
-    res.br_speed = motors_abs_to_rel(motor_br_speed, units);
+    /** @todo Are we checking hardware or just using the saved values?
+     *  Saved values sound fine to me (Alex). Pleas confirm. */
+    res.fl_speed = motor_fl_speed;
+    res.fr_speed = motor_fr_speed;
+    res.bl_speed = motor_bl_speed;
+    res.br_speed = motor_br_speed;
 
     ROS_DEBUG("Motor speeds queried");
+
     return true;
 }
 
@@ -136,7 +135,7 @@ int main(int argc, char **argv)
     /* Initialize hardware for motors */
     // Hardware init functions here
 
-    ROS_INFO("Ready to set motors.");
+    ROS_INFO("Motors node ready.");
     ros::spin();
 
     return 0;
diff --git a/scout/motors/src/motors.h b/scout/motors/src/motors.h
index 476cc16..ffced0e 100644
--- a/scout/motors/src/motors.h
+++ b/scout/motors/src/motors.h
@@ -32,24 +32,16 @@
  *
  * @author Colony Project, CMU Robotics Club
  * @author Benjamin Wasserman
+ * @author Alex Zirbel
  **/
 
 #ifndef _MOTORS_H_
 #define _MOTORS_H_
 
-#include "motors/query_motors.h"
-#include "motors/set_motors.h"
-
-#define MAXSPEED 255
-#define MOTOR_PERCENT 'p'
-#define MOTOR_MMS 'm'
-#define MOTOR_CMS 'c'
-#define MOTOR_ABSOLUTE 'a'
-
-/* The number of messages in the queue. If messages arrive faster than they are
- * handled, old ones are thrown out */
-#define QUEUE_SIZE 4
+#include <motors/set_motors.h>
+#include <motors/query_motors.h>
 
+#define QUEUE_SIZE 10
 
 /** @brief Initialize the motors module and driver. **/
 int main(int argc, char **argv);
diff --git a/scout/motors/src/motors_util.cpp b/scout/motors/src/motors_util.cpp
deleted file mode 100644
index 92f23ae..0000000
--- a/scout/motors/src/motors_util.cpp
+++ /dev/null
@@ -1,98 +0,0 @@
-/**
- * Copyright (c) 2011 Colony Project
- * 
- * Permission is hereby granted, free of charge, to any person
- * obtaining a copy of this software and associated documentation
- * files (the "Software"), to deal in the Software without
- * restriction, including without limitation the rights to use,
- * copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following
- * conditions:
- * 
- * The above copyright notice and this permission notice shall be
- * included in all copies or substantial portions of the Software.
- * 
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
- * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
- * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
- * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- **/
-
-/**
- * @file motors_util.cpp
- * @brief Contains helpful functions this node, or others, can use to control
- *        the motors.
- *
- * @author Colony Project, CMU Robotics Club
- * @author Ben Wasserman
- * @author Alex Zirbel
- **/
-
-#include "motors.h"
-#include <cstdlib>
-
-/**
- * @defgroup motors
- * @{
- **/
-
-/**
- * @brief Converts set speeds (of various units) to absolute speeds.
- *
- * @param speed The speed expressed in the desired units
- * @param units The units the desired speed is measured in
- * @return The absolute speed of the motor
- **/
-int motors_rel_to_abs(int rel_speed, int units)
-{
-    switch(units)
-    {
-      case MOTOR_ABSOLUTE:/* Speed given as absolute */
-        return rel_speed;
-      case MOTOR_PERCENT:/* Convert from percentage */
-        return rel_speed * MAXSPEED / 100;
-      case MOTOR_MMS:/* Convert from mm/s */
-        /** @todo Make math to do this conversion **/
-        return rel_speed;
-      case MOTOR_CMS:/* Convert from cm/s */
-        /** @todo Make math to do this conversion **/
-        return rel_speed;
-      default: /* The units aren't recognized */
-        /** @todo Decide on default case. Either percent or absolute. **/
-        return rel_speed;
-    }
-}
-
-/**
- * @brief Convert absolute speeds to speeds of various units.
- *
- * @param speed The speed expressed in absolute units.
- * @param units The units the desired speed is measured in.
- * @return The relative speed of the motor in desired units.
- **/
-int motors_abs_to_rel(int abs_speed, int units)
-{
-    switch(units)
-    {
-      case MOTOR_ABSOLUTE:/* Speed given as absolute */
-        return abs_speed;
-      case MOTOR_PERCENT:/* Convert from percentage */
-        return abs_speed * 100 / MAXSPEED;
-      case MOTOR_MMS:/* Convert from mm/s */
-        /** @todo Make math to do this conversion **/
-        return abs_speed;
-      case MOTOR_CMS:/* Convert from cm/s */
-        /** @todo Make math to do this conversion **/
-        return abs_speed;
-      default: /* The units aren't recognized */
-        /** @todo Decide on default case. Either percent or absolute. **/
-        return abs_speed;
-    }
-}
-
-/** @} **/
diff --git a/scout/motors/srv/query_motors.srv b/scout/motors/srv/query_motors.srv
index b54aa0e..6db1530 100644
--- a/scout/motors/srv/query_motors.srv
+++ b/scout/motors/srv/query_motors.srv
@@ -1,4 +1,3 @@
-int8 units
 ---
 int8 fl_speed
 int8 fr_speed
diff --git a/scout/scoutsim/CMakeLists.txt b/scout/scoutsim/CMakeLists.txt
index cd79b10..1ec2c32 100644
--- a/scout/scoutsim/CMakeLists.txt
+++ b/scout/scoutsim/CMakeLists.txt
@@ -32,6 +32,14 @@ rosbuild_add_executable(scoutsim_node src/scoutsim.cpp src/scout.cpp src/sim_fra
 rosbuild_link_boost(scoutsim_node thread)
 target_link_libraries(scoutsim_node ${wxWidgets_LIBRARIES})
 
+
+include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake)
+include_directories(${MOTORS_INCLUDE_DIRS})
+link_directories(${MOTORS_LIBRARY_DIRS})
+target_link_libraries(scoutsim_node ${MOTORS_LIBRARIES})
+
+target_link_libraries(scoutsim_node motors)
+
 # @TODO are these useful or necessary? Determine; add as needed
 #rosbuild_add_executable(turtle_teleop_key tutorials/teleop_turtle_key.cpp)
 #rosbuild_add_executable(draw_square tutorials/draw_square.cpp)
diff --git a/scout/scoutsim/images/palette.png b/scout/scoutsim/images/palette.png
deleted file mode 100644
index 7f8d11d..0000000
Binary files a/scout/scoutsim/images/palette.png and /dev/null differ
diff --git a/scout/scoutsim/images/scout.png b/scout/scoutsim/images/scout.png
index 01e1701..df005ad 100644
Binary files a/scout/scoutsim/images/scout.png and b/scout/scoutsim/images/scout.png differ
diff --git a/scout/scoutsim/images/scout_forreal.png b/scout/scoutsim/images/scout_forreal.png
deleted file mode 100644
index df005ad..0000000
Binary files a/scout/scoutsim/images/scout_forreal.png and /dev/null differ
diff --git a/scout/scoutsim/images/turtle.png b/scout/scoutsim/images/turtle.png
deleted file mode 100644
index da835ad..0000000
Binary files a/scout/scoutsim/images/turtle.png and /dev/null differ
diff --git a/scout/scoutsim/include/scoutsim/scout.h b/scout/scoutsim/include/scoutsim/scout.h
deleted file mode 100644
index 6bee4f8..0000000
--- a/scout/scoutsim/include/scoutsim/scout.h
+++ /dev/null
@@ -1,125 +0,0 @@
-/*
- * This package was developed by the CMU Robotics Club project using code
- * from Willow Garage, Inc.  Please see licensing.txt for details.
- *
- * All rights reserved.
- *
- * @file scout.h
- * @author Colony Project, CMU Robotics Club
- * @author Alex Zirbel
- */
-
-#ifndef SCOUTSIM_SCOUT_H
-#define SCOUTSIM_SCOUT_H
-
-#include <ros/ros.h>
-#include <boost/shared_ptr.hpp>
-
-#include <scoutsim/Pose.h>
-#include <scoutsim/Velocity.h>
-#include <scoutsim/SetPen.h>
-#include <scoutsim/TeleportRelative.h>
-#include <scoutsim/TeleportAbsolute.h>
-#include <scoutsim/Color.h>
-
-#include <wx/wx.h>
-
-#define PI 3.14159265
-
-namespace scoutsim
-{
-
-    struct Vector2
-    {
-        Vector2()
-            : x(0.0)
-              , y(0.0)
-        {}
-
-        Vector2(float _x, float _y)
-            : x(_x)
-              , y(_y)
-        {}
-
-        bool operator==(const Vector2& rhs)
-        {
-            return x == rhs.x && y == rhs.y;
-        }
-
-        bool operator!=(const Vector2& rhs)
-        {
-            return x != rhs.x || y != rhs.y;
-        }
-
-        float x;
-        float y;
-    };
-
-    class Scout
-    {
-        public:
-            Scout(const ros::NodeHandle& nh, const wxImage& scout_image,
-                  const Vector2& pos, float orient);
-
-            void update(double dt, wxMemoryDC& path_dc,
-                        const wxImage& path_image, wxColour background_color,
-                        float canvas_width, float canvas_height);
-            void paint(wxDC& dc);
-
-        private:
-            void velocityCallback(const VelocityConstPtr& vel);
-            bool setPenCallback(scoutsim::SetPen::Request&,
-                                scoutsim::SetPen::Response&);
-            bool teleportRelativeCallback(scoutsim::TeleportRelative::Request&,
-                                          scoutsim::TeleportRelative::Response&);
-            bool teleportAbsoluteCallback(scoutsim::TeleportAbsolute::Request&,
-                                          scoutsim::TeleportAbsolute::Response&);
-
-            ros::NodeHandle nh_;
-
-            wxImage scout_image_;
-            wxBitmap scout_;
-
-            Vector2 pos_;
-            float orient_;
-
-            float lin_vel_;
-            float ang_vel_;
-            bool pen_on_;
-            wxPen pen_;
-
-            ros::Subscriber velocity_sub_;
-            ros::Publisher pose_pub_;
-            ros::Publisher color_pub_;
-            ros::ServiceServer set_pen_srv_;
-            ros::ServiceServer teleport_relative_srv_;
-            ros::ServiceServer teleport_absolute_srv_;
-
-            ros::WallTime last_command_time_;
-
-            float meter_;
-
-            struct TeleportRequest
-            {
-                TeleportRequest(float x, float y, float _theta,
-                                float _linear, bool _relative)
-                    : pos(x, y)
-                      , theta(_theta)
-                      , linear(_linear)
-                      , relative(_relative)
-                {}
-
-                Vector2 pos;
-                float theta;
-                float linear;
-                bool relative;
-            };
-
-            typedef std::vector<TeleportRequest> V_TeleportRequest;
-            V_TeleportRequest teleport_requests_;
-    };
-    typedef boost::shared_ptr<Scout> ScoutPtr;
-
-}
-
-#endif
diff --git a/scout/scoutsim/include/scoutsim/sim_frame.h b/scout/scoutsim/include/scoutsim/sim_frame.h
deleted file mode 100644
index c07a713..0000000
--- a/scout/scoutsim/include/scoutsim/sim_frame.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * This package was developed by the CMU Robotics Club project using code
- * from Willow Garage, Inc.  Please see licensing.txt for details.
- *
- * All rights reserved.
- *
- * @file sim_frame.h
- * @author Colony Project, CMU Robotics Club
- * @author Alex Zirbel
- */
-
-#include <wx/wx.h>
-#include <wx/event.h>
-#include <wx/timer.h>
-
-#include <ros/ros.h>
-
-#include <std_srvs/Empty.h>
-#include <scoutsim/Spawn.h>
-#include <scoutsim/Kill.h>
-#include <map>
-
-#include "scout.h"
-
-#define SCOUTSIM_NUM_SCOUTS 5
-
-namespace scoutsim
-{
-    class SimFrame : public wxFrame
-    {
-        public:
-            SimFrame(wxWindow* parent);
-            ~SimFrame();
-
-            std::string spawnScout(const std::string& name,
-                    float x, float y, float angle);
-
-        private:
-            void onUpdate(wxTimerEvent& evt);
-            void onPaint(wxPaintEvent& evt);
-
-            void updateScouts();
-            void clear();
-            bool hasScout(const std::string& name);
-
-            bool clearCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-            bool resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-            bool spawnCallback(scoutsim::Spawn::Request&, scoutsim::Spawn::Response&);
-            bool killCallback(scoutsim::Kill::Request&, scoutsim::Kill::Response&);
-
-            ros::NodeHandle nh_;
-            wxTimer* update_timer_;
-            wxBitmap path_bitmap_;
-            wxImage path_image_;
-            wxMemoryDC path_dc_;
-
-            uint64_t frame_count_;
-
-            ros::WallTime last_scout_update_;
-
-            ros::ServiceServer clear_srv_;
-            ros::ServiceServer reset_srv_;
-            ros::ServiceServer spawn_srv_;
-            ros::ServiceServer kill_srv_;
-
-            typedef std::map<std::string, ScoutPtr> M_Scout;
-            M_Scout scouts_;
-            uint32_t id_counter_;
-
-            wxImage scout_images_[SCOUTSIM_NUM_SCOUTS];
-
-            float meter_;
-            float width_in_meters_;
-            float height_in_meters_;
-    };
-
-}
diff --git a/scout/scoutsim/manifest.xml b/scout/scoutsim/manifest.xml
index a8e681f..54a2d89 100644
--- a/scout/scoutsim/manifest.xml
+++ b/scout/scoutsim/manifest.xml
@@ -13,11 +13,15 @@
   <depend package="roslib"/>
   <depend package="rosconsole"/>
   <depend package="std_srvs"/>
+  <depend package="motors" />
   
   <rosdep name="wxwidgets"/>
 
-  <export>
+  <!--<export>
     <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp"/>
+  </export>-->
+  <export>
+    <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp -I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmotors"/>
   </export>
 
   <platform os="ubuntu" version="9.04"/>
@@ -29,6 +33,7 @@
     <depend package="roslib"/>
     <depend package="rosconsole"/>
     <depend package="std_srvs"/>
+    <depend package="motors" />
 
     <msgs>
       msg/Color.msg  msg/Pose.msg  msg/Velocity.msg 
diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index 287f0e9..662de4c 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -10,7 +10,7 @@
  * @author Alex Zirbel
  */
 
-#include "scoutsim/scout.h"
+#include "scout.h"
 
 #include <wx/wx.h>
 
@@ -20,12 +20,11 @@
 
 namespace scoutsim
 {
-
     Scout::Scout(const ros::NodeHandle& nh,
             const wxImage& scout_image,
             const Vector2& pos,
             float orient)
-        : nh_(nh)
+        : n_(nh)
           , scout_image_(scout_image)
           , pos_(pos)
           , orient_(orient)
@@ -37,27 +36,63 @@ namespace scoutsim
         pen_.SetWidth(3);
         scout_ = wxBitmap(scout_image_);
 
+        motors_sub_ = n_.subscribe("set_motors", 1, &Scout::setMotors, this);
+
+        /// @TODO take this out!
         velocity_sub_ =
-            nh_.subscribe("command_velocity",
+            n_.subscribe("command_velocity",
                           1,
                           &Scout::velocityCallback, this);
-        pose_pub_ = nh_.advertise<Pose>("pose", 1);
-        color_pub_ = nh_.advertise<Color>("color_sensor", 1);
-        set_pen_srv_ = nh_.advertiseService("set_pen",
+        pose_pub_ = n_.advertise<Pose>("pose", 1);
+        color_pub_ = n_.advertise<Color>("color_sensor", 1);
+        set_pen_srv_ = n_.advertiseService("set_pen",
                                             &Scout::setPenCallback,
                                             this);
         teleport_relative_srv_ =
-            nh_.advertiseService("teleport_relative",
+            n_.advertiseService("teleport_relative",
                                  &Scout::teleportRelativeCallback,
                                  this);
         teleport_absolute_srv_ =
-            nh_.advertiseService("teleport_absolute",
+            n_.advertiseService("teleport_absolute",
                                  &Scout::teleportAbsoluteCallback,
                                  this);
 
         meter_ = scout_.GetHeight();
     }
 
+    /**
+     * A callback function that sets velocity based on a set_motors
+     * request.
+     */
+    void Scout::setMotors(const motors::set_motors::ConstPtr& msg)
+    {
+        last_command_time_ = ros::WallTime::now();
+
+        if(msg->fl_set)
+        {
+            motor_fl_speed_ = msg->fl_speed;
+        }
+        if(msg->fr_set)
+        {
+            motor_fr_speed_ = msg->fr_speed;
+        }
+        if(msg->bl_set)
+        {
+            motor_bl_speed_ = msg->bl_speed;
+        }
+        if(msg->br_set)
+        {
+            motor_br_speed_ = msg->br_speed;
+        }
+
+        float l_speed = (float (motor_fl_speed_ + motor_bl_speed_)) / 2;
+        float r_speed = (float (motor_fr_speed_ + motor_br_speed_)) / 2;
+
+        /// @TODO Change this!
+        lin_vel_ = (l_speed + r_speed) / 2;
+        ang_vel_ = r_speed - l_speed;
+
+    }
 
     void Scout::velocityCallback(const VelocityConstPtr& vel)
     {
@@ -206,7 +241,7 @@ namespace scoutsim
         }
 
         ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",
-                  nh_.getNamespace().c_str(), pos_.x, pos_.y, orient_);
+                  n_.getNamespace().c_str(), pos_.x, pos_.y, orient_);
 
         if (pen_on_)
         {
diff --git a/scout/scoutsim/src/scout.h b/scout/scoutsim/src/scout.h
new file mode 100644
index 0000000..40d2dbc
--- /dev/null
+++ b/scout/scoutsim/src/scout.h
@@ -0,0 +1,134 @@
+/*
+ * This package was developed by the CMU Robotics Club project using code
+ * from Willow Garage, Inc.  Please see licensing.txt for details.
+ *
+ * All rights reserved.
+ *
+ * @file scout.h
+ * @author Colony Project, CMU Robotics Club
+ * @author Alex Zirbel
+ */
+
+#ifndef SCOUTSIM_SCOUT_H
+#define SCOUTSIM_SCOUT_H
+
+#include <ros/ros.h>
+#include <boost/shared_ptr.hpp>
+
+#include <motors/set_motors.h>
+
+#include <scoutsim/Pose.h>
+#include <scoutsim/Velocity.h>
+#include <scoutsim/SetPen.h>
+#include <scoutsim/TeleportRelative.h>
+#include <scoutsim/TeleportAbsolute.h>
+#include <scoutsim/Color.h>
+
+#include <wx/wx.h>
+
+#define PI 3.14159265
+
+namespace scoutsim
+{
+    struct Vector2
+    {
+        Vector2()
+            : x(0.0)
+              , y(0.0)
+        {}
+
+        Vector2(float _x, float _y)
+            : x(_x)
+              , y(_y)
+        {}
+
+        bool operator==(const Vector2& rhs)
+        {
+            return x == rhs.x && y == rhs.y;
+        }
+
+        bool operator!=(const Vector2& rhs)
+        {
+            return x != rhs.x || y != rhs.y;
+        }
+
+        float x;
+        float y;
+    };
+
+    class Scout
+    {
+        public:
+            Scout(const ros::NodeHandle& nh, const wxImage& scout_image,
+                  const Vector2& pos, float orient);
+
+            void update(double dt, wxMemoryDC& path_dc,
+                        const wxImage& path_image, wxColour background_color,
+                        float canvas_width, float canvas_height);
+            void paint(wxDC& dc);
+
+        private:
+            void setMotors(const motors::set_motors::ConstPtr& msg);
+            void velocityCallback(const VelocityConstPtr& vel);
+            bool setPenCallback(scoutsim::SetPen::Request&,
+                                scoutsim::SetPen::Response&);
+            bool teleportRelativeCallback(scoutsim::TeleportRelative::Request&,
+                                          scoutsim::TeleportRelative::Response&);
+            bool teleportAbsoluteCallback(scoutsim::TeleportAbsolute::Request&,
+                                          scoutsim::TeleportAbsolute::Response&);
+
+            ros::NodeHandle n_;
+
+            wxImage scout_image_;
+            wxBitmap scout_;
+
+            Vector2 pos_;
+            float orient_;
+
+            // Keep track of the last commanded speeds sent to the sim
+            int motor_fl_speed_;
+            int motor_fr_speed_;
+            int motor_bl_speed_;
+            int motor_br_speed_;
+
+            float lin_vel_;
+            float ang_vel_;
+            bool pen_on_;
+            wxPen pen_;
+
+            ros::Subscriber motors_sub_;
+            ros::Subscriber velocity_sub_;
+            ros::Publisher pose_pub_;
+            ros::Publisher color_pub_;
+            ros::ServiceServer set_pen_srv_;
+            ros::ServiceServer teleport_relative_srv_;
+            ros::ServiceServer teleport_absolute_srv_;
+
+            ros::WallTime last_command_time_;
+
+            float meter_;
+
+            struct TeleportRequest
+            {
+                TeleportRequest(float x, float y, float _theta,
+                                float _linear, bool _relative)
+                    : pos(x, y)
+                      , theta(_theta)
+                      , linear(_linear)
+                      , relative(_relative)
+                {}
+
+                Vector2 pos;
+                float theta;
+                float linear;
+                bool relative;
+            };
+
+            typedef std::vector<TeleportRequest> V_TeleportRequest;
+            V_TeleportRequest teleport_requests_;
+    };
+    typedef boost::shared_ptr<Scout> ScoutPtr;
+
+}
+
+#endif
diff --git a/scout/scoutsim/src/scoutsim.cpp b/scout/scoutsim/src/scoutsim.cpp
index 6e13413..6066137 100644
--- a/scout/scoutsim/src/scoutsim.cpp
+++ b/scout/scoutsim/src/scoutsim.cpp
@@ -17,7 +17,7 @@
 
 #include <boost/thread.hpp>
 
-#include "scoutsim/sim_frame.h"
+#include "sim_frame.h"
 
 #ifdef __WXMAC__
 #include <ApplicationServices/ApplicationServices.h>
diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index 75c30c6..94f8755 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -10,7 +10,7 @@
  * @author Alex Zirbel
  */
 
-#include "scoutsim/sim_frame.h"
+#include "sim_frame.h"
 
 #include <ros/package.h>
 #include <cstdlib>
@@ -46,7 +46,6 @@ namespace scoutsim
         std::string scouts[SCOUTSIM_NUM_SCOUTS] = 
         {
             "scout.png"
-            "scout_forreal.png"
         };
 
         std::string images_path = ros::package::getPath("scoutsim")+"/images/";
diff --git a/scout/scoutsim/src/sim_frame.h b/scout/scoutsim/src/sim_frame.h
new file mode 100644
index 0000000..71454ed
--- /dev/null
+++ b/scout/scoutsim/src/sim_frame.h
@@ -0,0 +1,77 @@
+/*
+ * This package was developed by the CMU Robotics Club project using code
+ * from Willow Garage, Inc.  Please see licensing.txt for details.
+ *
+ * All rights reserved.
+ *
+ * @file sim_frame.h
+ * @author Colony Project, CMU Robotics Club
+ * @author Alex Zirbel
+ */
+
+#include <wx/wx.h>
+#include <wx/event.h>
+#include <wx/timer.h>
+
+#include <ros/ros.h>
+
+#include <std_srvs/Empty.h>
+#include <scoutsim/Spawn.h>
+#include <scoutsim/Kill.h>
+#include <map>
+
+#include "scout.h"
+
+#define SCOUTSIM_NUM_SCOUTS 1
+
+namespace scoutsim
+{
+    class SimFrame : public wxFrame
+    {
+        public:
+            SimFrame(wxWindow* parent);
+            ~SimFrame();
+
+            std::string spawnScout(const std::string& name,
+                    float x, float y, float angle);
+
+        private:
+            void onUpdate(wxTimerEvent& evt);
+            void onPaint(wxPaintEvent& evt);
+
+            void updateScouts();
+            void clear();
+            bool hasScout(const std::string& name);
+
+            bool clearCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+            bool resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+            bool spawnCallback(scoutsim::Spawn::Request&, scoutsim::Spawn::Response&);
+            bool killCallback(scoutsim::Kill::Request&, scoutsim::Kill::Response&);
+
+            ros::NodeHandle nh_;
+            wxTimer* update_timer_;
+            wxBitmap path_bitmap_;
+            wxImage path_image_;
+            wxMemoryDC path_dc_;
+
+            uint64_t frame_count_;
+
+            ros::WallTime last_scout_update_;
+
+            ros::ServiceServer clear_srv_;
+            ros::ServiceServer reset_srv_;
+            ros::ServiceServer spawn_srv_;
+            ros::ServiceServer kill_srv_;
+
+            typedef std::map<std::string, ScoutPtr> M_Scout;
+            M_Scout scouts_;
+            uint32_t id_counter_;
+
+            wxImage scout_images_[SCOUTSIM_NUM_SCOUTS];
+
+            float meter_;
+            float width_in_meters_;
+            float height_in_meters_;
+    };
+
+}
