Fixes #185 Add SIM_wheelbot

2025-04-07 11:26:47 +00:00 · 2016-02-18 16:14:44 -06:00 · 2016-02-18 16:14:44 -06:00 · da08efe546
commit da08efe546
parent 39868aba46
78 changed files with 3724 additions and 0 deletions
--- a/trick_models/Wheelbot/Battery/.gitignore
+++ b/trick_models/Wheelbot/Battery/.gitignore
@ -0,0 +1,6 @@
+# Ignore compiled objects and libraries
+lib
+obj
+*.o
+test/BatteryTest
+XMLtestReports
--- a/trick_models/Wheelbot/Battery/include/DCBattery.hh
+++ b/trick_models/Wheelbot/Battery/include/DCBattery.hh
@ -0,0 +1,20 @@
+/******************************************
+ PURPOSE:	(DCBattery)
+ *******************************************/
+#ifndef DCBattery_H
+#define DCBattery_H
+
+class DCBattery
+{
+public:
+    DCBattery(double initialIdealVoltage, double initialInternalResistance);
+    void update ();
+    double getActualVoltage();
+    void setCurrent(double current);
+private:
+    double idealVoltage;
+    double internalResistance;
+    double actualVoltage;
+    double current;
+};
+#endif
--- a/trick_models/Wheelbot/Battery/makefile
+++ b/trick_models/Wheelbot/Battery/makefile
@ -0,0 +1,32 @@
+RM = rm -rf
+CPP = g++
+
+#CXXFLAGS += -DDIAGONAL_NEIGHBORS
+
+INCLUDE_DIRS = -Iinclude
+OBJDIR = obj
+LIBDIR = lib
+LIBNAME = libBattery.a
+LIBOBJS = $(OBJDIR)/DCBattery.o
+
+all: ${LIBDIR}/${LIBNAME}
+
+$(LIBOBJS): $(OBJDIR)/%.o : src/%.cpp | $(OBJDIR)
+	$(CPP) $(CXXFLAGS) ${INCLUDE_DIRS} -c $< -o $@
+
+${LIBDIR}/${LIBNAME}: ${LIBOBJS} | ${LIBDIR}
+	ar crs $@ ${LIBOBJS}
+
+${OBJDIR}:
+	mkdir -p ${OBJDIR}
+
+${LIBDIR}:
+	mkdir -p ${LIBDIR}
+
+clean:
+	${RM} *~
+	${RM} ${OBJDIR}
+
+spotless: clean
+	${RM} ${LIBDIR}/${LIBNAME}
+
--- a/trick_models/Wheelbot/Battery/src/DCBattery.cpp
+++ b/trick_models/Wheelbot/Battery/src/DCBattery.cpp
@ -0,0 +1,23 @@
+#include "DCBattery.hh"
+#include <iostream>
+
+
+DCBattery::DCBattery(double initialIdealVoltage, double initialInternalResistance)
+: actualVoltage(0), current(0), idealVoltage(initialIdealVoltage),
+  internalResistance(initialInternalResistance)  {
+}
+
+void DCBattery :: update ()
+{
+    actualVoltage = idealVoltage - internalResistance * current;
+    actualVoltage = std :: min (idealVoltage, actualVoltage);
+    actualVoltage = std :: max (0.0, actualVoltage);      
+}
+double DCBattery :: getActualVoltage ()
+{
+    return actualVoltage;
+}
+void DCBattery :: setCurrent (double value)
+{
+    current = value;
+}
--- a/trick_models/Wheelbot/Battery/test/DCBatteryTest.cpp
+++ b/trick_models/Wheelbot/Battery/test/DCBatteryTest.cpp
@ -0,0 +1,52 @@
+#include <gtest/gtest.h>
+#define private public
+#include "DCBattery.hh"
+
+TEST( BatteryTest , one ) {
+    // Attempt to create an Battery
+    DCBattery * battery;
+    battery = new DCBattery(9.0, 2.0);
+    EXPECT_NE( (void*)0, battery);
+    //delete battery;
+}
+
+TEST( BatteryTest , two ) {
+    // Test Ideal Voltage
+    DCBattery battery(9.0, 2.0);
+    EXPECT_DOUBLE_EQ( 9.0, battery.idealVoltage );
+
+}
+
+TEST( BatteryTest , three ) {
+    // Test Internal Resistance
+    DCBattery battery(9.0, 2.0);
+    EXPECT_DOUBLE_EQ( 2, battery.internalResistance );
+
+}
+
+TEST( BatteryTest , four ) {
+    // Test Actual Voltage doesnt go below zero
+    DCBattery battery(9.0, 2.0);
+    battery.setCurrent(6.0);
+    battery.update ();
+    EXPECT_DOUBLE_EQ( 0.0, battery.actualVoltage );
+    
+}
+
+TEST( BatteryTest , five ) {
+    // Test Actual Voltage under normal load
+    DCBattery battery(9.0, 2.0);
+    battery.setCurrent(4.0);
+    battery.update ();
+    EXPECT_DOUBLE_EQ( 1.0, battery.actualVoltage );
+    
+}
+
+TEST( BatteryTest , six ) {
+    // Test Actual Voltage doesnt go above Ideal Voltage
+    DCBattery battery(9.0, 2.0);
+    battery.setCurrent(-4.0);
+    battery.update ();
+    EXPECT_DOUBLE_EQ( 9.0, battery.actualVoltage );
+    
+}
--- a/trick_models/Wheelbot/Battery/test/makefile
+++ b/trick_models/Wheelbot/Battery/test/makefile
@ -0,0 +1,42 @@
+
+
+RM = rm -rf
+CC = cc
+CPP = c++
+
+DECL_DIR = ..
+GTEST_DIR = ${HOME}/gtest-1.7.0
+
+CFLAGS += -g -Wall -Wextra -I$(GTEST_DIR)/include -I$(DECL_DIR)/include
+
+LIBS = ../lib/libBattery.a -lpthread
+
+TESTS = DCBatteryTest
+
+GTEST_HEADERS = $(GTEST_DIR)/include/gtest/*.h \
+                $(GTEST_DIR)/include/gtest/internal/*.h
+
+all : $(TESTS)
+
+test: $(TESTS)
+
+	./DCBatteryTest              --gtest_output=xml:XMLtestReports/DCBatteryTestResults.xml
+
+clean :
+	rm -f $(TESTS) gtest.a gtest_main.a
+	rm -f *.o
+	rm -f *.cpp~
+	rm -f *.hh~
+	rm -rf XMLtestReports
+
+gtest-all.o :
+	$(CPP) -I$(GTEST_DIR) $(CFLAGS) -c $(GTEST_DIR)/src/gtest-all.cc
+
+gtest_main.o :
+	$(CPP) -I$(GTEST_DIR) $(CFLAGS) -c $(GTEST_DIR)/src/gtest_main.cc
+
+DCBatteryTest.o : DCBatteryTest.cpp
+	$(CPP) $(CFLAGS) -c $<
+
+DCBatteryTest : DCBatteryTest.o gtest_main.o gtest-all.o
+	$(CPP) $(CFLAGS) -o $@ $^ $(LIBS)
--- a/trick_models/Wheelbot/Control/include/differentialDriveController.hh
+++ b/trick_models/Wheelbot/Control/include/differentialDriveController.hh
@ -0,0 +1,41 @@
+#ifndef DIFFERENTIAL_DRIVE_CONTROLER_HH
+#define DIFFERENTIAL_DRIVE_CONTROLER_HH
+
+#include "Motor/include/motorSpeedController.hh"
+
+class DifferentialDriveController {
+
+    public:
+        DifferentialDriveController( double distanceBetweenWheels,
+                                     double wheelRadius,
+                                     double wheelSpeedLimit,
+                                     double headingRateLimit,
+                                     double slowDownDistance,
+                                     MotorSpeedController& rightMotorController,
+                                     MotorSpeedController& leftMotorController
+                                   );
+
+        int update( double distance_err,
+                    double heading_err);
+        void stop();
+
+    private:
+
+        // Do not allow the default constructor to be used.
+        DifferentialDriveController();
+
+        double distanceBetweenWheels;
+        double wheelRadius;
+        double wheelSpeedLimit;
+        double headingRateLimit;
+        double slowDownDistance;
+
+        MotorSpeedController& rightMotorController;
+        MotorSpeedController& leftMotorController;
+
+        double rightMotorSpeedCommand;
+        double leftMotorSpeedCommand;
+        double desiredHeadingRate;
+        double desiredRangeRate;
+};
+#endif
--- a/trick_models/Wheelbot/Control/include/testMotorController.hh
+++ b/trick_models/Wheelbot/Control/include/testMotorController.hh
@ -0,0 +1,19 @@
+#ifndef TEST_MOTOR_CONTROLLER_HH
+#define TEST_MOTOR_CONTROLLER_HH
+#include "Motor/include/motorSpeedController.hh"
+
+class TestMotorController : public MotorSpeedController {
+
+    // This class is for testing.
+    // It plays the roll of a Controller and a Motor.
+    public:
+        TestMotorController(): motorSpeed(0.0) {}
+
+        void setCommandedSpeed( double speed_command) {
+            commandedSpeed = speed_command;
+        }
+
+    private:
+        double commandedSpeed; // rad/s
+};
+#endif
--- a/trick_models/Wheelbot/Control/include/vehicleController.hh
+++ b/trick_models/Wheelbot/Control/include/vehicleController.hh
@ -0,0 +1,37 @@
+#ifndef VEHICLE_CONTROLLER_HH
+#define VEHICLE_CONTROLLER_HH
+
+#include <vector>
+#include "Guidance/include/point.hh"
+#include "Guidance/include/navigator.hh"
+#include "Control/include/differentialDriveController.hh"
+
+#ifndef PI
+#define PI 3.141592653589793
+#endif
+
+class VehicleController {
+    public:
+    VehicleController(std::vector<Point>* waypointQueue,
+                      Navigator& navigator,
+                      DifferentialDriveController& driveController,
+                      double arrival_distance);
+
+    int getCurrentDestination(Point& currentDestination);
+    void setWayPointQueue( std::vector<Point>* waypointQueue );
+    void printDestination();
+    void update();
+
+    private:
+    // Do not allow the default constructor to be used.
+    VehicleController();
+
+    std::vector<Point>* waypointQueue;
+    std::vector<Point>::iterator destination;
+    Point departure;
+    Navigator& navigator;
+    DifferentialDriveController& driveController;
+
+    double arrivalDistance;
+};
+#endif
--- a/trick_models/Wheelbot/Control/makefile
+++ b/trick_models/Wheelbot/Control/makefile
@ -0,0 +1,36 @@
+RM = rm -rf
+CPP = g++
+
+PROJECT_DIR = ..
+CXXFLAGS += -g
+
+INCLUDE_DIRS += -I$(PROJECT_DIR)
+
+OBJDIR = obj
+LIBDIR = lib
+LIBNAME = libControl.a
+LIBOBJS = $(OBJDIR)/differentialDriveController.o \
+          $(OBJDIR)/DCMotorSpeedController.o \
+	  $(OBJDIR)/vehicleController.o
+
+all: ${LIBDIR}/${LIBNAME}
+
+$(LIBOBJS): $(OBJDIR)/%.o : src/%.cpp | $(OBJDIR)
+	$(CPP) $(CXXFLAGS) ${INCLUDE_DIRS} -c $< -o $@
+
+${LIBDIR}/${LIBNAME}: ${LIBOBJS} | ${LIBDIR}
+	ar crs $@ ${LIBOBJS}
+
+${OBJDIR}:
+	mkdir -p ${OBJDIR}
+
+${LIBDIR}:
+	mkdir -p ${LIBDIR}
+
+clean:
+	${RM} *~
+	${RM} ${OBJDIR}
+
+spotless: clean
+	${RM} ${LIBDIR}/${LIBNAME}
+
--- a/trick_models/Wheelbot/Control/src/differentialDriveController.cpp
+++ b/trick_models/Wheelbot/Control/src/differentialDriveController.cpp
@ -0,0 +1,75 @@
+#include <iostream>
+#include <math.h>
+#include "Control/include/differentialDriveController.hh"
+#ifndef PI
+#define PI 3.14159265
+#endif
+
+DifferentialDriveController::
+    DifferentialDriveController ( double DistanceBetweenWheels,
+                                  double WheelRadius,
+                                  double WheelSpeedLimit,
+                                  double HeadingRateLimit,
+                                  double SlowDownDistance,
+                                  MotorSpeedController& RightMotorController,
+                                  MotorSpeedController& LeftMotorController
+                                ):
+    distanceBetweenWheels(DistanceBetweenWheels),
+    wheelRadius(WheelRadius),
+    wheelSpeedLimit(WheelSpeedLimit),
+    headingRateLimit(HeadingRateLimit),
+    slowDownDistance(SlowDownDistance),
+    rightMotorController(RightMotorController),
+    leftMotorController(LeftMotorController),
+
+    rightMotorSpeedCommand(0.0),
+    leftMotorSpeedCommand(0.0)
+{ }
+
+void DifferentialDriveController::stop() {
+    rightMotorController.setCommandedSpeed(0.0);
+    leftMotorController.setCommandedSpeed(0.0);
+}
+
+int DifferentialDriveController::update( double distance_err,  // m
+                                         double heading_err) { // rad (-PI..+PI)
+
+    rightMotorSpeedCommand = 0.0;
+    leftMotorSpeedCommand  = 0.0;
+
+ //   double K = (2.0 * wheelRadius / distanceBetweenWheels);
+
+    if ( cos(heading_err) > 0.999 ) {
+        desiredHeadingRate = 0.0;
+    } else if (heading_err > 0.0 ) {
+        desiredHeadingRate =  headingRateLimit;
+    } else {
+        desiredHeadingRate = -headingRateLimit;
+    }
+
+//    double wheelSpeedForHeadingRate = desiredHeadingRate / K;
+    double wheelSpeedForHeadingRate = (desiredHeadingRate * distanceBetweenWheels) / (2.0 * wheelRadius);
+
+    double availableWheelSpeedForRangeRate = wheelSpeedLimit - fabs(wheelSpeedForHeadingRate);
+
+    double wheelSpeedForRangeRate;
+
+    if (distance_err > slowDownDistance ) {
+        wheelSpeedForRangeRate = availableWheelSpeedForRangeRate;
+    } else {
+        wheelSpeedForRangeRate = (distance_err/slowDownDistance) * availableWheelSpeedForRangeRate;
+    }
+
+    desiredRangeRate = wheelSpeedForRangeRate * wheelRadius;
+
+    double desiredRightWheelSpeed =  wheelSpeedForRangeRate + wheelSpeedForHeadingRate;
+    double desiredLeftWheelSpeed  =  wheelSpeedForRangeRate - wheelSpeedForHeadingRate;
+
+    rightMotorSpeedCommand = -desiredRightWheelSpeed;
+    leftMotorSpeedCommand  =  desiredLeftWheelSpeed;
+
+    rightMotorController.setCommandedSpeed( rightMotorSpeedCommand);
+    leftMotorController.setCommandedSpeed( leftMotorSpeedCommand);
+
+    return 0;
+}
--- a/trick_models/Wheelbot/Control/src/vehicleController.cpp
+++ b/trick_models/Wheelbot/Control/src/vehicleController.cpp
@ -0,0 +1,62 @@
+#include "Control/include/vehicleController.hh"
+#include <iostream>
+
+VehicleController::VehicleController( std::vector<Point>* wayPoints,
+                                      Navigator& theNavigator,
+                                      DifferentialDriveController& theDriveController,
+                                      double arrival_distance):
+   navigator(theNavigator),
+   driveController(theDriveController) {
+
+    // Enforce minimum arrival distance.
+    if (arrival_distance > 0.01) {
+        arrivalDistance = arrival_distance;
+    } else {
+        arrivalDistance = 0.01;
+    }
+    waypointQueue = wayPoints;
+    destination = waypointQueue->begin();
+    printDestination();
+}
+
+void VehicleController::setWayPointQueue( std::vector<Point>* wayPoints ) {
+    waypointQueue = wayPoints;
+    destination = waypointQueue->begin();
+    printDestination();
+}
+
+int VehicleController::getCurrentDestination(Point& currentDestination) {
+    if (destination != waypointQueue->end()) {
+        currentDestination = *destination;
+        return 0;
+    }
+    return 1;
+}
+
+void VehicleController::printDestination() {
+    if (destination != waypointQueue->end()) {
+        std::cout << "Destination = (" << destination->x << "," << destination->y << ")." << std::endl;
+    } else {
+        std::cout << "No Destination." << std::endl;
+    }
+}
+
+void VehicleController::update() {
+
+    if (destination == waypointQueue->end()) {
+        driveController.update(0.0, 0.0);
+        driveController.stop();
+    } else {
+        double distance_err = navigator.distanceTo(*destination);
+        if ( distance_err > arrivalDistance) {
+            double heading_err = navigator.bearingTo(*destination);
+            driveController.update(distance_err, heading_err);
+        } else {
+            std::cout << "Arrived at Destination." << std::endl;
+            destination ++;
+            printDestination();
+        }
+    }
+}
+
+
--- a/trick_models/Wheelbot/Control/test/DifferentialDriveControllerTest.cpp
+++ b/trick_models/Wheelbot/Control/test/DifferentialDriveControllerTest.cpp
@ -0,0 +1,172 @@
+#include <gtest/gtest.h>
+#define private public
+#include "Control/include/testMotorController.hh"
+#include "Control/include/differentialDriveController.hh"
+#include <algorithm>
+
+#ifndef PI
+#define PI 3.1415926535
+#endif
+#define FLOAT_TOLERANCE 0.000001
+
+/*
+    Test Fixture.
+*/
+class differentialDriveControllerTest : public ::testing::Test {
+    protected:
+
+    TestMotorController* rightMotorController;
+    TestMotorController* leftMotorController;
+    DifferentialDriveController* driveController;
+
+    differentialDriveControllerTest() {
+    rightMotorController =
+        new TestMotorController();
+    leftMotorController =
+        new TestMotorController();
+    driveController =
+        new DifferentialDriveController(0.183, 0.045, 8.880, 0.200,
+                *rightMotorController, *leftMotorController);
+    }
+
+    ~differentialDriveControllerTest() {
+        delete driveController;
+        delete rightMotorController;
+        delete leftMotorController;
+    }
+};
+
+TEST_F( differentialDriveControllerTest , constructor) {
+
+    EXPECT_NEAR(0.045, driveController->wheelRadius, FLOAT_TOLERANCE);
+    EXPECT_NEAR(0.183, driveController->distanceBetweenWheels, FLOAT_TOLERANCE);
+    EXPECT_NEAR(8.880, driveController->wheelRotationRateLimit, FLOAT_TOLERANCE);
+    EXPECT_NEAR(0.200, driveController->slowDownDistance, FLOAT_TOLERANCE);
+}
+
+TEST_F( differentialDriveControllerTest , setDistanceBetweenWheels ) {
+
+    int result;
+
+    result = driveController->setDistanceBetweenWheels(0.1);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(0.1, driveController->distanceBetweenWheels, FLOAT_TOLERANCE);
+
+    result = driveController->setDistanceBetweenWheels(0.2);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(0.2, driveController->distanceBetweenWheels, FLOAT_TOLERANCE);
+
+    result = driveController->setDistanceBetweenWheels(-0.3);
+    EXPECT_EQ(1,result);
+    EXPECT_NEAR(0.2, driveController->distanceBetweenWheels, FLOAT_TOLERANCE);
+}
+
+TEST_F( differentialDriveControllerTest , setWheelRadius ) {
+
+    int result;
+    result = driveController->setWheelRadius(0.059);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(0.059, driveController->wheelRadius, FLOAT_TOLERANCE);
+
+    result = driveController->setWheelRadius(0.083);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(0.083, driveController->wheelRadius, FLOAT_TOLERANCE);
+
+    result = driveController->setWheelRadius(-0.075);
+    EXPECT_EQ(1,result);
+    EXPECT_NEAR(0.083, driveController->wheelRadius, FLOAT_TOLERANCE);
+}
+
+TEST_F( differentialDriveControllerTest , setWheelRotationRateLimit ) {
+
+    int result;
+    result = driveController->setWheelRotationRateLimit(7.123);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(7.123, driveController->wheelRotationRateLimit, FLOAT_TOLERANCE);
+
+    result = driveController->setWheelRotationRateLimit(5.234);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(5.234, driveController->wheelRotationRateLimit, FLOAT_TOLERANCE);
+
+    result = driveController->setWheelRotationRateLimit(-4.987);
+    EXPECT_EQ(1,result);
+    EXPECT_NEAR(5.234, driveController->wheelRotationRateLimit, FLOAT_TOLERANCE);
+}
+
+TEST_F( differentialDriveControllerTest , PositiveRangeErrorOnly) {
+
+    // If there is no heading error, and the distance error is non-zero,
+    // then the speeds should be the same, and equal to the wheelRotationRateLimit.
+
+    double distance_err = 1.0;
+    double heading_err  = 0.0;
+    driveController->update(distance_err, heading_err);
+
+    double rightMotorSpeedCommand, leftMotorSpeedCommand;
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    // the speeds should be the same
+    EXPECT_NEAR(rightMotorSpeedCommand, leftMotorSpeedCommand, FLOAT_TOLERANCE);
+    // and equal to the setWheelRotationRateLimit.
+    EXPECT_NEAR(rightMotorSpeedCommand, 8.880, FLOAT_TOLERANCE);
+}
+
+TEST_F( differentialDriveControllerTest , positiveHeadingError ) {
+
+    // If the heading error is positive, then we should turn to the right,
+    // meaning that the left wheel should move faster than the right wheel.
+
+    double rightMotorSpeedCommand, leftMotorSpeedCommand;
+
+    driveController->update(0.0, 0.1*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(50.0, 30*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(100.0, 60*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(0.0, 89*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(0.0, 90*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(50.0, 91*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(100.0, 120*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(0.0, 150*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(50.0, 179*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+}
+
+TEST_F( differentialDriveControllerTest, negativeHeadingError ) {
+
+    // If the heading error is negative, then we should turn to the left,
+    // meaning that the right wheel should move faster than the left wheel.
+
+    double distance_err = 0.0;
+    double heading_err  = -30*(PI/180.0);
+    driveController->update(distance_err, heading_err);
+
+    double rightMotorSpeedCommand, leftMotorSpeedCommand;
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(rightMotorSpeedCommand, leftMotorSpeedCommand);
+}
+
+
--- a/trick_models/Wheelbot/Control/test/VehicleControllerTest.cpp
+++ b/trick_models/Wheelbot/Control/test/VehicleControllerTest.cpp
@ -0,0 +1,58 @@
+#include <gtest/gtest.h>
+#define private public
+#include "Control/include/testMotorController.hh"
+#include "Control/include/vehicleController.hh"
+#include <algorithm>
+
+#ifndef PI
+#define PI 3.1415926535
+#endif
+#define FLOAT_TOLERANCE 0.000001
+
+/*
+Test Fixture.
+*/
+class vehicleControllerTest : public ::testing::Test {
+    protected:
+    std::vector<Point> waypointQueue;
+    Navigator *navigator;
+    TestMotorController* rightMotorController;
+    TestMotorController* leftMotorController;
+    DifferentialDriveController* driveController;
+    VehicleController* vehicleController;
+
+    vehicleControllerTest() {
+
+        Point waypoint(1.0, 3.0);
+        waypointQueue.push_back( waypoint);
+
+        Point initLocation(0.0, 0.0);
+        rightMotorController = new TestMotorController();
+        leftMotorController  = new TestMotorController();
+        driveController = new DifferentialDriveController(0.183, 0.045, 8.880, 0.200,
+                                                          *rightMotorController,
+                                                          *leftMotorController);
+        navigator = new Navigator(0.0, initLocation);
+        vehicleController = new VehicleController( &waypointQueue, *navigator, *driveController, 0.02);
+
+    }
+
+    ~vehicleControllerTest() {
+        delete navigator;
+        delete driveController;
+    }
+
+    void SetUp() {}
+    void TearDown() {}
+};
+
+
+TEST_F( vehicleControllerTest , one ) {
+
+    Point current_destination;
+    int result = vehicleController->getCurrentDestination(current_destination);
+    EXPECT_EQ(result, 0);
+    EXPECT_EQ(current_destination.x, 1);
+    EXPECT_EQ(current_destination.y, 3);
+}
+
--- a/trick_models/Wheelbot/Control/test/makefile
+++ b/trick_models/Wheelbot/Control/test/makefile
@ -0,0 +1,52 @@
+RM = rm -rf
+CC = cc
+CPP = c++
+
+PROJECT_DIR = ../..
+
+GTEST_DIR = ${HOME}/gtest-1.7.0
+
+CFLAGS += -g -Wall -Wextra
+
+INCLUDE_DIRS += -I$(PROJECT_DIR)\
+	        -I$(GTEST_DIR)/include
+
+LIBS = $(PROJECT_DIR)/Control/lib/libControl.a \
+       $(PROJECT_DIR)/Guidance/lib/libGuidance.a \
+       -lpthread
+
+TESTS = DifferentialDriveControllerTest VehicleControllerTest
+
+GTEST_HEADERS = $(GTEST_DIR)/include/gtest/*.h \
+                $(GTEST_DIR)/include/gtest/internal/*.h
+
+all : $(TESTS)
+
+test: $(TESTS)
+	./DifferentialDriveControllerTest --gtest_output=xml:XMLtestReports/DifferentialDriveControllerTestResults.xml
+	./VehicleControllerTest --gtest_output=xml:XMLtestReports/VehicleControllerTestResults.xml
+
+clean :
+	rm -f $(TESTS) gtest.a gtest_main.a
+	rm -f *.o
+	rm -f *.cpp~
+	rm -f *.hh~
+	rm -rf XMLtestReports
+
+gtest-all.o :
+	$(CPP) -I$(GTEST_DIR) -I$(GTEST_DIR)/include $(CFLAGS) -c $(GTEST_DIR)/src/gtest-all.cc
+
+gtest_main.o :
+	$(CPP) -I$(GTEST_DIR) -I$(GTEST_DIR)/include $(CFLAGS) -c $(GTEST_DIR)/src/gtest_main.cc
+
+DifferentialDriveControllerTest.o : DifferentialDriveControllerTest.cpp
+	$(CPP) $(CFLAGS) $(INCLUDE_DIRS) -c $<
+
+DifferentialDriveControllerTest : DifferentialDriveControllerTest.o gtest_main.o gtest-all.o
+	$(CPP) $(CFLAGS) $(INCLUDE_DIRS) -o $@ $^ $(LIBS)
+
+VehicleControllerTest.o : VehicleControllerTest.cpp
+	$(CPP) $(CFLAGS) $(INCLUDE_DIRS) -c $<
+
+VehicleControllerTest : VehicleControllerTest.o gtest_main.o gtest-all.o
+	$(CPP) $(CFLAGS) $(INCLUDE_DIRS) -o $@ $^ $(LIBS)
--- a/trick_models/Wheelbot/Electrical/include/ElectricalCircuit.hh
+++ b/trick_models/Wheelbot/Electrical/include/ElectricalCircuit.hh
@ -0,0 +1,24 @@
+/**************************************************************
+PURPOSE:              (Electrical Model)
+***************************************************************/
+#ifndef ELECTRICAL_CIRCUIT_H
+#define ELECTRICAL_CIRCUIT_H
+
+class DCBattery;
+class DCMotor;
+
+class ElectricalCircuit
+{
+public:
+    DCMotor*        motor1;
+    DCMotor*        motor2;
+    DCBattery*     battery;
+    ElectricalCircuit();
+    void init (DCMotor* motor1, DCMotor* motor2, DCBattery* battery);
+    void update();
+private:
+    double motorsCurrent;
+
+};
+
+#endif
--- a/trick_models/Wheelbot/Electrical/makefile
+++ b/trick_models/Wheelbot/Electrical/makefile
@ -0,0 +1,31 @@
+RM = rm -rf
+CPP = g++
+
+#CXXFLAGS += -DDIAGONAL_NEIGHBORS
+
+INCLUDE_DIRS = -Iinclude
+OBJDIR = obj
+LIBDIR = lib
+LIBNAME = libElectrical.a
+LIBOBJS = $(OBJDIR)/ElectricalCircuit.o
+
+all: ${LIBDIR}/${LIBNAME}
+
+$(LIBOBJS): $(OBJDIR)/%.o : src/%.cpp | $(OBJDIR)
+	$(CPP) $(CXXFLAGS) ${INCLUDE_DIRS} -c $< -o $@
+
+${LIBDIR}/${LIBNAME}: ${LIBOBJS} | ${LIBDIR}
+	ar crs $@ ${LIBOBJS}
+
+${OBJDIR}:
+	mkdir -p ${OBJDIR}
+
+${LIBDIR}:
+	mkdir -p ${LIBDIR}
+
+clean:
+	${RM} *~
+	${RM} ${OBJDIR}
+
+spotless: clean
+	${RM} ${LIBDIR}/${LIBNAME}
--- a/trick_models/Wheelbot/Electrical/src/ElectricalCircuit.cpp
+++ b/trick_models/Wheelbot/Electrical/src/ElectricalCircuit.cpp
@ -0,0 +1,23 @@
+#include "ElectricalCircuit.hh"
+#include <iostream>
+#include "../../Motor/include/DCMotor.hh"
+#include "../../Battery/include/DCBattery.hh"
+
+ElectricalCircuit::ElectricalCircuit ()
+: motorsCurrent(0), motor1(0), motor2(0), battery(0) {
+
+}
+
+void ElectricalCircuit :: init (DCMotor* motorOne, DCMotor* motorTwo, DCBattery* battery1)
+{
+    motor1 = motorOne;
+    motor2 = motorTwo;
+    battery = battery1;
+}
+
+void ElectricalCircuit :: update ()
+{
+    motorsCurrent = motor1 -> getCurrentLoad() + motor2 -> getCurrentLoad();
+    battery->setCurrent(motorsCurrent);
+
+}
--- a/trick_models/Wheelbot/Electrical/test/ElectricalCircuitTest.cpp
+++ b/trick_models/Wheelbot/Electrical/test/ElectricalCircuitTest.cpp
--- a/trick_models/Wheelbot/Electrical/test/makefile
+++ b/trick_models/Wheelbot/Electrical/test/makefile
@ -0,0 +1,41 @@
+RM = rm -rf
+CC = cc
+CPP = c++
+
+DECL_DIR = ..
+GTEST_DIR = ${HOME}/gtest-1.7.0
+
+CFLAGS += -g -Wall -Wextra -I$(GTEST_DIR)/include -I$(DECL_DIR)/include
+
+LIBS = ../lib/libElectrical.a     -lpthread
+
+TESTS = ElectricalCircuitTest
+
+GTEST_HEADERS = $(GTEST_DIR)/include/gtest/*.h \
+                $(GTEST_DIR)/include/gtest/internal/*.h
+
+all : $(TESTS)
+
+test: $(TESTS)
+
+	./ElectricalCircuitTest              --gtest_output=xml:XMLtestReports/ElectricalCircuitTestResults.xml
+    
+clean :
+	rm -f $(TESTS) gtest.a gtest_main.a
+	rm -f *.o
+	rm -f *.cpp~
+	rm -f *.hh~
+	rm -rf XMLtestReports
+
+gtest-all.o :
+	$(CPP) -I$(GTEST_DIR) $(CFLAGS) -c $(GTEST_DIR)/src/gtest-all.cc
+
+gtest_main.o :
+	$(CPP) -I$(GTEST_DIR) $(CFLAGS) -c $(GTEST_DIR)/src/gtest_main.cc
+
+ElectricalCircuitTest.o : ElectricalCircuitTest.cpp
+	$(CPP) $(CFLAGS) -c $<
+
+ElectricalCircuitTest : ElectricalCircuitTest.o gtest_main.o gtest-all.o
+	$(CPP) $(CFLAGS) -o $@ $^ $(LIBS)
+	
--- a/trick_models/Wheelbot/Graphics/Makefile
+++ b/trick_models/Wheelbot/Graphics/Makefile
@ -0,0 +1,36 @@
+SHELL = /bin/sh
+
+PROJECT_NAME = EVDisplay
+SRC_DIR = src
+BUILD_DIR = build
+CLASSES_DIR = $(BUILD_DIR)/classes
+JAR_DIR = dist
+MAIN_CLASS = trick.EVDisplay
+
+all: jar
+
+clean:
+	rm -rf $(BUILD_DIR)
+	rm -f manifest
+
+spotless: clean
+	rm -rf dist
+
+$(CLASSES_DIR):
+	@ mkdir -p $(CLASSES_DIR)
+
+compile: | $(CLASSES_DIR)
+	javac -sourcepath $(SRC_DIR) -d $(CLASSES_DIR) $(SRC_DIR)/trick/EVDisplay.java
+
+manifest:
+	@ echo "Main-Class: $(MAIN_CLASS)" > $@
+
+$(JAR_DIR):
+	@ mkdir -p $(JAR_DIR)
+
+jar: compile manifest | $(JAR_DIR)
+	jar cvfm $(JAR_DIR)/$(PROJECT_NAME).jar manifest -C $(CLASSES_DIR) .
+	@ echo "-------------------------------------------------------------------------------"
+	@ echo "                              BUILD COMPLETE"
+	@ echo "The Java jar file (the Java Executable) is located at: $(JAR_DIR)/$(PROJECT_NAME).jar"
+	@ echo "-------------------------------------------------------------------------------"
--- a/trick_models/Wheelbot/Graphics/src/trick/EVDisplay.java
+++ b/trick_models/Wheelbot/Graphics/src/trick/EVDisplay.java
@ -0,0 +1,247 @@
+/*
+ * Trick
+ * 2016 (c) National Aeronautics and Space Administration (NASA)
+ */
+
+package trick;
+
+import java.awt.Graphics2D;
+import java.awt.Graphics;
+import java.awt.Image;
+import java.awt.geom.AffineTransform;
+import java.awt.image.AffineTransformOp;
+import java.awt.image.BufferedImage;
+import java.io.BufferedOutputStream;
+import java.io.BufferedReader;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.io.InputStreamReader;
+import java.io.FileReader;
+import java.net.Socket;
+import java.util.*;
+import javax.swing.ImageIcon;
+import javax.swing.JFrame;
+import javax.swing.JPanel;
+
+/**
+ *
+ * @author penn
+ */
+
+class Feature {
+
+    public double x, y;
+    public double heading;
+    public BufferedImage bufImage;
+
+    public Feature(double X, double Y, double H, String imageFile) {
+        x = X;
+        y = Y;
+        heading = H;
+
+        Image img = new ImageIcon(imageFile).getImage();
+        bufImage = new BufferedImage(img.getWidth(null), img.getHeight(null), 
+                BufferedImage.TYPE_INT_ARGB);
+        Graphics2D gContext = bufImage.createGraphics();
+        gContext.drawImage(img,0,0,null);
+        gContext.dispose();
+    }
+
+    public void setState(double X, double Y, double H) {
+        x = X;
+        y = Y;
+        heading = H;
+    }
+}
+
+class ArenaMap extends JPanel {
+
+    private List<Feature> featureList;
+    private double unitsPerPixel;
+
+    public ArenaMap(List<Feature> flist, double mapScale) {
+        featureList = flist;
+        unitsPerPixel = mapScale;
+        SetScale(mapScale);
+    }
+
+    public void SetScale (double mapScale) {
+        if (mapScale < 0.00001) {
+            unitsPerPixel = 0.00001;
+        } else {
+            unitsPerPixel = mapScale;
+        }
+    }
+
+    private void doDrawing(Graphics g) {
+        Graphics2D g2d = (Graphics2D) g;
+
+        int width = getWidth();
+        int height = getHeight();
+
+        // Translate origin to the center of the panel.
+        Graphics2D gCenter = (Graphics2D)g2d.create();
+        gCenter.translate(width/2, height/2);
+
+        for (int ii=0 ; ii < featureList.size() ; ++ii ) {
+            Feature feature = featureList.get(ii);
+            BufferedImage featureImage = feature.bufImage;
+            int ImgOffsetX = featureImage.getWidth()/2;
+            int ImgOffsetY = featureImage.getHeight()/2;
+            int drawLocationX = (int)(feature.x / unitsPerPixel) - ImgOffsetX;
+            int drawLocationY = (int)(feature.y / unitsPerPixel) - ImgOffsetY;
+            AffineTransform tx = AffineTransform.getRotateInstance(feature.heading, ImgOffsetX, ImgOffsetY);
+            AffineTransformOp op = new AffineTransformOp(tx, AffineTransformOp.TYPE_BILINEAR);
+            gCenter.drawImage(op.filter(featureImage, null), drawLocationX, drawLocationY, null);
+        }
+        gCenter.dispose();
+    }
+
+    @Override
+    public void paintComponent(Graphics g) {
+        super.paintComponent(g);
+        doDrawing(g);
+    }
+}
+
+public class EVDisplay extends JFrame {
+
+    private ArenaMap arenaMap;
+    private BufferedReader in;
+    private DataOutputStream out;
+
+    public EVDisplay(ArenaMap arena) {
+        arenaMap = arena;
+        add( arenaMap);
+        setTitle("Vehicle Arena");
+        setSize(800, 800);
+        setLocationRelativeTo(null);
+        setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
+    }
+
+    public void connectToServer(String host, int port ) throws IOException {
+        Socket socket = new Socket(host, port);
+        in = new BufferedReader( new InputStreamReader( socket.getInputStream()));
+        out = new DataOutputStream(new BufferedOutputStream(socket.getOutputStream()));
+    }
+
+    public void drawArenaMap() {
+        arenaMap.repaint();
+    }
+
+    private static void  printHelpText() {
+        System.out.println(
+            "----------------------------------------------------------------------\n"
+          + "usage: java jar EVDisplay.jar <port-number>\n"
+          + "----------------------------------------------------------------------\n"
+          );
+    }
+
+    public static void main(String[] args) throws IOException {
+
+        String host = "localHost";
+        int port = 0;
+        String wayPointsFile = null;
+        String vehicleImageFile = null;
+
+        int ii = 0;
+        while (ii < args.length) {
+            switch (args[ii]) {
+                case "-help" :
+                case "--help" : {
+                    printHelpText();
+                    System.exit(0);
+                } break;
+                case "-v" : {
+                    ++ii;
+                    if (ii < args.length) {
+                        vehicleImageFile = args[ii];
+                    }
+                } break;
+                case "-w" : {
+                    ++ii;
+                    if (ii < args.length) {
+                        wayPointsFile = args[ii];
+                    }
+                } break;
+                default : {
+                    port = (Integer.parseInt(args[ii]));
+                } break;
+            }
+            ++ii;
+        }
+
+       if (port == 0) {
+           System.out.println("No variable server port specified.");
+           printHelpText();
+           System.exit(0);
+       }
+
+       if (wayPointsFile == null) {
+           System.out.println("No waypoints file specified. Use the -w option to specify a waypoints file.");
+           printHelpText();
+           System.exit(0);
+       }
+
+       if (vehicleImageFile == null) {
+           System.out.println("No vehicle image file specified. Use the -v option to specify the vehicle image file.");
+           printHelpText();
+           System.exit(0);
+       }
+
+        List<Feature> featureList = new ArrayList<>();
+
+        try ( BufferedReader br = new BufferedReader( new FileReader( wayPointsFile))) {
+            String line;
+            while ((line = br.readLine()) != null) {
+                String field[] = line.split(",");
+                double X = Double.parseDouble(field[0]);
+                double Y = Double.parseDouble(field[1]);
+                double H = Double.parseDouble(field[2]);
+                String imageFileName = field[3];
+                featureList.add(new Feature( X, Y, H, imageFileName));
+            }
+        }
+
+        Feature vehicle = new Feature(0, 0, Math.toRadians(0), vehicleImageFile);
+        featureList.add(vehicle);
+
+        EVDisplay evd = new EVDisplay( new ArenaMap( featureList, 0.015));
+        evd.setVisible(true);
+
+        System.out.println("Connecting to: " + host + ":" + port);
+        evd.connectToServer(host, port);
+
+        evd.out.writeBytes("trick.var_set_client_tag(\"EVDisplay\") \n");
+        evd.out.flush();
+
+        evd.out.writeBytes("trick.var_add(\"veh.vehicle.position[0]\") \n" +
+                           "trick.var_add(\"veh.vehicle.position[1]\") \n" +
+                           "trick.var_add(\"veh.vehicle.heading\") \n");
+        evd.out.flush();
+
+        evd.out.writeBytes("trick.var_ascii() \n" +
+                           "trick.var_cycle(0.1) \n" +
+                           "trick.var_send() \n" );
+        evd.out.flush();
+
+        Boolean go = true;
+
+        while (go) {
+            String field[];
+            try {
+                String line;
+                line = evd.in.readLine();
+                field = line.split("\t");
+                double X = Double.parseDouble(field[1]);
+                double Y = Double.parseDouble(field[2]);
+                double H = Double.parseDouble(field[3]) + 1.570796325;
+                vehicle.setState(X,Y,H);
+
+            } catch (IOException | NullPointerException e ) {
+                go = false;
+            }
+            evd.drawArenaMap();
+        }
+    }
+}
--- a/trick_models/Wheelbot/Guidance/include/arena.hh
+++ b/trick_models/Wheelbot/Guidance/include/arena.hh
@ -0,0 +1,33 @@
+#ifndef ARENA_H
+#define ARENA_H
+#include <iostream>
+#include <vector>
+#include "gridSquare.hh"
+#include "point.hh"
+
+class Arena {
+    public:
+        Arena(unsigned int width, unsigned int height);
+        Arena(unsigned int width, unsigned int height, unsigned char bits[]);
+        ~Arena();
+        void block(unsigned int x, unsigned int y);
+        void unblock(unsigned int x, unsigned int y);
+        void mark(unsigned int x, unsigned int y, char c);
+        std::vector<GridSquare*> getNeighbors(GridSquare* gridSquarePointer);
+        GridSquare* getGridSquare(unsigned int x, unsigned int y);
+        int getGridSquareCoordinates(GridSquare* gridSquarePointer, Point& coords);
+        int movementCostEstimate(GridSquare* orig, GridSquare* dest, int& cost);
+        int distanceBetween(GridSquare* orig, GridSquare* dest, int& distance);
+        int getHeight(){return height;}
+        int getWidth(){return width;}
+
+        friend std::ostream& operator<< (std::ostream& s, const Arena& arena);
+
+    private:
+        int height;
+        int width;
+        GridSquare *grid;
+        int calcOffset(unsigned int x, unsigned int y, size_t& offset);
+        int calcOffset(GridSquare* gridSquare, size_t& offset);
+};
+#endif
--- a/trick_models/Wheelbot/Guidance/include/findpath.hh
+++ b/trick_models/Wheelbot/Guidance/include/findpath.hh
@ -0,0 +1,5 @@
+#ifndef FINDPATH_HH
+#define FINDPATH_HH
+#include "arena.hh"
+std::vector<Point> FindPath( GridSquare* origin, GridSquare* goal, Arena* arena);
+#endif
--- a/trick_models/Wheelbot/Guidance/include/gridSquare.hh
+++ b/trick_models/Wheelbot/Guidance/include/gridSquare.hh
@ -0,0 +1,12 @@
+#ifndef GRIDSQUARE_HH
+#define GRIDSQUARE_HH
+
+class GridSquare {
+    public:
+    bool isBlocked;
+    char mark;
+    GridSquare* parent;
+    int g_score;
+    int f_score;
+};
+#endif
--- a/trick_models/Wheelbot/Guidance/include/navigator.hh
+++ b/trick_models/Wheelbot/Guidance/include/navigator.hh
@ -0,0 +1,35 @@
+/********************************* TRICK HEADER *******************************
+PURPOSE: ()
+LIBRARY DEPENDENCY:
+    ((Guidance/src/navigator.cpp)
+         )
+PROGRAMMERS:
+    (((John M. Penn) (L3 Communications) (March 2015) (Trick Refresher Project)))
+*******************************************************************************/
+
+#ifndef NAVIGATOR_HH
+#define NAVIGATOR_HH
+
+#include "point.hh"
+
+    class Navigator {
+        public:
+        Navigator(double initial_heading, Point initial_location):
+         heading(initial_heading), location(initial_location)
+        { }
+        void setHeading(double heading);
+        void setLocation(double north, double west);
+
+        double distanceTo(Point& mapPoint);
+        double bearingTo(Point& mapPoint);
+
+        Point convertMapToPlatform(Point& mapPoint);
+        Point convertPlatformToMap(Point& platformPoint);
+        Point convertPlatformToBody(Point& platformPoint);
+        Point convertBodyToPlatform(Point& bodyPoint);
+
+        private:
+        double heading;
+        Point  location;
+    };
+#endif
--- a/trick_models/Wheelbot/Guidance/include/point.hh
+++ b/trick_models/Wheelbot/Guidance/include/point.hh
@ -0,0 +1,10 @@
+#ifndef POINT_HH
+#define POINT_HH
+class Point {
+    public:
+    double x;
+    double y;
+    Point(): x(0.0),y(0.0) {}
+    Point(double X, double Y): x(X),y(Y) {}
+};
+#endif
--- a/trick_models/Wheelbot/Guidance/makefile
+++ b/trick_models/Wheelbot/Guidance/makefile
@ -0,0 +1,36 @@
+RM = rm -rf
+CPP = g++
+
+CXXFLAGS += -DDIAGONAL_NEIGHBORS
+
+INCLUDE_DIRS = -Iinclude
+OBJDIR = obj
+LIBDIR = lib
+LIBNAME = libGuidance.a
+LIBOBJS = $(OBJDIR)/arena.o \
+	  $(OBJDIR)/navigator.o \
+	  $(OBJDIR)/findpath.o
+
+all: ${LIBDIR}/${LIBNAME}
+
+$(LIBOBJS): $(OBJDIR)/%.o : src/%.cpp | $(OBJDIR)
+	$(CPP) $(CXXFLAGS) ${INCLUDE_DIRS} -c $< -o $@
+
+${LIBDIR}/${LIBNAME}: ${LIBOBJS} | ${LIBDIR}
+	ar crs $@ ${LIBOBJS}
+
+${OBJDIR}:
+	mkdir -p ${OBJDIR}
+
+${LIBDIR}:
+	mkdir -p ${LIBDIR}
+
+clean:
+	${RM} *~
+	${RM} ${OBJDIR}
+	rm -f **/*.cpp~
+	rm -f **/*.hh~
+
+spotless: clean
+	${RM} ${LIBDIR}/${LIBNAME}
+
--- a/trick_models/Wheelbot/Guidance/src/arena.cpp
+++ b/trick_models/Wheelbot/Guidance/src/arena.cpp
@ -0,0 +1,201 @@
+#include "arena.hh"
+#include <math.h>      // sqrt()
+#include <stdlib.h>    // abs()
+#include <iostream>
+
+Arena::Arena(unsigned int width, unsigned int height)
+    : height(height), width(width) {
+
+    unsigned int area = height * width;
+    grid = new GridSquare[area];
+    for (int i=0; i < area; i++) {
+        grid[i].isBlocked = false;
+        grid[i].mark = ' ' ;
+    }
+}
+
+Arena::Arena(unsigned int width, unsigned int height, unsigned char bits[])
+      : height(height), width(width) {
+
+   unsigned int area = height * width;
+   grid = new GridSquare[area];
+
+   unsigned int cx =0;
+   for (int row=0; row<height; row++) {
+       unsigned char octet = bits[cx];
+       unsigned int bx=0;
+       while ( bx < width ) {
+           unsigned int ii = row*width+bx;
+           if ((bx != 0) && ((bx % 8)==0)) {
+               cx ++;
+               octet=bits[cx];
+           }
+           grid[ii].isBlocked = (0x01 & octet);
+           octet = octet >> 1;
+           grid[ii].mark = ' ' ;
+           bx++;
+       }
+       cx ++;
+       octet=bits[cx];
+   }
+}
+
+Arena::~Arena() {
+    delete grid;
+}
+
+//straightDistance
+int Arena::distanceBetween( GridSquare* orig, GridSquare* dest, int& distance ) {
+    Point origPt;
+    Point destPt;
+    if ((( getGridSquareCoordinates(orig, origPt) ) == 0) &&
+        (( getGridSquareCoordinates(dest, destPt) ) == 0)) {
+        distance = 10 * sqrt((destPt.x - origPt.x) * (destPt.x - origPt.x) +
+                             (destPt.y - origPt.y) * (destPt.y - origPt.y));
+        return 0;
+    }
+    std::cerr << "Arena::distanceBetween: bad pointer parameter(s)." << std::endl;
+    return 1;
+}
+
+//manhattenDistance
+int Arena::movementCostEstimate( GridSquare* orig, GridSquare* dest, int& costEstimate ) {
+    Point origPt;
+    Point destPt;
+    if ((( getGridSquareCoordinates(orig, origPt) ) == 0) &&
+        (( getGridSquareCoordinates(dest, destPt) ) == 0)) {
+        costEstimate = 10 * (abs(destPt.x - origPt.x) + abs(destPt.y - origPt.y));
+        return 0;
+    }
+    std::cerr << "Arena::movementCostEstimate: bad pointer parameter(s)." << std::endl;
+    return 1;
+}
+
+void Arena::block(unsigned int x, unsigned int y) {
+    GridSquare* gridSquare;
+    if ( (gridSquare = getGridSquare(x, y)) != (GridSquare*)0) {
+        gridSquare->isBlocked = true;
+    }
+}
+
+void Arena::unblock(unsigned int x, unsigned int y) {
+    GridSquare* gridSquare;
+    if ( (gridSquare = getGridSquare(x, y)) != (GridSquare*)0) {
+        gridSquare->isBlocked = false;
+    }
+}
+
+void Arena::mark(unsigned int x, unsigned int y, char c) {
+    GridSquare* gridSquare;
+    if ( (gridSquare = getGridSquare(x, y)) != (GridSquare*)0) {
+        gridSquare->mark = c;
+    }
+}
+
+int Arena::calcOffset(unsigned int x, unsigned int y, size_t& offset) {
+    if ((x < width) && (y < height)) {
+        offset = x+width*y;
+        return 0;
+    }
+    return 1;
+}
+
+int Arena::calcOffset(GridSquare* gridSquare, size_t& offset) {
+
+    if (gridSquare >= grid) {
+        size_t toffset = (gridSquare - grid);
+        if (toffset < (width * height)) {
+            offset = toffset;
+            return 0;
+        }
+    }
+    return 1;
+}
+
+GridSquare* Arena::getGridSquare(unsigned int x, unsigned int y) {
+    size_t offset;
+    if ( calcOffset(x,y,offset) == 0 ) {
+        return ( grid + offset );
+    }
+    return ((GridSquare*)0);
+}
+
+int Arena::getGridSquareCoordinates( GridSquare* gridSquarePointer, Point& coords ) {
+    size_t offset;
+    if ( calcOffset( gridSquarePointer, offset) == 0) {
+        coords.x = offset % width;
+        coords.y = offset / width;
+        return 0;
+    } else {
+        std::cerr << "Arena::getGridSquareCoordinates: problem." << std::endl;
+        return 1;
+    }
+}
+
+std::vector<GridSquare*> Arena::getNeighbors( GridSquare* gridSquarePointer ) {
+
+    std::vector<GridSquare*> neighbors;
+    GridSquare* neighbor;
+    Point loc;
+
+    if ( getGridSquareCoordinates( gridSquarePointer, loc ) == 0) {
+
+#ifdef DIAGONAL_NEIGHBORS
+        if ((neighbor=getGridSquare(loc.x+1, loc.y+1)) != (GridSquare*)0) {
+            if (!neighbor->isBlocked)
+                neighbors.push_back(neighbor);
+        }
+        if ((neighbor=getGridSquare(loc.x+1, loc.y-1)) != (GridSquare*)0) {
+            if (!neighbor->isBlocked)
+                neighbors.push_back(neighbor);
+        }
+        if ((neighbor=getGridSquare(loc.x-1, loc.y-1)) != (GridSquare*)0) {
+            if (!neighbor->isBlocked)
+                neighbors.push_back(neighbor);
+        }
+        if ((neighbor=getGridSquare(loc.x-1, loc.y+1)) != (GridSquare*)0) {
+            if (!neighbor->isBlocked)
+                neighbors.push_back(neighbor);
+        }
+#endif
+        if ((neighbor=getGridSquare(loc.x, loc.y+1)) != (GridSquare*)0) {
+            if (!neighbor->isBlocked)
+                neighbors.push_back(neighbor);
+        }
+        if ((neighbor=getGridSquare(loc.x, loc.y-1)) != (GridSquare*)0) {
+            if (!neighbor->isBlocked)
+                neighbors.push_back(neighbor);
+        }
+        if ((neighbor=getGridSquare(loc.x-1, loc.y)) != (GridSquare*)0) {
+            if (!neighbor->isBlocked)
+                neighbors.push_back(neighbor);
+        }
+        if ((neighbor=getGridSquare(loc.x+1, loc.y)) != (GridSquare*)0) {
+            if (!neighbor->isBlocked)
+                neighbors.push_back(neighbor);
+        }
+
+    } else {
+        std::cerr << "Arena::getNeighbors: invalid gridSquarePointer.";
+    }
+    return neighbors;
+}
+
+std::ostream& operator<< (std::ostream& s, const Arena& arena) {
+    s << "Arena height=" << arena.height << " width=" << arena.width << std::endl;
+
+    for (int y=0; y < arena.height; y++) {
+        s << "|";
+        for (int x=0; x < arena.width; x++) {
+            if ( arena.grid[x + arena.width * y].isBlocked ) {
+                s << "\x1b[41m" << arena.grid[x + arena.width * y].mark << "\x1b[47m" ;
+            } else {
+                s << arena.grid[x + arena.width * y].mark ;
+            }
+            s << "|" ;
+        }
+        s << std::endl;
+    }
+
+    return s;
+}
--- a/trick_models/Wheelbot/Guidance/src/findpath.cpp
+++ b/trick_models/Wheelbot/Guidance/src/findpath.cpp
@ -0,0 +1,110 @@
+#include <stdlib.h>
+#include <vector>    // std::vector
+#include <algorithm> // std::find
+#include "findpath.hh"
+
+std::vector<Point> FindPath( GridSquare* origin, GridSquare* goal, Arena* arena) {
+
+    std::vector<GridSquare*> openSet;
+    std::vector<GridSquare*> closedSet;
+    std::vector<Point> failure;
+
+
+    if (arena == (Arena*)0) {
+        return failure;
+    }
+    Point origPt, goalPt;
+    if (arena->getGridSquareCoordinates(origin, origPt) != 0) {
+        std::cerr << "FindPath: Bad Origin.";
+        return failure;
+    }
+    if (arena->getGridSquareCoordinates(goal, goalPt) != 0) {
+        std::cerr << "FindPath: Bad Goal.";
+        return failure;
+    }
+
+    origin->parent = (GridSquare*)0;
+    origin->g_score = 0;
+    int f_score;
+    arena->movementCostEstimate(origin, goal, f_score);
+    origin->f_score = f_score;
+
+    openSet.push_back(origin);
+
+    while ( !openSet.empty() ) {
+
+        // Get the item in the openSet that has the lowest f_score.
+        std::vector<GridSquare*>::iterator curr, best;
+        for (best=curr=openSet.begin(); curr != openSet.end(); ++curr) {
+             if ( (*curr)->f_score < (*best)->f_score ) {
+                 best = curr;
+             }
+        }
+
+        // Get and remove the best from the open set. This is current.
+        GridSquare* current = *best;
+        openSet.erase(best);
+
+        // if the current position is the goal then backtrack, build
+        // and return the path to get here.
+        if (current == goal) {
+            std::vector<Point> path;
+            while (current != (GridSquare*)0) {
+                Point coordinates;
+                if ( arena->getGridSquareCoordinates( current, coordinates ) == 0) {
+                    path.insert( path.begin(), coordinates);
+                }
+                current = current->parent;
+            }
+            return path;
+        }
+
+        // Add current to the closed set.
+        closedSet.push_back(current);
+
+        // Get the neighbors of current.
+        std::vector<GridSquare*> neighbors = arena->getNeighbors( current );
+
+        // For each of the neighbors
+        std::vector<GridSquare*>::iterator neighborsIterator;
+        for ( neighborsIterator=neighbors.begin();
+              neighborsIterator!=neighbors.end();
+              ++neighborsIterator) {
+
+            GridSquare* neighbor = *neighborsIterator;
+
+            // Search for the neighbor in the closed set.
+            std::vector<GridSquare*>::iterator closedSetIterator =
+                find ( closedSet.begin(), closedSet.end(), neighbor );
+
+            // if neighbor is not in the closed set.
+            if (closedSetIterator == closedSet.end()) {
+
+                // Calculate a g_score for this neighbor.
+                int cost_to_move_to_neighbor;
+                arena->distanceBetween( current, neighbor, cost_to_move_to_neighbor );
+                int neighbor_g_score = current->g_score + cost_to_move_to_neighbor;
+
+                // Search for the neighbor in the openset.
+                std::vector<GridSquare*>::iterator openSetIterator =
+                    find ( openSet.begin(), openSet.end(), neighbor );
+
+                // if neighbor is in the openset and the tentative g score is better than the current score
+                if ((openSetIterator == openSet.end()) || (neighbor_g_score < neighbor->g_score)) {
+
+                    neighbor->parent = current;
+                    neighbor->g_score = neighbor_g_score;
+                    int estimated_cost_to_goal;
+                    arena->movementCostEstimate(neighbor, goal, estimated_cost_to_goal );
+                    neighbor->f_score = neighbor->g_score + estimated_cost_to_goal;
+
+                    if (openSetIterator == openSet.end()) {
+                        openSet.push_back(neighbor);
+                    }
+                }
+            }
+        }
+    }
+    std::cerr << "Failed to find a path to the goal.";
+    return failure;
+}
--- a/trick_models/Wheelbot/Guidance/src/navigator.cpp
+++ b/trick_models/Wheelbot/Guidance/src/navigator.cpp
@ -0,0 +1,60 @@
+#include <math.h> // for: sqrt(), atan2(), cos(), and sin()
+#include "navigator.hh"
+#include <iostream>
+
+void Navigator::setHeading(double h) {
+    heading = h;
+}
+
+void Navigator::setLocation(double x, double y) {
+    location.x = x;
+    location.y = y;
+}
+
+double Navigator::distanceTo(Point& mapPoint ) {
+
+    double deltaX = location.x - mapPoint.x;
+    double deltaY = location.y - mapPoint.y;
+    double distance = sqrt( deltaX * deltaX + deltaY * deltaY );
+    return (distance) ;
+}
+
+double Navigator::bearingTo(Point& mapPoint ) {
+
+    Point platformPoint = convertMapToPlatform(mapPoint);
+    Point bodyPoint = convertPlatformToBody(platformPoint);
+    double head2 = asin(bodyPoint.y/distanceTo(mapPoint) );
+    return head2;
+}
+
+Point Navigator::convertMapToPlatform(Point& mapPoint ) {
+
+    Point platformPoint;
+    platformPoint.x = mapPoint.x - location.x;
+    platformPoint.y = mapPoint.y - location.y;
+    return (platformPoint);
+}
+
+Point Navigator::convertPlatformToMap( Point& platformPoint ) {
+
+    Point mapPoint;
+    mapPoint.x = platformPoint.x + location.x;
+    mapPoint.y = platformPoint.y + location.y;
+    return (mapPoint);
+}
+
+Point Navigator::convertPlatformToBody( Point& platformPoint ) {
+
+    Point bodyPoint;
+    bodyPoint.x =  cos(heading) * platformPoint.x + sin(heading) * platformPoint.y;
+    bodyPoint.y = -sin(heading) * platformPoint.x + cos(heading) * platformPoint.y;
+    return (bodyPoint);
+}
+
+Point Navigator::convertBodyToPlatform( Point& bodyPoint ) {
+
+    Point platformPoint;
+    platformPoint.x =  cos(heading) * bodyPoint.x - sin(heading) * bodyPoint.y;
+    platformPoint.y =  sin(heading) * bodyPoint.x + cos(heading) * bodyPoint.y;
+    return (platformPoint);
+}
--- a/trick_models/Wheelbot/Guidance/test/ArenaTest.cpp
+++ b/trick_models/Wheelbot/Guidance/test/ArenaTest.cpp
@ -0,0 +1,375 @@
+#include <gtest/gtest.h>
+#define private public
+#include "arena.hh"
+#include <algorithm>
+
+TEST( ArenaTest , one ) {
+    // Attempt to create an arena
+    Arena * arena;
+    arena = new Arena(5,5);
+    EXPECT_NE( (void*)0, arena);
+}
+
+TEST( ArenaTest , two ) {
+    // Make sure that arena size is what we specified.
+    Arena arena(5,5);
+    EXPECT_EQ( arena.getWidth(), 5);
+    EXPECT_EQ( arena.getHeight(), 5);
+}
+
+TEST( ArenaTest , three ) {
+    // Make sure that arena size is what we specified.
+    //Checks that x is width and y is height
+    Arena arena(10,7);
+    EXPECT_EQ( arena.getWidth(), 10);
+    EXPECT_EQ( arena.getHeight(), 7);
+}
+
+TEST( ArenaTest , four ) {
+    // Make sure that arena size is what we specified.
+
+    unsigned char ARENA_bits[] = {
+    0x10, 0x00, 0x86, 0x00, 0xe8, 0x00, 0x28, 0x00, 0xe2, 0x00, 0x02, 0x00};
+
+    Arena arena(10,6,ARENA_bits);
+    EXPECT_EQ( arena.getWidth(), 10);
+    EXPECT_EQ( arena.getHeight(), 6);
+}
+
+TEST( ArenaTest , calcOffset_one ) {
+    // Make sure that calcOffset properly adds its value to the variable offset
+    Arena arena(10,7);
+    size_t sz;
+    arena.calcOffset(1,1, sz);
+    EXPECT_EQ( sz, (size_t)11);
+}
+
+TEST( ArenaTest, calcOffset_two )
+{
+  //Checks to make sure calcOffset function returns 1 if the if-statements
+  //evaluate to false
+  Arena arena(10,7);
+  size_t sz;
+  EXPECT_EQ (arena.calcOffset(15,12,sz), 1);
+}
+
+TEST( ArenaTest, calcOffset_three )
+{
+  //Checks to make sure calcOffset function returns 0 if the if-statements
+  //evaluate to true
+  Arena arena(10,7);
+  size_t sz;
+  EXPECT_EQ (arena.calcOffset(5,6,sz), 0);
+}
+
+
+TEST ( ArenaTest, getGridSquare_one)
+{
+  //Tests to make sure getGridSquare gets the gridSquare pointer at the location specified 
+  //Which in this case is the first gridSquare
+  Arena arena(10,7);
+  GridSquare *agridsquare = arena.getGridSquare(0,0);
+  EXPECT_EQ (agridsquare, arena.grid); 
+}
+
+TEST ( ArenaTest, getGridSquare_two)
+{
+  //Tests if the gridSquare that getGridSquare picked up is the correct gridSquare 
+  //by counting the spaces in memory
+  Arena arena(10,7);
+  GridSquare *agridsquare = arena.getGridSquare(2,3);
+  EXPECT_EQ (agridsquare, arena.grid + 32); 
+}
+
+TEST (ArenaTest, getGridSquare_three)
+{
+  //failure case for if-statement within getGridSquare
+  Arena arena(10,7);
+  EXPECT_EQ (arena.getGridSquare(50,70), ((GridSquare*)0));
+}
+
+TEST( ArenaTest, calcOffset2_one )
+{
+  //Checks to make sure calcOffset function returns 1 if the if-statements 
+  //evaluate to false
+  //failure case for first if-statement in calcOffset function
+    Arena arena(10,7);
+    size_t sz;
+    GridSquare* gridSquare = (GridSquare*)(arena.grid-1);
+    EXPECT_EQ (arena.calcOffset(gridSquare,sz), 1);
+}
+
+TEST(ArenaTest, calcOffset2_two)
+{
+  //Tests if calcOffset function returns 0 when all if statement conditions are met
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(2,3);
+  size_t sz;
+  EXPECT_EQ (arena.calcOffset(agridSquare, sz), 0);
+}
+
+TEST(ArenaTest, calcOffset2_three)
+{
+  //Tests if offset value is calculated correctly and put in the correct memory location
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(9,6);
+  size_t sz;
+  arena.calcOffset(agridSquare, sz);
+  EXPECT_EQ (sz, 69);
+}
+
+TEST(ArenaTest, calcOffset2_four)
+{
+  //failure case for second if-statement in calcOffset function
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(15,10);
+  size_t sz;  
+  EXPECT_EQ (arena.calcOffset(agridSquare, sz), 1);
+}
+
+TEST(ArenaTest, getGridSquareCoordinates_one)
+{
+  //failure case for if-statement in getGridSquareCoordinates function
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(15,10);
+  Point coordinate;
+  std::cout << std::endl;
+  std::cout << "The following error message is expected from this test." << std::endl;
+  EXPECT_EQ (arena.getGridSquareCoordinates(agridSquare, coordinate),1); 
+  std::cout << std::endl;
+}
+
+TEST(ArenaTest, getGridSquareCoordinates_two)
+{
+  //Tests if the if-statement evaluates to true that the remainder of code (for that function)
+  //runs through
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(9,6);
+  Point coordinate;
+  EXPECT_EQ (arena.getGridSquareCoordinates(agridSquare, coordinate),0);
+}
+
+TEST(ArenaTest, getGridSquareCoordinates_three)
+{
+  //Tests the arithmetic within the getGridSquareCooordinates function 
+  //once the first if-statement evaluates to true
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(9,6);
+  Point coordinate; 
+  arena.getGridSquareCoordinates(agridSquare, coordinate);
+  EXPECT_EQ (coordinate.x, 9);
+  EXPECT_EQ (coordinate.y, 6);
+}
+
+TEST(ArenaTest, movementCostEstimate_one)
+{
+  //Failure case for the if-statement 
+  //Ensures that the movementCostEstimate function fails appropiately 
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(11,3);
+  GridSquare *anothergridSquare = arena.getGridSquare(12,4);
+  int costestimate;
+  std::cout << std::endl;
+  std::cout << "The following error messages are expected from this test." << std::endl;
+  EXPECT_EQ (arena.movementCostEstimate(agridSquare,anothergridSquare,costestimate), 1);
+  std::cout << std::endl;
+}
+
+TEST(ArenaTest, movementCostEstimate_two)
+{
+  //Tests the case that the if-statement evaluates to true and if the following lines of code
+  //get executed in their entirety
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(1,2);
+  GridSquare *anothergridSquare = arena.getGridSquare(3,4);
+  int costestimate;
+  EXPECT_EQ (arena.movementCostEstimate(agridSquare,anothergridSquare,costestimate), 0);
+}
+
+TEST(ArenaTest, movementCostEstimate_three)
+{
+  //Tests to ensure that movementCostEstimate function is calculating the movement cost correctly
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(1,2);
+  GridSquare *anothergridSquare = arena.getGridSquare(3,4);
+  int costestimate;
+  arena.movementCostEstimate(agridSquare,anothergridSquare,costestimate);
+  EXPECT_EQ (costestimate, 40);
+}
+
+TEST(ArenaTest, distanceBetween_one)
+{
+  //Failure case for the if-statement 
+  //Ensures that the distanceBetween function fails appropiately 
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(11,3);
+  GridSquare *anothergridSquare = arena.getGridSquare(12,4);
+  int dist;
+  std::cout << std::endl;
+  std::cout << "The following error messages are expected from this test." << std::endl;
+  EXPECT_EQ (arena.distanceBetween(agridSquare,anothergridSquare,dist), 1);
+  std::cout << std::endl;
+}
+
+TEST(ArenaTest, distanceBetween_two)
+{
+  //Tests the case that the if-statement evaluates to true and if the following lines of code
+  //get executed in their entirety
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(1,2);
+  GridSquare *anothergridSquare = arena.getGridSquare(3,4);
+  int dist;
+  EXPECT_EQ (arena.distanceBetween(agridSquare,anothergridSquare,dist), 0);
+}
+
+TEST(ArenaTest, distanceBetween_three)
+{
+  //Tests that the distanceBetween function properly calculates the distance between two gridSquares
+  //using gridSquare pointers
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(1,2);
+  GridSquare *anothergridSquare = arena.getGridSquare(3,4);
+  int dist;
+  arena.distanceBetween(agridSquare,anothergridSquare,dist);
+  EXPECT_EQ (dist, 28);
+}
+
+TEST(ArenaTest, blockunblock_one)
+{
+  //Tests to ensure that the block and unblock functions change the isBlocked member 
+  //respective to their names
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(1,2);
+  arena.block(1,2);
+  EXPECT_EQ (agridSquare->isBlocked,true);
+  arena.unblock(1,2);
+  EXPECT_EQ (agridSquare->isBlocked,false);
+}
+
+TEST(ArenaTest, mark_one)
+{
+  //Tests to make sure that the mark function places a mark in the gridSquare desired
+  Arena arena(10,7);
+  GridSquare *agridSquare = arena.getGridSquare(1,2);
+  arena.mark(1,2, 'c');
+  EXPECT_EQ (agridSquare->mark,'c');
+}
+
+TEST(ArenaTest, getNeighbors_one)
+{
+  //Tests that error message displays when trying to 
+  std::cout << std::endl;
+  std::cout << "The following error messages are expected from this test." << std::endl;
+  Arena arena(10,7);
+  std::vector<GridSquare*> neighbors;  
+  GridSquare *agridSquare = arena.getGridSquare(11,22);
+  neighbors = arena.getNeighbors(agridSquare);
+  int length = neighbors.size();  
+  std::cout << std::endl; std::cout << std::endl;
+  EXPECT_EQ (length, 0);  
+}
+
+TEST(ArenaTest, getNeighbors_two)
+{
+  //Tests that getNeighbors returns the correct amount of neighbors within the vector
+  Arena arena(3,3);
+  GridSquare *agridSquare = arena.getGridSquare(1,1);
+  std::vector<GridSquare*> neighbors; 
+  neighbors = arena.getNeighbors(agridSquare);
+  int length = neighbors.size();  
+  EXPECT_EQ (length, 8);
+}
+
+TEST(ArenaTest, getNeighbors_three)
+{
+  //Tests that getNeighbors returns the correct amount of neighbors when 
+  //certain neighbors are blocked
+  Arena arena(3,3);
+  GridSquare *agridSquare = arena.getGridSquare(1,1);
+  std::vector<GridSquare*> neighbors; 
+  arena.block(0,0);
+  arena.block(2,0);
+  arena.block(2,2);
+  neighbors = arena.getNeighbors(agridSquare);  
+  int length = neighbors.size();  
+  EXPECT_EQ (length, 5);
+}
+
+TEST(ArenaTest, getNeighbors_four)
+{
+  //Tests that getNeighbors returns the correct GridSquare pointers in the neighbors vector
+  Arena arena(3,3);
+  GridSquare *agridSquare = arena.getGridSquare(1,1);
+  std::vector<GridSquare*> neighbors; 
+  
+  arena.block(0,0);
+  GridSquare* n0_1 = arena.getGridSquare(0,1);//0,1
+  GridSquare* n0_2 = arena.getGridSquare(0,2);//0,2
+  GridSquare* n1_0 = arena.getGridSquare(1,0);//1,0
+  GridSquare* n1_2 = arena.getGridSquare(1,2);// 1,2
+  arena.block(2,0);
+  GridSquare* n2_1 = arena.getGridSquare(2,1);//2,1
+  arena.block(2,2);
+  
+  neighbors = arena.getNeighbors(agridSquare); 
+  std::vector<GridSquare*>::iterator neighborsIterator;
+  
+  //Test for (0,1) gridSquare
+  bool n0_1_found_flag = false;
+  neighborsIterator = find (neighbors.begin(),neighbors.end(), n0_1); //search for neighbor (0,1) in neighbors
+  if (neighborsIterator != neighbors.end()) //if the value is found
+    n0_1_found_flag = true; //change the found flag to true
+    
+    
+  Point point;
+  arena.getGridSquareCoordinates(*neighborsIterator,point);
+  std::cout << point.x << " "<< point.y << std::endl;
+  
+  EXPECT_EQ(n0_1_found_flag, true);
+  
+  //Test for (0,2) gridSquare
+  bool n0_2_found_flag = false;
+  neighborsIterator = find (neighbors.begin(),neighbors.end(), n0_2); //search for neighbor (0,2) in neighbors
+  if (neighborsIterator != neighbors.end()) //if the value is found
+    n0_2_found_flag = true; //change the found flag to true
+    
+  arena.getGridSquareCoordinates(*neighborsIterator,point);
+  std::cout << point.x << " "<< point.y << std::endl;
+  
+  EXPECT_EQ(n0_2_found_flag, true);
+  
+   //Test for (1,0) gridSquare
+  bool n1_0_found_flag = false;
+  neighborsIterator = find (neighbors.begin(),neighbors.end(), n1_0); //search for neighbor (0,2) in neighbors
+  if (neighborsIterator != neighbors.end()) //if the value is found
+    n1_0_found_flag = true; //change the found flag to true
+    
+  arena.getGridSquareCoordinates(*neighborsIterator,point);
+  std::cout << point.x << " "<< point.y << std::endl;
+  
+  EXPECT_EQ(n1_0_found_flag, true);
+  
+   //Test for (1,2) gridSquare
+  bool n1_2_found_flag = false;
+  neighborsIterator = find (neighbors.begin(),neighbors.end(), n1_2); //search for neighbor (1,2) in neighbors
+  if (neighborsIterator != neighbors.end()) //if the value is found
+    n1_2_found_flag = true; //change the found flag to true
+    
+  arena.getGridSquareCoordinates(*neighborsIterator,point);
+  std::cout << point.x << " "<< point.y << std::endl;
+  
+  EXPECT_EQ(n1_2_found_flag, true);
+  
+   //Test for (2,1) gridSquare
+  bool n2_1_found_flag = false;
+  neighborsIterator = find (neighbors.begin(),neighbors.end(), n2_1); //search for neighbor (2,1) in neighbors
+  if (neighborsIterator != neighbors.end()) //if the value is found
+    n2_1_found_flag = true; //change the found flag to true
+    
+  arena.getGridSquareCoordinates(*neighborsIterator,point);
+  std::cout << point.x << " "<< point.y << std::endl;
+  
+  EXPECT_EQ(n2_1_found_flag, true); 
+  
+  
+}
--- a/trick_models/Wheelbot/Guidance/test/FindPathTest.cpp
+++ b/trick_models/Wheelbot/Guidance/test/FindPathTest.cpp
@ -0,0 +1,243 @@
+#include <gtest/gtest.h>
+#include "findpath.hh"
+#include "arena.hh"
+#include <algorithm>
+#include <vector> 
+
+TEST(FindPathTest, FindPath_one)
+{
+ //Tests the failure case of the FindPath function
+ //This is when the goal is completely blocked 
+ Arena arena(10,7);
+ Arena* testarena;
+ testarena = &arena;
+ 
+ GridSquare *origin = arena.getGridSquare(2,1);
+ GridSquare *goal = arena.getGridSquare(7,5); 
+ 
+ arena.block(7,4);
+ arena.block(6,4);
+ arena.block(8,4);
+ arena.block(6,5);
+ arena.block(8,5);
+ arena.block(6,6);
+ arena.block(8,6);
+ 
+ std::cout << std::endl;
+  
+ std::vector<Point> returnvalue = FindPath(origin,goal,testarena);
+ 
+ std::cout << std::endl;
+ std::cout << std::endl;
+ 
+ EXPECT_EQ(returnvalue.size(),0);
+  
+}
+
+TEST(FindPathTest, FindPath_two)
+{
+ //Tests if FindPath finds a path 
+ Arena arena(10,7);
+ Arena* testarena;
+ testarena = &arena;
+ 
+ GridSquare *origin = arena.getGridSquare(2,1);
+ GridSquare *goal = arena.getGridSquare(7,5); 
+ 
+ 
+ arena.block(2,0);
+ arena.block(7,1);
+ arena.block(0,2);
+ arena.block(1,2);
+ arena.block(3,2);
+ arena.block(6,2);
+ arena.block(3,3);
+ arena.block(6,3);
+ arena.block(8,3);
+ arena.block(2,4);
+ arena.block(3,4);
+ arena.block(6,4);
+ arena.block(8,4);
+ arena.block(2,5);
+ arena.block(6,6);
+ arena.block(7,6);
+ 
+ std::vector<Point> returnvalue = FindPath(origin,goal,testarena);
+ 
+ EXPECT_GT(returnvalue.size(),0);
+  
+}
+
+TEST(FindPathTest, FindPath_three)
+{
+ //Tests if FindPath finds a path when start and goal are the same
+ Arena arena(10,7);
+ Arena* testarena;
+ testarena = &arena;
+ 
+ GridSquare *origin = arena.getGridSquare(2,1);
+ GridSquare *goal = arena.getGridSquare(2,1); 
+ 
+ 
+ arena.block(2,0);
+ arena.block(7,1);
+ arena.block(0,2);
+ arena.block(1,2);
+ arena.block(3,2);
+ arena.block(6,2);
+ arena.block(3,3);
+ arena.block(6,3);
+ arena.block(8,3);
+ arena.block(2,4);
+ arena.block(3,4);
+ arena.block(6,4);
+ arena.block(8,4);
+ arena.block(2,5);
+ arena.block(6,6);
+ arena.block(7,6);
+ 
+ std::vector<Point> returnvalue = FindPath(origin,goal,testarena);
+ 
+ EXPECT_EQ(returnvalue.size(),1);
+  
+}
+
+TEST(FindPathTest, FindPath_four)
+{
+ //Tests if FindPath finds a path when start is outside the arena
+ Arena arena(10,7);
+ Arena* testarena;
+ testarena = &arena;
+ 
+ GridSquare *origin = arena.getGridSquare(11,13);
+ GridSquare *goal = arena.getGridSquare(2,1); 
+ 
+ 
+ 
+ arena.block(2,0);
+ arena.block(7,1);
+ arena.block(0,2);
+ arena.block(1,2);
+ arena.block(3,2);
+ arena.block(6,2);
+ arena.block(3,3);
+ arena.block(6,3);
+ arena.block(8,3);
+ arena.block(2,4);
+ arena.block(3,4);
+ arena.block(6,4);
+ arena.block(8,4);
+ arena.block(2,5);
+ arena.block(6,6);
+ arena.block(7,6);
+ 
+ std::cout << std::endl;
+ std::vector<Point> returnvalue = FindPath(origin,goal,testarena);
+ std::cout << std::endl;
+ std::cout << std::endl;
+ 
+ EXPECT_EQ(returnvalue.size(),0);
+  
+}
+
+TEST(FindPathTest, FindPath_five)
+{
+ //Tests if FindPath finds a path when goal is outside the arena
+ Arena arena(10,7);
+ Arena* testarena;
+ testarena = &arena;
+ 
+ GridSquare *origin = arena.getGridSquare(2,1);
+ GridSquare *goal = arena.getGridSquare(11,12); 
+ 
+ 
+ arena.block(2,0);
+ arena.block(7,1);
+ arena.block(0,2);
+ arena.block(1,2);
+ arena.block(3,2);
+ arena.block(6,2);
+ arena.block(3,3);
+ arena.block(6,3);
+ arena.block(8,3);
+ arena.block(2,4);
+ arena.block(3,4);
+ arena.block(6,4);
+ arena.block(8,4);
+ arena.block(2,5);
+ arena.block(6,6);
+ arena.block(7,6);
+ 
+ std::cout << std::endl;
+ 
+ std::vector<Point> returnvalue = FindPath(origin,goal,testarena);
+ 
+ std::cout << std::endl;
+ std::cout << std::endl;
+ 
+ EXPECT_EQ(returnvalue.size(),0);
+  
+}
+
+/*TEST(FindPathTest, FindPath_three)
+{
+  //Tests if FindPath returns the correct values
+  Arena arena(10,7);
+ Arena* testarena;
+ testarena = &arena;
+ 
+ GridSquare *origin = arena.getGridSquare(2,1);
+ GridSquare *goal = arena.getGridSquare(7,5); 
+ 
+ 
+ arena.block(2,0);
+ arena.block(7,1);
+ arena.block(0,2);
+ arena.block(1,2);
+ arena.block(3,2);
+ arena.block(6,2);
+ arena.block(3,3);
+ arena.block(6,3);
+ arena.block(8,3);
+ arena.block(2,4);
+ arena.block(3,4);
+ arena.block(6,4);
+ arena.block(8,4);
+ arena.block(2,5);
+ arena.block(6,6);
+ arena.block(7,6);
+ 
+ std::vector<Point> desiredvalues (7);
+ 
+ desiredvalues[0].x = 2;
+ desiredvalues[0].y = 1;
+ desiredvalues[1].x = 3;
+ desiredvalues[1].y = 1;
+ desiredvalues[2].x = 4;
+ desiredvalues[2].y = 2;
+ desiredvalues[3].x = 5;
+ desiredvalues[3].y = 3;
+ desiredvalues[4].x = 5;
+ desiredvalues[4].y = 4;
+ desiredvalues[5].x = 6;
+ desiredvalues[5].y = 5;
+ desiredvalues[6].x = 7;
+ desiredvalues[6].y = 5;
+ 
+ std::vector<Point> returnvalue = FindPath(origin,goal,testarena);
+ 
+ EXPECT_EQ(desiredvalues[0].x, returnvalue[0].x);
+ EXPECT_EQ(desiredvalues[0].y, returnvalue[0].y);
+ EXPECT_EQ(desiredvalues[1].x, returnvalue[1].x);
+ EXPECT_EQ(desiredvalues[1].y, returnvalue[1].y);
+ EXPECT_EQ(desiredvalues[2].x, returnvalue[2].x);
+ EXPECT_EQ(desiredvalues[2].y, returnvalue[2].y);
+ EXPECT_EQ(desiredvalues[3].x, returnvalue[3].x);
+ EXPECT_EQ(desiredvalues[3].y, returnvalue[3].y);
+ EXPECT_EQ(desiredvalues[4].x, returnvalue[4].x);
+ EXPECT_EQ(desiredvalues[4].y, returnvalue[4].y);
+ EXPECT_EQ(desiredvalues[5].x, returnvalue[5].x);
+ EXPECT_EQ(desiredvalues[5].y, returnvalue[5].y);
+ EXPECT_EQ(desiredvalues[6].x, returnvalue[6].x);
+ EXPECT_EQ(desiredvalues[6].y, returnvalue[6].y);
+}*/
--- a/trick_models/Wheelbot/Guidance/test/NavigatorTest.cpp
+++ b/trick_models/Wheelbot/Guidance/test/NavigatorTest.cpp
@ -0,0 +1,143 @@
+#include <gtest/gtest.h>
+#define private public
+#include "navigator.hh"
+#include <math.h>
+
+#define PI 3.14159265358979
+#define FP_TOLERANCE 0.000000001
+
+TEST(NavigatorTest, distanceTo_one) {
+
+  Point location(0.0, 0.0);
+  double heading = 0.0;
+  Navigator navigator(heading, location);
+  Point mapPoint(3.0, 4.0);
+  double distance = navigator.distanceTo(mapPoint);
+  EXPECT_NEAR(distance, 5.0, FP_TOLERANCE);
+}
+
+TEST(NavigatorTest, distanceTo_two) {
+
+  Point location(2.0, 2.0);
+  double heading = 0.0;
+  Navigator navigator(heading, location);
+  Point mapPoint(5.0, 6.0);
+  double distance = navigator.distanceTo(mapPoint);
+  EXPECT_NEAR(distance, 5.0, FP_TOLERANCE);
+}
+
+TEST(NavigatorTest, distanceTo_three) {
+  //Tests if distance is found correctly from a negative location to a positive mapPoint
+  Point location(-4,-5);
+  Navigator navigator(PI/6, location);
+  Point mapPoint(6, 9);
+  double distance = navigator.distanceTo(mapPoint);
+
+  EXPECT_NEAR(distance, 17.204650534, FP_TOLERANCE);
+}
+
+TEST(NavigatorTest, convertMapToPlatform_one)
+{
+  //Tests if the mapPoint gets converted to platform correctly 
+  Point location(5,4);
+  Navigator navigator(PI/6, location);  
+  Point mapPoint(6,9);  
+  Point platformPoint;
+  platformPoint = navigator.convertMapToPlatform(mapPoint);
+  
+  EXPECT_EQ(platformPoint.x, 1);
+  EXPECT_EQ(platformPoint.y, 5);
+}
+
+TEST(NavigatorTest, convertMapToPlatform_two)
+{
+  //Tests if the mapPoint gets converted to platform correctly
+  //under slightly more strenuous conditions than the previous test
+  Point location(-8,-9);
+  Navigator navigator(5*PI/6, location);
+  Point mapPoint(3,-5);
+  Point platformPoint = navigator.convertMapToPlatform(mapPoint);
+
+  EXPECT_EQ(platformPoint.x, 11);
+  EXPECT_EQ(platformPoint.y, 4);
+}
+
+TEST(NavigatorTest, convertPlatformToBody_one)
+{
+  // If heading is 45 degrees then <1,0> in platform coordinates should be
+  // <sqrt(2)/2, sqrt(2)/2> in body coordinates.
+  double heading = 45.0 * (PI/180.0);
+  Point location(0,0);
+  Navigator navigator(heading, location);
+  Point platformPoint(1,0);
+  Point bodyPoint = navigator.convertPlatformToBody(platformPoint);
+
+  double expectedResult = sqrt(2.0)/2.0;
+  EXPECT_NEAR(bodyPoint.x, expectedResult, FP_TOLERANCE);
+  EXPECT_NEAR(bodyPoint.y, expectedResult, FP_TOLERANCE);
+}
+
+TEST(NavigatorTest, convertPlatformToBody_two)
+{
+  // If heading is 45 degrees, then <0,1> in platform coordinates
+  // should be <sqrt(2)/2, sqrt(2)/2> in body coordinates.
+  double heading = 45.0 * (PI/180.0);
+  Point location(0,0);
+  Navigator navigator(heading, location);
+  Point platformPoint(1,0);
+  Point bodyPoint = navigator.convertPlatformToBody(platformPoint);
+
+  double expectedResult = sqrt(2.0)/2.0;
+  EXPECT_NEAR(bodyPoint.x, expectedResult, FP_TOLERANCE);
+  EXPECT_NEAR(bodyPoint.y, expectedResult, FP_TOLERANCE);
+}
+
+TEST(NavigatorTest, convertBodyToPlatform_one)
+{
+  // This test is the inverse of convertPlatformToBody_one.
+  double heading = 45.0 * (PI/180.0);
+  Point location(0,0);
+  Navigator navigator(heading, location);
+
+  double H  = sqrt(2.0)/2.0;
+  Point bodyPoint(H,H);
+  Point platformPoint = navigator.convertBodyToPlatform(bodyPoint);
+
+  EXPECT_NEAR(platformPoint.x, 1.0, 00001);
+  EXPECT_NEAR(platformPoint.y, 0.0, 00001);
+}
+
+TEST(NavigatorTest, convertPlatformToMap_one)
+{
+  //Tests if Platform points get converted to mapPoints correctly
+  Point location(-8,-9);
+  Navigator navigator(PI/6, location);    
+  Point platformPoint(11,4);  
+  Point mapPoint;
+  mapPoint = navigator.convertPlatformToMap(platformPoint);
+  
+  EXPECT_EQ (mapPoint.x, 3);
+  EXPECT_EQ (mapPoint.y, -5);  
+}
+
+TEST(NavigatorTest, bearingTo_one)
+{
+  Point location(0,0);
+  Navigator navigator(PI/6, location);      
+  Point mapPoint(3,0);   
+  double bearing;
+  bearing = navigator.bearingTo(mapPoint);
+  
+  EXPECT_NEAR (bearing, (-PI/6), FP_TOLERANCE);
+}
+
+TEST(NavigatorTest, bearingTo_two)
+{
+  Point location(20,0);
+  Navigator navigator(0.0, location);
+  Point mapPoint(20,20);
+  double bearing;
+  bearing = navigator.bearingTo(mapPoint);
+  std::cout << "bearing = " << bearing << std::endl; 
+  EXPECT_NEAR (bearing, (-PI/6), FP_TOLERANCE);
+}
--- a/trick_models/Wheelbot/Guidance/test/makefile
+++ b/trick_models/Wheelbot/Guidance/test/makefile
@ -0,0 +1,55 @@
+
+
+RM = rm -rf
+CC = cc
+CPP = c++
+
+DECL_DIR = ..
+GTEST_DIR = ${HOME}/gtest-1.7.0
+
+CFLAGS += -g -Wall -Wextra -I$(GTEST_DIR)/include -I$(DECL_DIR)/include
+
+LIBS = ../lib/libGuidance.a -lpthread
+
+TESTS = ArenaTest FindPathTest NavigatorTest
+
+GTEST_HEADERS = $(GTEST_DIR)/include/gtest/*.h \
+                $(GTEST_DIR)/include/gtest/internal/*.h
+
+all : $(TESTS)
+
+test: $(TESTS)
+	./ArenaTest              --gtest_output=xml:XMLtestReports/ArenaTestResults.xml
+	./FindPathTest           --gtest_output=xml:XMLtestReports/FindPathTestResults.xml
+	./NavigatorTest		 --gtest_output=xml:XMLtestReports/NavigatorTestResults.xml
+
+clean :
+	rm -f $(TESTS) gtest.a gtest_main.a
+	rm -f *.o
+	rm -f *.cpp~
+	rm -f *.hh~
+	rm -rf XMLtestReports
+
+gtest-all.o :
+	$(CPP) -I$(GTEST_DIR) $(CFLAGS) -c $(GTEST_DIR)/src/gtest-all.cc
+
+gtest_main.o :
+	$(CPP) -I$(GTEST_DIR) $(CFLAGS) -c $(GTEST_DIR)/src/gtest_main.cc
+
+ArenaTest.o : ArenaTest.cpp
+	$(CPP) $(CFLAGS) -c $<
+
+ArenaTest : ArenaTest.o gtest_main.o gtest-all.o
+	$(CPP) $(CFLAGS) -o $@ $^ $(LIBS)
+
+FindPathTest.o : FindPathTest.cpp
+	$(CPP) $(CFLAGS) -c $<
+
+FindPathTest : FindPathTest.o gtest_main.o gtest-all.o
+	$(CPP) $(CFLAGS) -o $@ $^ $(LIBS)
+
+NavigatorTest.o : NavigatorTest.cpp
+	$(CPP) $(CFLAGS) -c $<
+
+NavigatorTest : NavigatorTest.o gtest_main.o gtest-all.o
+	$(CPP) $(CFLAGS) -o $@ $^ $(LIBS)
--- a/trick_models/Wheelbot/Motor/include/DCMotor.hh
+++ b/trick_models/Wheelbot/Motor/include/DCMotor.hh
@ -0,0 +1,27 @@
+#ifndef DCMotor_H
+#define DCMotor_H
+/********************************* TRICK HEADER *******************************
+PURPOSE: ()
+LIBRARY DEPENDENCY:
+    ((DCMotor.o))
+*******************************************************************************/
+#include "PWM.hh"
+
+class DCMotor {
+public:
+    DCMotor(const double initialInternalResistance, const double initialMotorTorqueConstant);
+    void update (const double motorVoltage);
+    void update(const PWM& PulseWidth);
+    double getTorque ();
+    double getCurrentLoad ();
+
+private:
+    double motorTorque;
+    double motorCurrent;
+    double currentLoad;
+    double internalResistance;
+    double motorTorqueConstant;
+};
+
+#endif
+
--- a/trick_models/Wheelbot/Motor/include/DCMotorSpeedController.hh
+++ b/trick_models/Wheelbot/Motor/include/DCMotorSpeedController.hh
@ -0,0 +1,32 @@
+#ifndef DC_MOTOR_SPEED_CONTROLLER_HH
+#define DC_MOTOR_SPEED_CONTROLLER_HH
+
+#include "motorSpeedController.hh"
+#include "DCMotor.hh"
+
+class DCMotorSpeedController : public MotorSpeedController {
+
+    public:
+    DCMotorSpeedController( DCMotor& dc_motor,
+                            double gain,
+                            const double& actual_speed,
+                            const double& supply_voltage);
+
+    ~DCMotorSpeedController() {}
+
+    void setCommandedSpeed( double commandedSpeed );
+    double getMotorVoltage();
+
+    private:
+
+    double motorVoltage;
+    DCMotor& motor;
+    double gain;
+    const double& actualSpeed;
+    const double& supplyVoltage;
+
+    // Don't Allow the default constructor to be used.
+    DCMotorSpeedController();
+};
+
+#endif
--- a/trick_models/Wheelbot/Motor/include/Motor.hh
+++ b/trick_models/Wheelbot/Motor/include/Motor.hh
@ -0,0 +1,20 @@
+#ifndef MOTOR
+#define MOTOR
+/********************************* TRICK HEADER *******************************
+PURPOSE: ()
+LIBRARY DEPENDENCY:
+    ((Motor.o))
+*******************************************************************************/
+#include "PWM.hh"
+class Motor
+{
+  public:
+
+    Motor ();
+    
+    virtual void update (const PWM& PulseWidth) = 0;
+    virtual ~Motor();
+    
+    virtual double getActualSpeed ();
+};
+#endif
--- a/trick_models/Wheelbot/Motor/include/PWM.hh
+++ b/trick_models/Wheelbot/Motor/include/PWM.hh
@ -0,0 +1,34 @@
+#ifndef PWM_H
+#define PWM_H
+/********************************* TRICK HEADER *******************************
+PURPOSE: ()
+LIBRARY DEPENDENCY:
+    ((PWM.o))
+*******************************************************************************/
+
+#include<stdexcept>
+
+class PWM {
+
+  public:
+
+    double highVoltage;
+    double lowVoltage;
+
+    PWM( double HighVoltage,
+         double LowVoltage,
+         double DutyCycle) throw (std::logic_error);
+
+    ~PWM() {}
+
+    void   setDutyCycle( double DutyCycle) throw (std::logic_error);
+    double getDutyCycle() const;
+    double getAverageVoltage() const;
+
+  private:
+
+    PWM(){};
+    double dutyCycle;
+};
+
+#endif
--- a/trick_models/Wheelbot/Motor/include/ServoMotor.hh
+++ b/trick_models/Wheelbot/Motor/include/ServoMotor.hh
@ -0,0 +1,16 @@
+#ifndef SERVO_MOTOR_HH
+#define SERVO_MOTOR_HH
+
+class ServoMotor
+{
+  public:
+    ServoMotor (char side);
+    double getActualSpeed( int PulseWidth );
+  private:
+    ServoMotor();
+    char _side;
+    int _PulseWidth;
+    double actualspeed;
+}
+
+#endif
--- a/trick_models/Wheelbot/Motor/include/ServoSpeedController.hh
+++ b/trick_models/Wheelbot/Motor/include/ServoSpeedController.hh
@ -0,0 +1,20 @@
+#ifndef SERVO_SPEED_CONTROLLER_HH
+#define SERVO_SPEED_CONTROLLER_HH
+
+#include "motorSpeedController.hh"
+#include "ServoMotor.hh"
+
+class ServoSpeedController : public MotorSpeedController 
+{
+  public:
+    ServoSpeedController (ServoMotor& Servo_Motor, char side);
+    void setCommandedSpeed (double commandedSpeed);
+  private:
+    ServoSpeedController();
+    ServoMotor& servo;
+    char _side;
+    double commanded_Speed;
+    double _PulseWidth;
+}
+
+#endif
--- a/trick_models/Wheelbot/Motor/include/motorSpeedController.hh
+++ b/trick_models/Wheelbot/Motor/include/motorSpeedController.hh
@ -0,0 +1,18 @@
+#ifndef MOTOR_SPEED_CONTROLLER_HH
+#define MOTOR_SPEED_CONTROLLER_HH
+
+class MotorSpeedController {
+
+    public:
+    virtual ~MotorSpeedController() {}
+
+    virtual void setCommandedSpeed( double commanded_speed ) {
+        commandedSpeed = commanded_speed;
+    }
+    double getCommandedSpeed() { return commandedSpeed; }
+
+    protected:
+    double commandedSpeed;
+};
+
+#endif
--- a/trick_models/Wheelbot/Motor/makefile
+++ b/trick_models/Wheelbot/Motor/makefile
@ -0,0 +1,34 @@
+RM = rm -rf
+CPP = g++
+
+#CXXFLAGS += -DDIAGONAL_NEIGHBORS
+
+INCLUDE_DIRS = -Iinclude
+OBJDIR = obj
+LIBDIR = lib
+LIBNAME = libMotor.a
+LIBOBJS = $(OBJDIR)/DCMotor.o \
+          $(OBJDIR)/PWM.o \
+          $(OBJDIR)/Motor.o
+
+all: ${LIBDIR}/${LIBNAME}
+
+$(LIBOBJS): $(OBJDIR)/%.o : src/%.cpp | $(OBJDIR)
+	$(CPP) $(CXXFLAGS) ${INCLUDE_DIRS} -c $< -o $@
+
+${LIBDIR}/${LIBNAME}: ${LIBOBJS} | ${LIBDIR}
+	ar crs $@ ${LIBOBJS}
+
+${OBJDIR}:
+	mkdir -p ${OBJDIR}
+
+${LIBDIR}:
+	mkdir -p ${LIBDIR}
+
+clean:
+	${RM} *~
+	${RM} ${OBJDIR}
+	
+
+spotless: clean
+	${RM} ${LIBDIR}/${LIBNAME}
--- a/trick_models/Wheelbot/Motor/src/DCMotor.cpp
+++ b/trick_models/Wheelbot/Motor/src/DCMotor.cpp
@ -0,0 +1,35 @@
+#include "../include/DCMotor.hh"
+#include <iostream>
+#include <cmath>
+
+
+DCMotor::DCMotor (const double initialInternalResistance,
+                  const double initialMotorTorqueConstant)
+                : motorTorque(0.0),
+                  motorCurrent(0.0),
+                  currentLoad(0.0),
+                  internalResistance(initialInternalResistance),
+                  motorTorqueConstant(initialMotorTorqueConstant)
+{ }
+
+void DCMotor :: update (const double motorVoltage)
+{
+    motorCurrent = motorVoltage / internalResistance ;
+    motorTorque = motorCurrent * motorTorqueConstant;
+    currentLoad = std :: abs (motorCurrent);
+}
+
+void DCMotor::update (const PWM& PulseWidth)
+{
+    update(PulseWidth.getAverageVoltage());
+}
+
+double DCMotor :: getTorque()
+{
+    return motorTorque;
+}
+
+double DCMotor :: getCurrentLoad()
+{
+    return currentLoad;
+}
--- a/trick_models/Wheelbot/Motor/src/DCMotorSpeedController.cpp
+++ b/trick_models/Wheelbot/Motor/src/DCMotorSpeedController.cpp
@ -0,0 +1,29 @@
+
+#include "Motor/include/DCMotorSpeedController.hh"
+#include <iostream>
+
+DCMotorSpeedController::DCMotorSpeedController(
+        DCMotor& dc_motor,
+        double Gain,
+        const double& actual_speed,
+        const double& supply_voltage)
+        : motorVoltage(0.0),
+          motor(dc_motor),
+          gain(Gain),
+          actualSpeed(actual_speed),
+          supplyVoltage(supply_voltage)
+{};
+
+void DCMotorSpeedController::setCommandedSpeed ( double commandedSpeed)
+{
+    MotorSpeedController::setCommandedSpeed( commandedSpeed);
+    motorVoltage = supplyVoltage * gain * (commandedSpeed - actualSpeed);
+    motorVoltage = std :: min (supplyVoltage, motorVoltage);
+    motorVoltage = std :: max (-supplyVoltage, motorVoltage);
+    motor.update( motorVoltage);
+}
+
+double DCMotorSpeedController::getMotorVoltage()
+{
+    return motorVoltage;
+}
--- a/trick_models/Wheelbot/Motor/src/Motor.cpp
+++ b/trick_models/Wheelbot/Motor/src/Motor.cpp
@ -0,0 +1,5 @@
+#include "Motor.hh"
+
+Motor::Motor () {}
+Motor::~Motor() {}
+double Motor::getActualSpeed() {return 0;}
--- a/trick_models/Wheelbot/Motor/src/PWM.cpp
+++ b/trick_models/Wheelbot/Motor/src/PWM.cpp
@ -0,0 +1,29 @@
+#include "../include/PWM.hh"
+#include <iostream>
+#include <cmath>
+
+PWM::PWM ( double HighVoltage,
+           double LowVoltage,
+           double DutyCycle) throw (std::logic_error) {
+
+   highVoltage = HighVoltage;
+   lowVoltage  = LowVoltage;
+   setDutyCycle( DutyCycle);
+}
+
+void PWM::setDutyCycle( double DutyCycle) throw (std::logic_error) {
+   if (( DutyCycle >= 0.0 ) && ( DutyCycle <= 1.0 )) {
+       dutyCycle = DutyCycle;
+   } else {
+       throw std::logic_error("PWM::PWM(): DutyCycle must be between 0.0 .. 1.0.");
+   }
+}
+
+double PWM :: getDutyCycle() const {
+    return dutyCycle;
+}
+
+double PWM::getAverageVoltage() const {
+
+    return ( (highVoltage * dutyCycle + lowVoltage * (1 - dutyCycle)) / 2);
+}
--- a/trick_models/Wheelbot/Motor/src/ServoMotor.cpp
+++ b/trick_models/Wheelbot/Motor/src/ServoMotor.cpp
@ -0,0 +1,35 @@
+#include "Motor/include/ServoMotor.hh"
+
+ServoMotor::ServoMotor (char side)
+{
+  _side = side;
+}
+
+double ServoMotor::getActualSpeed(int PulseWidth)
+{
+  _PulseWidth = PulseWidth;
+  
+  if (_side == 'L')
+  {
+     actualspeed = -1.8147280722744906e+001 * pow(_PulseWidth,0)
+		    + -3.4553463215611258e-001 * pow(_PulseWidth,1)
+		    +  4.5593326051360884e-002 * pow(_PulseWidth,2)
+		    + -1.8392645176315394e-003 * pow(_PulseWidth,3)
+		    +  3.3261726281542813e-005 * pow(_PulseWidth,4)
+		    + -2.8937430901462806e-007 * pow(_PulseWidth,5)
+		    +  1.2003663411874751e-009 * pow(_PulseWidth,6)
+		    + -1.9140644089539568e-012 * pow(_PulseWidth,7);
+  }
+  else if (_side == 'R')
+  {
+      actualspeed = 1.8147280722744906e+001 * pow(_PulseWidth,0)
+		    +  3.4553463215611258e-001 * pow(_PulseWidth,1)
+		    + -4.5593326051360884e-002 * pow(_PulseWidth,2)
+		    +  1.8392645176315394e-003 * pow(_PulseWidth,3)
+		    + -3.3261726281542813e-005 * pow(_PulseWidth,4)
+		    +  2.8937430901462806e-007 * pow(_PulseWidth,5)
+		    + -1.2003663411874751e-009 * pow(_PulseWidth,6)
+		    +  1.9140644089539568e-012 * pow(_PulseWidth,7);
+  }
+  return actualspeed;
+}
--- a/trick_models/Wheelbot/Motor/src/ServoSpeedController.cpp
+++ b/trick_models/Wheelbot/Motor/src/ServoSpeedController.cpp
@ -0,0 +1,59 @@
+#include "Motor/include/ServoSpeedController.hh"
+
+
+ServoSpeedController::ServoSpeedController(ServoMotor& Servo_Motor, char side):
+					  servo(Servo_Motor), _side(side)
+{};
+
+void ServoSpeedController::setCommandedSpeed (double commandedSpeed)
+{
+  double PulseWidth;
+  
+  if (_side == 'L')
+  {    
+    std::cout <<"The left wheel desired speed is: " << desiredspeed << std::endl;
+    
+    PulseWidth = 9.1296697267545980e+001 * pow(desiredspeed,0)
+		+  1.3551549019843796e+000 * pow(desiredspeed,1)
+		+ -2.5748263162935388e-002 * pow(desiredspeed,2)
+		+ -3.7691759514032080e-003 * pow(desiredspeed,3)
+		+  3.8490572015823302e-004 * pow(desiredspeed,4)
+		+  4.5526955758039407e-005 * pow(desiredspeed,5)
+		+ -6.7622608926425730e-007 * pow(desiredspeed,6);
+    std::cout << "The pulse width for the side above is " << PulseWidth << std::endl; 
+  
+    _PulseWidth = (int)PulseWidth;  
+  } 
+  else if(_side == 'R')
+  {
+   
+    std::cout <<"The right wheel desired speed is: " << desiredspeed << std::endl;
+    
+    PulseWidth = 9.1296697267545980e+001 * pow(desiredspeed,0)
+		  + -1.3551549019843796e+000 * pow(desiredspeed,1)
+		  + -2.5748263162935388e-002 * pow(desiredspeed,2)
+		  +  3.7691759514032080e-003 * pow(desiredspeed,3)
+		  +  3.8490572015823302e-004 * pow(desiredspeed,4)
+		  + -4.5526955758039407e-005 * pow(desiredspeed,5)
+		  + -6.7622608926425730e-007 * pow(desiredspeed,6);
+    std::cout << "The pulse width for the side above is " << PulseWidth << std::endl; 
+  
+    _PulseWidth = (int)PulseWidth;  
+  }
+  
+  if (_PulseWidth > 180)
+  {
+    _PulseWidth = 180;
+  }
+  else if (_PulseWidth < 0)
+  {
+    _PulseWidth = 0;
+  }
+  
+  if (_side == 'L')
+    std::cout<<"Left Servo angle is: " << _PulseWidth <<std::endl;
+  else if (_side == 'R')
+    std::cout<<"Right Servo angle is: "<< _PulseWidth <<std::endl;
+    
+  //return _PulseWidth;
+}
--- a/trick_models/Wheelbot/Motor/test/DCMotorTest.cpp
+++ b/trick_models/Wheelbot/Motor/test/DCMotorTest.cpp
@ -0,0 +1,83 @@
+#include <gtest/gtest.h>
+#define private public
+#include "DCMotor.hh"
+
+TEST( MotorModelTest , one ) {
+    // Attempt to create a Motor 
+   DCMotor * motor;
+   motor = new DCMotor(8.0, 7.0);
+   EXPECT_NE( (void*)0, motor); 
+   //delete motor;
+}
+
+TEST( MotorModelTest , two ) {
+    // Test Motor Torque
+    DCMotor motor(8.0, 7.0);
+    EXPECT_DOUBLE_EQ( 0.0, motor.motorTorque );
+
+}
+
+TEST( MotorModelTest , three ) {
+    // Test Motor Current
+    DCMotor motor(8.0, 7.0);
+    EXPECT_DOUBLE_EQ( 0.0, motor.motorCurrent );
+
+}
+
+TEST( MotorModelTest , four ) {
+    // Test Battery Current
+    DCMotor motor(8.0, 7.0);
+    EXPECT_DOUBLE_EQ( 0.0, motor.currentLoad );
+
+}
+
+TEST( MotorModelTest , five ) {
+    // Test Internal Resistance
+    DCMotor motor(8.0, 7.0);
+    EXPECT_DOUBLE_EQ( 8.0, motor.internalResistance );
+
+}
+
+TEST( MotorModelTest , six ) {
+    // Test Motor Torque Constant
+    DCMotor motor(8.0, 7.0);
+    EXPECT_DOUBLE_EQ( 7.0, motor.motorTorqueConstant );
+
+}
+
+TEST( MotorModelTest , seven ) {
+    // Test Caculation for Motor Torque
+    DCMotor motor(8.0, 7.0);
+    double motorVoltage = (16.0);
+    motor.update (motorVoltage );
+    EXPECT_DOUBLE_EQ( 14.0, motor.motorTorque );
+    
+}
+
+TEST( MotorModelTest , eight ) {
+    // Test Caculation for Motor Current
+    DCMotor motor(8.0, 7.0);
+    double motorVoltage = (16.0);
+    motor.update (motorVoltage );
+    EXPECT_DOUBLE_EQ( 2.0, motor.motorCurrent );
+    
+}
+
+TEST( MotorModelTest , nine ) {
+    // Test Caculation for Battery Current
+    DCMotor motor(8.0, 7.0);
+    double motorVoltage = (24.0);
+    motor.update (motorVoltage );
+    EXPECT_DOUBLE_EQ( 3.0, motor.currentLoad );
+    
+}
+
+
+TEST( MotorModelTest , ten ) {
+    // Test Caculation for Battery Current that returns an Absolute Value
+    DCMotor motor(8.0, 7.0);
+    double motorVoltage = (-24.0);
+    motor.update (motorVoltage );
+    EXPECT_DOUBLE_EQ( 3.0, motor.currentLoad );
+    
+}
--- a/trick_models/Wheelbot/Motor/test/PWMTest.cpp
+++ b/trick_models/Wheelbot/Motor/test/PWMTest.cpp
--- a/trick_models/Wheelbot/Motor/test/makefile
+++ b/trick_models/Wheelbot/Motor/test/makefile
@ -0,0 +1,48 @@
+
+RM = rm -rf
+CC = cc
+CPP = c++
+
+DECL_DIR = ..
+GTEST_DIR = ${HOME}/gtest-1.7.0
+
+CFLAGS += -g -Wall -Wextra -I$(GTEST_DIR)/include -I$(DECL_DIR)/include
+
+LIBS = ../lib/libMotor.a   -lpthread
+
+TESTS = DCMotorTest PWMTest 
+
+GTEST_HEADERS = $(GTEST_DIR)/include/gtest/*.h \
+                $(GTEST_DIR)/include/gtest/internal/*.h
+
+all : $(TESTS)
+
+test: $(TESTS)
+
+	./DCMotorTest              --gtest_output=xml:XMLtestReports/DCMotorTestResults.xml
+	./PWMTest              --gtest_output=xml:XMLtestReports/PWMTestResults.xml
+	
+clean :
+	rm -f $(TESTS) gtest.a gtest_main.a
+	rm -f *.o
+	rm -f *.cpp~
+	rm -f *.hh~
+	rm -rf XMLtestReports
+
+gtest-all.o :
+	$(CPP) -I$(GTEST_DIR) $(CFLAGS) -c $(GTEST_DIR)/src/gtest-all.cc
+
+gtest_main.o :
+	$(CPP) -I$(GTEST_DIR) $(CFLAGS) -c $(GTEST_DIR)/src/gtest_main.cc
+
+DCMotorTest.o : DCMotorTest.cpp
+	$(CPP) $(CFLAGS) -c $<
+
+DCMotorTest : DCMotorTest.o gtest_main.o gtest-all.o
+	$(CPP) $(CFLAGS) -o $@ $^ $(LIBS)
+
+PWMTest.o : PWMTest.cpp
+	$(CPP) $(CFLAGS) -c $<
+
+PWMTest : PWMTest.o gtest_main.o gtest-all.o
+	$(CPP) $(CFLAGS) -o $@ $^ $(LIBS)
--- a/trick_models/Wheelbot/Motor/test/test/DifferentialDriveControllerTest.cpp
+++ b/trick_models/Wheelbot/Motor/test/test/DifferentialDriveControllerTest.cpp
@ -0,0 +1,172 @@
+#include <gtest/gtest.h>
+#define private public
+#include "Control/include/testMotorController.hh"
+#include "Control/include/differentialDriveController.hh"
+#include <algorithm>
+
+#ifndef PI
+#define PI 3.1415926535
+#endif
+#define FLOAT_TOLERANCE 0.000001
+
+/*
+    Test Fixture.
+*/
+class differentialDriveControllerTest : public ::testing::Test {
+    protected:
+
+    TestMotorController* rightMotorController;
+    TestMotorController* leftMotorController;
+    DifferentialDriveController* driveController;
+
+    differentialDriveControllerTest() {
+    rightMotorController =
+        new TestMotorController();
+    leftMotorController =
+        new TestMotorController();
+    driveController =
+        new DifferentialDriveController(0.183, 0.045, 8.880, 0.200,
+                *rightMotorController, *leftMotorController);
+    }
+
+    ~differentialDriveControllerTest() {
+        delete driveController;
+        delete rightMotorController;
+        delete leftMotorController;
+    }
+};
+
+TEST_F( differentialDriveControllerTest , constructor) {
+
+    EXPECT_NEAR(0.045, driveController->wheelRadius, FLOAT_TOLERANCE);
+    EXPECT_NEAR(0.183, driveController->distanceBetweenWheels, FLOAT_TOLERANCE);
+    EXPECT_NEAR(8.880, driveController->wheelRotationRateLimit, FLOAT_TOLERANCE);
+    EXPECT_NEAR(0.200, driveController->slowDownDistance, FLOAT_TOLERANCE);
+}
+
+TEST_F( differentialDriveControllerTest , setDistanceBetweenWheels ) {
+
+    int result;
+
+    result = driveController->setDistanceBetweenWheels(0.1);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(0.1, driveController->distanceBetweenWheels, FLOAT_TOLERANCE);
+
+    result = driveController->setDistanceBetweenWheels(0.2);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(0.2, driveController->distanceBetweenWheels, FLOAT_TOLERANCE);
+
+    result = driveController->setDistanceBetweenWheels(-0.3);
+    EXPECT_EQ(1,result);
+    EXPECT_NEAR(0.2, driveController->distanceBetweenWheels, FLOAT_TOLERANCE);
+}
+
+TEST_F( differentialDriveControllerTest , setWheelRadius ) {
+
+    int result;
+    result = driveController->setWheelRadius(0.059);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(0.059, driveController->wheelRadius, FLOAT_TOLERANCE);
+
+    result = driveController->setWheelRadius(0.083);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(0.083, driveController->wheelRadius, FLOAT_TOLERANCE);
+
+    result = driveController->setWheelRadius(-0.075);
+    EXPECT_EQ(1,result);
+    EXPECT_NEAR(0.083, driveController->wheelRadius, FLOAT_TOLERANCE);
+}
+
+TEST_F( differentialDriveControllerTest , setWheelRotationRateLimit ) {
+
+    int result;
+    result = driveController->setWheelRotationRateLimit(7.123);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(7.123, driveController->wheelRotationRateLimit, FLOAT_TOLERANCE);
+
+    result = driveController->setWheelRotationRateLimit(5.234);
+    EXPECT_EQ(0,result);
+    EXPECT_NEAR(5.234, driveController->wheelRotationRateLimit, FLOAT_TOLERANCE);
+
+    result = driveController->setWheelRotationRateLimit(-4.987);
+    EXPECT_EQ(1,result);
+    EXPECT_NEAR(5.234, driveController->wheelRotationRateLimit, FLOAT_TOLERANCE);
+}
+
+TEST_F( differentialDriveControllerTest , PositiveRangeErrorOnly) {
+
+    // If there is no heading error, and the distance error is non-zero,
+    // then the speeds should be the same, and equal to the wheelRotationRateLimit.
+
+    double distance_err = 1.0;
+    double heading_err  = 0.0;
+    driveController->update(distance_err, heading_err);
+
+    double rightMotorSpeedCommand, leftMotorSpeedCommand;
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    // the speeds should be the same
+    EXPECT_NEAR(rightMotorSpeedCommand, leftMotorSpeedCommand, FLOAT_TOLERANCE);
+    // and equal to the setWheelRotationRateLimit.
+    EXPECT_NEAR(rightMotorSpeedCommand, 8.880, FLOAT_TOLERANCE);
+}
+
+TEST_F( differentialDriveControllerTest , positiveHeadingError ) {
+
+    // If the heading error is positive, then we should turn to the right,
+    // meaning that the left wheel should move faster than the right wheel.
+
+    double rightMotorSpeedCommand, leftMotorSpeedCommand;
+
+    driveController->update(0.0, 0.1*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(50.0, 30*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(100.0, 60*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(0.0, 89*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(0.0, 90*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(50.0, 91*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(100.0, 120*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(0.0, 150*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+
+    driveController->update(50.0, 179*(PI/180.0));
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(leftMotorSpeedCommand, rightMotorSpeedCommand);
+}
+
+TEST_F( differentialDriveControllerTest, negativeHeadingError ) {
+
+    // If the heading error is negative, then we should turn to the left,
+    // meaning that the right wheel should move faster than the left wheel.
+
+    double distance_err = 0.0;
+    double heading_err  = -30*(PI/180.0);
+    driveController->update(distance_err, heading_err);
+
+    double rightMotorSpeedCommand, leftMotorSpeedCommand;
+    driveController->getCommandedMotorSpeeds(leftMotorSpeedCommand, rightMotorSpeedCommand);
+    EXPECT_GT(rightMotorSpeedCommand, leftMotorSpeedCommand);
+}
+
+
--- a/trick_models/Wheelbot/Motor/test/test/VehicleControllerTest.cpp
+++ b/trick_models/Wheelbot/Motor/test/test/VehicleControllerTest.cpp
@ -0,0 +1,58 @@
+#include <gtest/gtest.h>
+#define private public
+#include "Control/include/testMotorController.hh"
+#include "Control/include/vehicleController.hh"
+#include <algorithm>
+
+#ifndef PI
+#define PI 3.1415926535
+#endif
+#define FLOAT_TOLERANCE 0.000001
+
+/*
+Test Fixture.
+*/
+class vehicleControllerTest : public ::testing::Test {
+    protected:
+    std::vector<Point> waypointQueue;
+    Navigator *navigator;
+    TestMotorController* rightMotorController;
+    TestMotorController* leftMotorController;
+    DifferentialDriveController* driveController;
+    VehicleController* vehicleController;
+
+    vehicleControllerTest() {
+
+        Point waypoint(1.0, 3.0);
+        waypointQueue.push_back( waypoint);
+
+        Point initLocation(0.0, 0.0);
+        rightMotorController = new TestMotorController();
+        leftMotorController  = new TestMotorController();
+        driveController = new DifferentialDriveController(0.183, 0.045, 8.880, 0.200,
+                                                          *rightMotorController,
+                                                          *leftMotorController);
+        navigator = new Navigator(0.0, initLocation);
+        vehicleController = new VehicleController( &waypointQueue, *navigator, *driveController, 0.02);
+
+    }
+
+    ~vehicleControllerTest() {
+        delete navigator;
+        delete driveController;
+    }
+
+    void SetUp() {}
+    void TearDown() {}
+};
+
+
+TEST_F( vehicleControllerTest , one ) {
+
+    Point current_destination;
+    int result = vehicleController->getCurrentDestination(current_destination);
+    EXPECT_EQ(result, 0);
+    EXPECT_EQ(current_destination.x, 1);
+    EXPECT_EQ(current_destination.y, 3);
+}
+
--- a/trick_models/Wheelbot/Motor/test/test/makefile
+++ b/trick_models/Wheelbot/Motor/test/test/makefile
@ -0,0 +1,52 @@
+RM = rm -rf
+CC = cc
+CPP = c++
+
+PROJECT_DIR = ../..
+
+GTEST_DIR = ${HOME}/gtest-1.7.0
+
+CFLAGS += -g -Wall -Wextra
+
+INCLUDE_DIRS += -I$(PROJECT_DIR)\
+	        -I$(GTEST_DIR)/include
+
+LIBS = $(PROJECT_DIR)/Control/lib/libControl.a \
+       $(PROJECT_DIR)/Guidance/lib/libGuidance.a \
+       -lpthread
+
+TESTS = DifferentialDriveControllerTest VehicleControllerTest
+
+GTEST_HEADERS = $(GTEST_DIR)/include/gtest/*.h \
+                $(GTEST_DIR)/include/gtest/internal/*.h
+
+all : $(TESTS)
+
+test: $(TESTS)
+	./DifferentialDriveControllerTest --gtest_output=xml:XMLtestReports/DifferentialDriveControllerTestResults.xml
+	./VehicleControllerTest --gtest_output=xml:XMLtestReports/VehicleControllerTestResults.xml
+
+clean :
+	rm -f $(TESTS) gtest.a gtest_main.a
+	rm -f *.o
+	rm -f *.cpp~
+	rm -f *.hh~
+	rm -rf XMLtestReports
+
+gtest-all.o :
+	$(CPP) -I$(GTEST_DIR) -I$(GTEST_DIR)/include $(CFLAGS) -c $(GTEST_DIR)/src/gtest-all.cc
+
+gtest_main.o :
+	$(CPP) -I$(GTEST_DIR) -I$(GTEST_DIR)/include $(CFLAGS) -c $(GTEST_DIR)/src/gtest_main.cc
+
+DifferentialDriveControllerTest.o : DifferentialDriveControllerTest.cpp
+	$(CPP) $(CFLAGS) $(INCLUDE_DIRS) -c $<
+
+DifferentialDriveControllerTest : DifferentialDriveControllerTest.o gtest_main.o gtest-all.o
+	$(CPP) $(CFLAGS) $(INCLUDE_DIRS) -o $@ $^ $(LIBS)
+
+VehicleControllerTest.o : VehicleControllerTest.cpp
+	$(CPP) $(CFLAGS) $(INCLUDE_DIRS) -c $<
+
+VehicleControllerTest : VehicleControllerTest.o gtest_main.o gtest-all.o
+	$(CPP) $(CFLAGS) $(INCLUDE_DIRS) -o $@ $^ $(LIBS)
--- a/trick_models/Wheelbot/Vehicle/include/vehicleOne.hh
+++ b/trick_models/Wheelbot/Vehicle/include/vehicleOne.hh
@ -0,0 +1,70 @@
+#ifndef TEST_VEHICLE_HH
+#define TEST_VEHICLE_HH
+/*************************************************************************
+PURPOSE: ()
+**************************************************************************/
+#include "Guidance/include/point.hh"
+#include "Control/include/vehicleController.hh"
+#include "Control/include/differentialDriveController.hh"
+#include "Motor/include/DCMotorSpeedController.hh"
+
+class VehicleOne {
+    public:
+    std::vector<Point> waypointQueue;
+    Navigator *navigator;
+    MotorSpeedController* rightMotorController;
+    MotorSpeedController* leftMotorController;
+    DCMotor* rightDCMotor;
+    DCMotor* leftDCMotor;
+
+    DifferentialDriveController* driveController;
+    VehicleController* vehicleController;
+
+    double distanceBetweenWheels;     /* m */
+    double wheelRadius;               /* m */
+    double vehicleMass;               /* kg */
+    double ZAxisMomentofInertia;
+
+    // Vehicle Controller Parameters
+    double slowDownDistance;          /* m */
+    double arrivalDistance;           /* m */
+    double wheelSpeedLimit;           /* r/s */
+    double headingRateLimit;          /* r/s */
+
+    // DCMotor Parameters
+    double DCMotorInternalResistance; /* ohm */
+    double DCMotorTorqueConstant;     /* N*m/amp */
+
+    double wheelDragConstant;
+    double corningStiffness;
+
+    double position[3];              /* m */
+    double velocity[3];              /* m/s */
+    double acceleration[3];          /* m/s2 */
+
+    double heading;                  /* r */
+    double headingRate;              /* r/s */
+    double headingAccel;             /* r/s2 */
+
+    double rightMotorSpeed;          /* r/s */
+    double leftMotorSpeed;           /* r/s */
+
+    // Forces
+    double driveForce[2];            /* N */
+    double lateralTireForce[2];      /* N */
+    double rollingResistForce[2];    /* N */
+    double forceTotal[2];            /* N */
+    double vehicleZTorque;           /* N*m */
+
+    double batteryVoltage;
+
+    void add_waypoint(double x, double y);
+
+    int default_data();
+    int state_init();
+    void control();
+    int state_deriv();
+    int state_integ();
+};
+
+#endif
--- a/trick_models/Wheelbot/Vehicle/makefile
+++ b/trick_models/Wheelbot/Vehicle/makefile
@ -0,0 +1,33 @@
+RM = rm -rf
+CPP = g++
+
+CXXFLAGS += -g
+
+INCLUDE_DIRS = -Iinclude -I../Guidance/include
+OBJDIR = obj
+LIBDIR = lib
+LIBNAME = libControl.a
+LIBOBJS = $(OBJDIR)/differentialDriveController.o \
+	  $(OBJDIR)/vehicleController.o
+
+all: ${LIBDIR}/${LIBNAME}
+
+$(LIBOBJS): $(OBJDIR)/%.o : src/%.cpp | $(OBJDIR)
+	$(CPP) $(CXXFLAGS) ${INCLUDE_DIRS} -c $< -o $@
+
+${LIBDIR}/${LIBNAME}: ${LIBOBJS} | ${LIBDIR}
+	ar crs $@ ${LIBOBJS}
+
+${OBJDIR}:
+	mkdir -p ${OBJDIR}
+
+${LIBDIR}:
+	mkdir -p ${LIBDIR}
+
+clean:
+	${RM} *~
+	${RM} ${OBJDIR}
+
+spotless: clean
+	${RM} ${LIBDIR}/${LIBNAME}
+
--- a/trick_models/Wheelbot/Vehicle/src/vehicleOne.cpp
+++ b/trick_models/Wheelbot/Vehicle/src/vehicleOne.cpp
@ -0,0 +1,235 @@
+/********************************* TRICK HEADER *******************************
+PURPOSE: ( Simulate a two wheeled robotic vehicle.)
+LIBRARY DEPENDENCY:
+    ((vehicleOne.o)
+     (Control/src/vehicleController.o)
+     (Control/src/differentialDriveController.o)
+     (Motor/src/DCMotorSpeedController.o)
+     (Motor/src/DCMotor.o))
+PROGRAMMERS:
+    (((John M. Penn) (L3 Communications) (June 2015) (Trick Refresher Project)))
+*******************************************************************************/
+#include "Vehicle/include/vehicleOne.hh"
+#include <iostream>
+#include <math.h>
+#ifndef PI
+#define PI 3.1415926535
+#endif
+
+int VehicleOne::default_data() {
+
+    distanceBetweenWheels = 0.183;
+    wheelRadius = 0.045;
+    vehicleMass  = 2.0;
+    double axelRadius = 0.5 * distanceBetweenWheels;
+    ZAxisMomentofInertia = 0.5 * vehicleMass * axelRadius * axelRadius;
+
+    // Vehicle Controller Parameters
+    slowDownDistance = 0.2;
+    arrivalDistance  = 0.1;
+    wheelSpeedLimit = 8.880;
+    headingRateLimit = PI/3;
+
+    // DCMotor Parameters
+    // At 5v the following parameters will result in a current of
+    // 0.5 amperes, and a torque of 0.5 x 0.15 = 0.075 Newton-meters.
+    DCMotorInternalResistance = 10.0; // Ohms
+    DCMotorTorqueConstant     = 0.15; // Newton-meters / Ampere
+
+   // Assuming torque = 0.075 Nm, then the wheel force due to
+   // torque = 0.075 Nm / 0.045 = 1.67N. If we want the wheel force
+   // to be 0 when wheel speed = 0.4 m/s
+   // 0.075 = wheelDragConstant * 0.4
+   // wheelDragConstant = 0.075/0.4 = 1.875
+    wheelDragConstant  = 1.875;
+    corningStiffness = 10.0;
+
+    // Dynamic State
+    position[0] = 0.0;
+    position[1] = 0.0;
+    position[2] = 0.0;
+
+    velocity[0] = 0.0;
+    velocity[1] = 0.0;
+    velocity[2] = 0.0;
+
+    acceleration[0] = 0.0;
+    acceleration[1] = 0.0;
+    acceleration[2] = 0.0;
+
+    heading     = 0.0;
+    headingRate = 0.0;
+    headingAccel = 0.0;
+
+    // Feedback to Motors
+    rightMotorSpeed = 0.0;
+    leftMotorSpeed  = 0.0;
+
+    batteryVoltage  = 5.0;
+
+    return 0;
+}
+
+int VehicleOne::state_init() {
+
+
+    Point initLocation( position[0],
+                        position[1]);
+
+    rightDCMotor = new DCMotor( DCMotorInternalResistance, DCMotorTorqueConstant );
+    leftDCMotor  = new DCMotor( DCMotorInternalResistance, DCMotorTorqueConstant );
+
+    // Note that right and left motor speeds are passed by reference.
+    rightMotorController = new DCMotorSpeedController(*rightDCMotor, .3, rightMotorSpeed, batteryVoltage );
+    leftMotorController  = new DCMotorSpeedController(*leftDCMotor,  .3, leftMotorSpeed,  batteryVoltage);
+
+    driveController =
+        new DifferentialDriveController(distanceBetweenWheels,
+                                        wheelRadius,
+                                        wheelSpeedLimit,
+                                        headingRateLimit,
+                                        slowDownDistance,
+                                        *rightMotorController,
+                                        *leftMotorController);
+
+    navigator =
+        new Navigator(heading, initLocation);
+
+    vehicleController =
+        new VehicleController( &waypointQueue, *navigator, *driveController, arrivalDistance);
+
+    return (0);
+}
+
+void VehicleOne::add_waypoint(double x, double y) {
+    Point wayPoint(x,y);
+    waypointQueue.push_back(wayPoint);
+}
+
+
+void VehicleOne::control() {
+
+    // Perfect Sensors for now.
+    navigator->setHeading(heading);
+    navigator->setLocation(position[0], position[1]);
+
+    vehicleController->update();
+}
+
+
+int VehicleOne::state_deriv() {
+
+   double speed = sqrt(velocity[0] * velocity[0] + velocity[1] * velocity[1]);
+
+   // Direction that the vehicle is moving.
+   double velocity_unit[2];
+   velocity_unit[0] = velocity[0] / speed;
+   velocity_unit[1] = velocity[1] / speed;
+
+   // Direction that the vehicle is pointing.
+   double heading_unit[2];
+   heading_unit[0] =  cos(heading);
+   heading_unit[1] =  sin(heading);
+
+   // Meters/second, positive forward
+   double turningSpeed = 0.5 * distanceBetweenWheels * headingRate;
+
+   // Motorspeed: (radians/second), positive counter-clockwise.
+   // Feedback to the motor controllers.
+   rightMotorSpeed = -(speed + turningSpeed) / wheelRadius;
+   leftMotorSpeed  =  (speed - turningSpeed) / wheelRadius;
+
+
+   // Traction Force: Newtons positive forward
+   double leftWheelTractionForce  =  leftDCMotor->getTorque()  / wheelRadius;
+   double rightWheelTractionForce = -rightDCMotor->getTorque() / wheelRadius;
+   double driveForceMag = leftWheelTractionForce + rightWheelTractionForce;
+
+   // Traction Torque
+   vehicleZTorque = (rightWheelTractionForce - leftWheelTractionForce) * (0.5 * distanceBetweenWheels) ;
+
+   driveForce[0] = cos(heading) * driveForceMag;
+   driveForce[1] = sin(heading) * driveForceMag;
+
+   // Lateral Tire (Turning) Force
+   if (speed == 0.0) {
+       lateralTireForce[0] = 0.0;
+       lateralTireForce[1] = 0.0;
+   } else {
+       double tireSlip[2];
+       tireSlip[0] = heading_unit[0] - velocity_unit[0];
+       tireSlip[1] = heading_unit[1] - velocity_unit[1];
+
+       lateralTireForce[0] = corningStiffness * tireSlip[0];
+       lateralTireForce[1] = corningStiffness * tireSlip[1];
+   }
+
+   // Rolling Tire Resistance Force
+   rollingResistForce[0] = -velocity[0] * wheelDragConstant;
+   rollingResistForce[1] = -velocity[1] * wheelDragConstant;
+
+   // Total Body Force
+   forceTotal[0] = driveForce[0] + lateralTireForce[0] + rollingResistForce[0];
+   forceTotal[1] = driveForce[1] + lateralTireForce[1] + rollingResistForce[1];
+
+   // Body Rotational Acceleration
+   headingAccel = vehicleZTorque / ZAxisMomentofInertia;
+
+   // Body Linear Acceleration
+   acceleration[0] = forceTotal[0] / vehicleMass;
+   acceleration[1] = forceTotal[1] / vehicleMass;
+
+   return 0;
+}
+
+#include "sim_services/Integrator/include/integrator_c_intf.h"
+
+int VehicleOne::state_integ() {
+    int integration_step;
+
+    load_state(
+                &heading,
+                &headingRate,
+                &position[0],
+                &position[1],
+                &position[2],
+                &velocity[0],
+                &velocity[1],
+                &velocity[2],
+                (double*)0
+              );
+
+    load_deriv(
+                &headingRate,
+                &headingAccel,
+                &velocity[0],
+                &velocity[1],
+                &velocity[2],
+                &acceleration[0],
+                &acceleration[1],
+                &acceleration[2],
+                (double*)0
+              );
+
+    integration_step = integrate();
+
+    unload_state(
+                &heading,
+                &headingRate,
+                &position[0],
+                &position[1],
+                &position[2],
+                &velocity[0],
+                &velocity[1],
+                &velocity[2],
+                (double*)0
+              );
+
+    if (!integration_step) {
+        if (heading < -PI) heading +=  2*PI;
+        if (heading >  PI) heading += -2*PI;
+    }
+
+    return(integration_step);
+
+}
--- a/trick_sims/SIM_wheelbot/Modified_data/cross.waypoints
+++ b/trick_sims/SIM_wheelbot/Modified_data/cross.waypoints
@ -0,0 +1,12 @@
+ 2, 0, 0,images/wp1.png
+ 2, 2, 0,images/wp2.png
+ 0, 2, 0,images/wp3.png
+ 0, 4, 0,images/wp4.png
+-2, 4, 0,images/wp5.png
+-2, 2, 0,images/wp6.png
+-4, 2, 0,images/wp7.png
+-4, 0, 0,images/wp8.png
+-2, 0, 0,images/wp9.png
+-2,-2, 0,images/wp10.png
+ 0,-2, 0,images/wp11.png
+ 0, 0, 0,images/wp0.png
--- a/trick_sims/SIM_wheelbot/Modified_data/realtime.py
+++ b/trick_sims/SIM_wheelbot/Modified_data/realtime.py
@ -0,0 +1,10 @@
+trick.frame_log_on()
+trick.real_time_enable()
+trick.exec_set_software_frame(0.1)
+trick.itimer_enable()
+
+trick.exec_set_enable_freeze(True)
+trick.exec_set_freeze_command(True)
+
+simControlPanel = trick.SimControlPanel()
+trick.add_external_application(simControlPanel)
--- a/trick_sims/SIM_wheelbot/Modified_data/vehicleState.dr
+++ b/trick_sims/SIM_wheelbot/Modified_data/vehicleState.dr
@ -0,0 +1,28 @@
+global DR_GROUP_ID
+global drg
+try:
+    if DR_GROUP_ID >= 0:
+        DR_GROUP_ID += 1
+except NameError:
+    DR_GROUP_ID = 0
+    drg = []
+
+drg.append(trick.DRAscii("vehicleState"))
+drg[DR_GROUP_ID].set_freq(trick.DR_Always)
+drg[DR_GROUP_ID].set_cycle(0.05)
+drg[DR_GROUP_ID].set_single_prec_only(False)
+drg[DR_GROUP_ID].add_variable("veh.vehicle.position[0]")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.position[1]")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.position[2]")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.velocity[0]")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.velocity[1]")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.velocity[2]")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.acceleration[0]")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.acceleration[1]")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.acceleration[2]")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.heading")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.headingRate")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.rightMotorSpeed")
+drg[DR_GROUP_ID].add_variable("veh.vehicle.leftMotorSpeed")
+trick.add_data_record_group(drg[DR_GROUP_ID], trick.DR_Buffer)
+drg[DR_GROUP_ID].enable()
--- a/trick_sims/SIM_wheelbot/README.md
+++ b/trick_sims/SIM_wheelbot/README.md
@ -0,0 +1,4 @@
+#SIM\_rover
+---
+This is a simulation of a two-wheeled robotic electric vehicle.
+
--- a/trick_sims/SIM_wheelbot/RUN_test/input.py
+++ b/trick_sims/SIM_wheelbot/RUN_test/input.py
@ -0,0 +1,49 @@
+execfile("Modified_data/realtime.py")
+execfile("Modified_data/vehicleState.dr")
+
+trick.TMM_reduced_checkpoint(0)
+
+veh_integloop.getIntegrator(trick.Runge_Kutta_4, 8)
+
+veh.vehicle.position[0] = 0.0
+veh.vehicle.position[1] = 0.0
+veh.vehicle.heading = 0.0;
+
+veh.vehicle.distanceBetweenWheels = 0.183
+veh.vehicle.wheelRadius = 0.045
+veh.vehicle.wheelSpeedLimit = 8.880
+veh.vehicle.headingRateLimit = 3.14159/3
+
+veh.vehicle.slowDownDistance = 0.5
+veh.vehicle.arrivalDistance  = 0.1
+
+#==========================================
+# Add the waypoints to the SIM.
+#==========================================
+waypoints_path = "Modified_data/cross.waypoints"
+fp = open(waypoints_path, "r")
+for line in fp:
+    fields = line.split(r",")
+    veh.vehicle.add_waypoint( float(fields[0]), float(fields[1]))
+
+#==========================================
+# Start the display VarServer Client
+#==========================================
+varServerPort = trick.var_server_get_port();
+trick_home = os.environ['TRICK_HOME']
+EVDisplay_path = trick_home + "/trick_models/Wheelbot/Graphics/dist/EVDisplay.jar"
+
+if (os.path.isfile(EVDisplay_path)) :
+    EVDisplay_cmd = "java -jar " \
+                  + EVDisplay_path \
+                  + " -v images/twoWheelRover.png " \
+                  + " -w " + waypoints_path \
+                  + " " + str(varServerPort) + " &" ;
+    print(EVDisplay_cmd)
+    os.system( EVDisplay_cmd);
+else :
+    print('============================')
+    print('EVDisplay needs to be built.')
+    print('============================')
+
+trick.stop(100)
--- a/trick_sims/SIM_wheelbot/S_define
+++ b/trick_sims/SIM_wheelbot/S_define
@ -0,0 +1,28 @@
+/************************TRICK HEADER*************************
+PURPOSE:
+    ( Simulate a skydiver jump from very high altitude. )
+LIBRARY DEPENDENCIES:
+    ((Vehicle/src/vehicleOne.cpp))
+*************************************************************/
+
+#include "sim_objects/default_trick_sys.sm"
+
+##include "Vehicle/include/vehicleOne.hh"
+
+class VehicleSimObject : public Trick::SimObject {
+
+    public:
+    VehicleOne vehicle;
+
+    VehicleSimObject() {
+        ("default_data")     vehicle.default_data() ;
+        ("initialization")   vehicle.state_init() ;
+        (0.10, "scheduled")  vehicle.control();
+        ("derivative")       vehicle.state_deriv() ;
+        ("integration") trick_ret = vehicle.state_integ() ;
+    }
+};
+
+VehicleSimObject veh ;
+
+IntegLoop veh_integloop (0.10) veh;
--- a/trick_sims/SIM_wheelbot/S_overrides.mk
+++ b/trick_sims/SIM_wheelbot/S_overrides.mk
@ -0,0 +1,2 @@
+TRICK_CFLAGS += -I${TRICK_HOME}/trick_models/Wheelbot
+TRICK_CXXFLAGS += -I${TRICK_HOME}/trick_models/Wheelbot
--- a/trick_sims/SIM_wheelbot/images/twoWheelRover.png
+++ b/trick_sims/SIM_wheelbot/images/twoWheelRover.png
--- a/trick_sims/SIM_wheelbot/images/wp0.png
+++ b/trick_sims/SIM_wheelbot/images/wp0.png
--- a/trick_sims/SIM_wheelbot/images/wp1.png
+++ b/trick_sims/SIM_wheelbot/images/wp1.png
--- a/trick_sims/SIM_wheelbot/images/wp10.png
+++ b/trick_sims/SIM_wheelbot/images/wp10.png
--- a/trick_sims/SIM_wheelbot/images/wp11.png
+++ b/trick_sims/SIM_wheelbot/images/wp11.png
--- a/trick_sims/SIM_wheelbot/images/wp2.png
+++ b/trick_sims/SIM_wheelbot/images/wp2.png
--- a/trick_sims/SIM_wheelbot/images/wp3.png
+++ b/trick_sims/SIM_wheelbot/images/wp3.png
--- a/trick_sims/SIM_wheelbot/images/wp4.png
+++ b/trick_sims/SIM_wheelbot/images/wp4.png
--- a/trick_sims/SIM_wheelbot/images/wp5.png
+++ b/trick_sims/SIM_wheelbot/images/wp5.png
--- a/trick_sims/SIM_wheelbot/images/wp6.png
+++ b/trick_sims/SIM_wheelbot/images/wp6.png
--- a/trick_sims/SIM_wheelbot/images/wp7.png
+++ b/trick_sims/SIM_wheelbot/images/wp7.png
--- a/trick_sims/SIM_wheelbot/images/wp8.png
+++ b/trick_sims/SIM_wheelbot/images/wp8.png
--- a/trick_sims/SIM_wheelbot/images/wp9.png
+++ b/trick_sims/SIM_wheelbot/images/wp9.png