diff --git a/trick_models/Wheelbot/Battery/.gitignore b/trick_models/Wheelbot/Battery/.gitignore
new file mode 100644
index 00000000..c4f4b009
--- /dev/null
+++ b/trick_models/Wheelbot/Battery/.gitignore
@@ -0,0 +1,6 @@
+# Ignore compiled objects and libraries
+lib
+obj
+*.o
+test/BatteryTest
+XMLtestReports
diff --git a/trick_models/Wheelbot/Battery/include/DCBattery.hh b/trick_models/Wheelbot/Battery/include/DCBattery.hh
new file mode 100644
index 00000000..52a4aa1b
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Battery/makefile b/trick_models/Wheelbot/Battery/makefile
new file mode 100644
index 00000000..3ae3ce03
--- /dev/null
+++ 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}
+
diff --git a/trick_models/Wheelbot/Battery/src/DCBattery.cpp b/trick_models/Wheelbot/Battery/src/DCBattery.cpp
new file mode 100644
index 00000000..adc89066
--- /dev/null
+++ 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;
+}
diff --git a/trick_models/Wheelbot/Battery/test/DCBatteryTest.cpp b/trick_models/Wheelbot/Battery/test/DCBatteryTest.cpp
new file mode 100644
index 00000000..c0df790a
--- /dev/null
+++ 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 );
+
+}
diff --git a/trick_models/Wheelbot/Battery/test/makefile b/trick_models/Wheelbot/Battery/test/makefile
new file mode 100644
index 00000000..14af12b1
--- /dev/null
+++ 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)
diff --git a/trick_models/Wheelbot/Control/include/differentialDriveController.hh b/trick_models/Wheelbot/Control/include/differentialDriveController.hh
new file mode 100644
index 00000000..50e75ef4
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Control/include/testMotorController.hh b/trick_models/Wheelbot/Control/include/testMotorController.hh
new file mode 100644
index 00000000..dba39630
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Control/include/vehicleController.hh b/trick_models/Wheelbot/Control/include/vehicleController.hh
new file mode 100644
index 00000000..d94ea5c6
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Control/makefile b/trick_models/Wheelbot/Control/makefile
new file mode 100644
index 00000000..3910168e
--- /dev/null
+++ 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}
+
diff --git a/trick_models/Wheelbot/Control/src/differentialDriveController.cpp b/trick_models/Wheelbot/Control/src/differentialDriveController.cpp
new file mode 100644
index 00000000..e149f67d
--- /dev/null
+++ 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;
+}
diff --git a/trick_models/Wheelbot/Control/src/vehicleController.cpp b/trick_models/Wheelbot/Control/src/vehicleController.cpp
new file mode 100644
index 00000000..51d9324d
--- /dev/null
+++ 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();
+ }
+ }
+}
+
+
diff --git a/trick_models/Wheelbot/Control/test/DifferentialDriveControllerTest.cpp b/trick_models/Wheelbot/Control/test/DifferentialDriveControllerTest.cpp
new file mode 100644
index 00000000..7c9e2fc2
--- /dev/null
+++ 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);
+}
+
+
diff --git a/trick_models/Wheelbot/Control/test/VehicleControllerTest.cpp b/trick_models/Wheelbot/Control/test/VehicleControllerTest.cpp
new file mode 100644
index 00000000..bfd57b03
--- /dev/null
+++ 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);
+}
+
diff --git a/trick_models/Wheelbot/Control/test/makefile b/trick_models/Wheelbot/Control/test/makefile
new file mode 100644
index 00000000..c863277a
--- /dev/null
+++ 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)
diff --git a/trick_models/Wheelbot/Electrical/include/ElectricalCircuit.hh b/trick_models/Wheelbot/Electrical/include/ElectricalCircuit.hh
new file mode 100644
index 00000000..181ecd36
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Electrical/makefile b/trick_models/Wheelbot/Electrical/makefile
new file mode 100644
index 00000000..2d5044f1
--- /dev/null
+++ 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}
diff --git a/trick_models/Wheelbot/Electrical/src/ElectricalCircuit.cpp b/trick_models/Wheelbot/Electrical/src/ElectricalCircuit.cpp
new file mode 100644
index 00000000..a3b5e5c3
--- /dev/null
+++ 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);
+
+}
diff --git a/trick_models/Wheelbot/Electrical/test/ElectricalCircuitTest.cpp b/trick_models/Wheelbot/Electrical/test/ElectricalCircuitTest.cpp
new file mode 100644
index 00000000..e69de29b
diff --git a/trick_models/Wheelbot/Electrical/test/makefile b/trick_models/Wheelbot/Electrical/test/makefile
new file mode 100644
index 00000000..6feffff6
--- /dev/null
+++ 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)
+
\ No newline at end of file
diff --git a/trick_models/Wheelbot/Graphics/Makefile b/trick_models/Wheelbot/Graphics/Makefile
new file mode 100644
index 00000000..d89ed460
--- /dev/null
+++ 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 "-------------------------------------------------------------------------------"
diff --git a/trick_models/Wheelbot/Graphics/src/trick/EVDisplay.java b/trick_models/Wheelbot/Graphics/src/trick/EVDisplay.java
new file mode 100644
index 00000000..35cb7f12
--- /dev/null
+++ 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();
+ }
+ }
+}
diff --git a/trick_models/Wheelbot/Guidance/include/arena.hh b/trick_models/Wheelbot/Guidance/include/arena.hh
new file mode 100644
index 00000000..0b7f5f60
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Guidance/include/findpath.hh b/trick_models/Wheelbot/Guidance/include/findpath.hh
new file mode 100644
index 00000000..318e8aa0
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Guidance/include/gridSquare.hh b/trick_models/Wheelbot/Guidance/include/gridSquare.hh
new file mode 100644
index 00000000..32a28790
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Guidance/include/navigator.hh b/trick_models/Wheelbot/Guidance/include/navigator.hh
new file mode 100644
index 00000000..cb6e5c7d
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Guidance/include/point.hh b/trick_models/Wheelbot/Guidance/include/point.hh
new file mode 100644
index 00000000..a3bcfa7c
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Guidance/makefile b/trick_models/Wheelbot/Guidance/makefile
new file mode 100644
index 00000000..c6cdd70e
--- /dev/null
+++ 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}
+
diff --git a/trick_models/Wheelbot/Guidance/src/arena.cpp b/trick_models/Wheelbot/Guidance/src/arena.cpp
new file mode 100644
index 00000000..d92e6b09
--- /dev/null
+++ 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;
+}
diff --git a/trick_models/Wheelbot/Guidance/src/findpath.cpp b/trick_models/Wheelbot/Guidance/src/findpath.cpp
new file mode 100644
index 00000000..bf524bb6
--- /dev/null
+++ 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;
+}
diff --git a/trick_models/Wheelbot/Guidance/src/navigator.cpp b/trick_models/Wheelbot/Guidance/src/navigator.cpp
new file mode 100644
index 00000000..45fd57e0
--- /dev/null
+++ 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);
+}
diff --git a/trick_models/Wheelbot/Guidance/test/ArenaTest.cpp b/trick_models/Wheelbot/Guidance/test/ArenaTest.cpp
new file mode 100644
index 00000000..1902d7a6
--- /dev/null
+++ 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);
+
+
+}
\ No newline at end of file
diff --git a/trick_models/Wheelbot/Guidance/test/FindPathTest.cpp b/trick_models/Wheelbot/Guidance/test/FindPathTest.cpp
new file mode 100644
index 00000000..46b0febc
--- /dev/null
+++ 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);
+}*/
\ No newline at end of file
diff --git a/trick_models/Wheelbot/Guidance/test/NavigatorTest.cpp b/trick_models/Wheelbot/Guidance/test/NavigatorTest.cpp
new file mode 100644
index 00000000..4c0ad8db
--- /dev/null
+++ 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);
+}
diff --git a/trick_models/Wheelbot/Guidance/test/makefile b/trick_models/Wheelbot/Guidance/test/makefile
new file mode 100644
index 00000000..8372803b
--- /dev/null
+++ 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)
diff --git a/trick_models/Wheelbot/Motor/include/DCMotor.hh b/trick_models/Wheelbot/Motor/include/DCMotor.hh
new file mode 100644
index 00000000..e32592f5
--- /dev/null
+++ 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
+
diff --git a/trick_models/Wheelbot/Motor/include/DCMotorSpeedController.hh b/trick_models/Wheelbot/Motor/include/DCMotorSpeedController.hh
new file mode 100644
index 00000000..5506be0f
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Motor/include/Motor.hh b/trick_models/Wheelbot/Motor/include/Motor.hh
new file mode 100644
index 00000000..a87cbc6a
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Motor/include/PWM.hh b/trick_models/Wheelbot/Motor/include/PWM.hh
new file mode 100644
index 00000000..3c677ff6
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Motor/include/ServoMotor.hh b/trick_models/Wheelbot/Motor/include/ServoMotor.hh
new file mode 100644
index 00000000..d0c65fee
--- /dev/null
+++ 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
\ No newline at end of file
diff --git a/trick_models/Wheelbot/Motor/include/ServoSpeedController.hh b/trick_models/Wheelbot/Motor/include/ServoSpeedController.hh
new file mode 100644
index 00000000..58398541
--- /dev/null
+++ 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
\ No newline at end of file
diff --git a/trick_models/Wheelbot/Motor/include/motorSpeedController.hh b/trick_models/Wheelbot/Motor/include/motorSpeedController.hh
new file mode 100644
index 00000000..2a2b5365
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Motor/makefile b/trick_models/Wheelbot/Motor/makefile
new file mode 100644
index 00000000..59abe5b4
--- /dev/null
+++ 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}
diff --git a/trick_models/Wheelbot/Motor/src/DCMotor.cpp b/trick_models/Wheelbot/Motor/src/DCMotor.cpp
new file mode 100644
index 00000000..ebe82c68
--- /dev/null
+++ 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;
+}
diff --git a/trick_models/Wheelbot/Motor/src/DCMotorSpeedController.cpp b/trick_models/Wheelbot/Motor/src/DCMotorSpeedController.cpp
new file mode 100644
index 00000000..381f51ea
--- /dev/null
+++ 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;
+}
diff --git a/trick_models/Wheelbot/Motor/src/Motor.cpp b/trick_models/Wheelbot/Motor/src/Motor.cpp
new file mode 100644
index 00000000..1369c0f0
--- /dev/null
+++ b/trick_models/Wheelbot/Motor/src/Motor.cpp
@@ -0,0 +1,5 @@
+#include "Motor.hh"
+
+Motor::Motor () {}
+Motor::~Motor() {}
+double Motor::getActualSpeed() {return 0;}
diff --git a/trick_models/Wheelbot/Motor/src/PWM.cpp b/trick_models/Wheelbot/Motor/src/PWM.cpp
new file mode 100644
index 00000000..75196d4c
--- /dev/null
+++ 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);
+}
diff --git a/trick_models/Wheelbot/Motor/src/ServoMotor.cpp b/trick_models/Wheelbot/Motor/src/ServoMotor.cpp
new file mode 100644
index 00000000..27c08e2e
--- /dev/null
+++ 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;
+}
\ No newline at end of file
diff --git a/trick_models/Wheelbot/Motor/src/ServoSpeedController.cpp b/trick_models/Wheelbot/Motor/src/ServoSpeedController.cpp
new file mode 100644
index 00000000..50b36b1d
--- /dev/null
+++ 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;
+}
\ No newline at end of file
diff --git a/trick_models/Wheelbot/Motor/test/DCMotorTest.cpp b/trick_models/Wheelbot/Motor/test/DCMotorTest.cpp
new file mode 100644
index 00000000..ff6b21cf
--- /dev/null
+++ 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 );
+
+}
diff --git a/trick_models/Wheelbot/Motor/test/PWMTest.cpp b/trick_models/Wheelbot/Motor/test/PWMTest.cpp
new file mode 100644
index 00000000..e69de29b
diff --git a/trick_models/Wheelbot/Motor/test/makefile b/trick_models/Wheelbot/Motor/test/makefile
new file mode 100644
index 00000000..54e63538
--- /dev/null
+++ 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)
diff --git a/trick_models/Wheelbot/Motor/test/test/DifferentialDriveControllerTest.cpp b/trick_models/Wheelbot/Motor/test/test/DifferentialDriveControllerTest.cpp
new file mode 100644
index 00000000..7c9e2fc2
--- /dev/null
+++ 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);
+}
+
+
diff --git a/trick_models/Wheelbot/Motor/test/test/VehicleControllerTest.cpp b/trick_models/Wheelbot/Motor/test/test/VehicleControllerTest.cpp
new file mode 100644
index 00000000..bfd57b03
--- /dev/null
+++ 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);
+}
+
diff --git a/trick_models/Wheelbot/Motor/test/test/makefile b/trick_models/Wheelbot/Motor/test/test/makefile
new file mode 100644
index 00000000..c863277a
--- /dev/null
+++ 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)
diff --git a/trick_models/Wheelbot/Vehicle/include/vehicleOne.hh b/trick_models/Wheelbot/Vehicle/include/vehicleOne.hh
new file mode 100644
index 00000000..beda347b
--- /dev/null
+++ 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
diff --git a/trick_models/Wheelbot/Vehicle/makefile b/trick_models/Wheelbot/Vehicle/makefile
new file mode 100644
index 00000000..2c710f4a
--- /dev/null
+++ 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}
+
diff --git a/trick_models/Wheelbot/Vehicle/src/vehicleOne.cpp b/trick_models/Wheelbot/Vehicle/src/vehicleOne.cpp
new file mode 100644
index 00000000..4f1207a7
--- /dev/null
+++ 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);
+
+}
diff --git a/trick_sims/SIM_wheelbot/Modified_data/cross.waypoints b/trick_sims/SIM_wheelbot/Modified_data/cross.waypoints
new file mode 100644
index 00000000..5fe919ac
--- /dev/null
+++ 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
diff --git a/trick_sims/SIM_wheelbot/Modified_data/realtime.py b/trick_sims/SIM_wheelbot/Modified_data/realtime.py
new file mode 100644
index 00000000..559070ed
--- /dev/null
+++ 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)
diff --git a/trick_sims/SIM_wheelbot/Modified_data/vehicleState.dr b/trick_sims/SIM_wheelbot/Modified_data/vehicleState.dr
new file mode 100644
index 00000000..5ccdee63
--- /dev/null
+++ 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()
diff --git a/trick_sims/SIM_wheelbot/README.md b/trick_sims/SIM_wheelbot/README.md
new file mode 100644
index 00000000..a54a0f9a
--- /dev/null
+++ b/trick_sims/SIM_wheelbot/README.md
@@ -0,0 +1,4 @@
+#SIM\_rover
+---
+This is a simulation of a two-wheeled robotic electric vehicle.
+
diff --git a/trick_sims/SIM_wheelbot/RUN_test/input.py b/trick_sims/SIM_wheelbot/RUN_test/input.py
new file mode 100644
index 00000000..cf426ab9
--- /dev/null
+++ 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)
diff --git a/trick_sims/SIM_wheelbot/S_define b/trick_sims/SIM_wheelbot/S_define
new file mode 100644
index 00000000..201e27c5
--- /dev/null
+++ 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;
diff --git a/trick_sims/SIM_wheelbot/S_overrides.mk b/trick_sims/SIM_wheelbot/S_overrides.mk
new file mode 100644
index 00000000..4acb9a1e
--- /dev/null
+++ 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
diff --git a/trick_sims/SIM_wheelbot/images/twoWheelRover.png b/trick_sims/SIM_wheelbot/images/twoWheelRover.png
new file mode 100644
index 00000000..5a2622d6
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/twoWheelRover.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp0.png b/trick_sims/SIM_wheelbot/images/wp0.png
new file mode 100644
index 00000000..c7fb54b4
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp0.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp1.png b/trick_sims/SIM_wheelbot/images/wp1.png
new file mode 100644
index 00000000..ce828ed8
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp1.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp10.png b/trick_sims/SIM_wheelbot/images/wp10.png
new file mode 100644
index 00000000..504b111f
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp10.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp11.png b/trick_sims/SIM_wheelbot/images/wp11.png
new file mode 100644
index 00000000..53d26eb3
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp11.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp2.png b/trick_sims/SIM_wheelbot/images/wp2.png
new file mode 100644
index 00000000..ccbf4d44
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp2.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp3.png b/trick_sims/SIM_wheelbot/images/wp3.png
new file mode 100644
index 00000000..14e44780
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp3.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp4.png b/trick_sims/SIM_wheelbot/images/wp4.png
new file mode 100644
index 00000000..74ea4681
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp4.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp5.png b/trick_sims/SIM_wheelbot/images/wp5.png
new file mode 100644
index 00000000..f411fab6
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp5.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp6.png b/trick_sims/SIM_wheelbot/images/wp6.png
new file mode 100644
index 00000000..beee900e
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp6.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp7.png b/trick_sims/SIM_wheelbot/images/wp7.png
new file mode 100644
index 00000000..95ce3a8c
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp7.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp8.png b/trick_sims/SIM_wheelbot/images/wp8.png
new file mode 100644
index 00000000..e7f376bf
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp8.png differ
diff --git a/trick_sims/SIM_wheelbot/images/wp9.png b/trick_sims/SIM_wheelbot/images/wp9.png
new file mode 100644
index 00000000..d6b98d36
Binary files /dev/null and b/trick_sims/SIM_wheelbot/images/wp9.png differ