From da08efe5465b9addbbb118c90239eae61d873d21 Mon Sep 17 00:00:00 2001 From: "John M. Penn" Date: Thu, 18 Feb 2016 16:14:44 -0600 Subject: [PATCH] Fixes #185 Add SIM_wheelbot --- trick_models/Wheelbot/Battery/.gitignore | 6 + .../Wheelbot/Battery/include/DCBattery.hh | 20 + trick_models/Wheelbot/Battery/makefile | 32 ++ .../Wheelbot/Battery/src/DCBattery.cpp | 23 ++ .../Wheelbot/Battery/test/DCBatteryTest.cpp | 52 +++ trick_models/Wheelbot/Battery/test/makefile | 42 ++ .../include/differentialDriveController.hh | 41 ++ .../Control/include/testMotorController.hh | 19 + .../Control/include/vehicleController.hh | 37 ++ trick_models/Wheelbot/Control/makefile | 36 ++ .../src/differentialDriveController.cpp | 75 ++++ .../Control/src/vehicleController.cpp | 62 +++ .../test/DifferentialDriveControllerTest.cpp | 172 ++++++++ .../Control/test/VehicleControllerTest.cpp | 58 +++ trick_models/Wheelbot/Control/test/makefile | 52 +++ .../Electrical/include/ElectricalCircuit.hh | 24 ++ trick_models/Wheelbot/Electrical/makefile | 31 ++ .../Electrical/src/ElectricalCircuit.cpp | 23 ++ .../Electrical/test/ElectricalCircuitTest.cpp | 0 .../Wheelbot/Electrical/test/makefile | 41 ++ trick_models/Wheelbot/Graphics/Makefile | 36 ++ .../Graphics/src/trick/EVDisplay.java | 247 ++++++++++++ .../Wheelbot/Guidance/include/arena.hh | 33 ++ .../Wheelbot/Guidance/include/findpath.hh | 5 + .../Wheelbot/Guidance/include/gridSquare.hh | 12 + .../Wheelbot/Guidance/include/navigator.hh | 35 ++ .../Wheelbot/Guidance/include/point.hh | 10 + trick_models/Wheelbot/Guidance/makefile | 36 ++ trick_models/Wheelbot/Guidance/src/arena.cpp | 201 ++++++++++ .../Wheelbot/Guidance/src/findpath.cpp | 110 +++++ .../Wheelbot/Guidance/src/navigator.cpp | 60 +++ .../Wheelbot/Guidance/test/ArenaTest.cpp | 375 ++++++++++++++++++ .../Wheelbot/Guidance/test/FindPathTest.cpp | 243 ++++++++++++ .../Wheelbot/Guidance/test/NavigatorTest.cpp | 143 +++++++ trick_models/Wheelbot/Guidance/test/makefile | 55 +++ .../Wheelbot/Motor/include/DCMotor.hh | 27 ++ .../Motor/include/DCMotorSpeedController.hh | 32 ++ trick_models/Wheelbot/Motor/include/Motor.hh | 20 + trick_models/Wheelbot/Motor/include/PWM.hh | 34 ++ .../Wheelbot/Motor/include/ServoMotor.hh | 16 + .../Motor/include/ServoSpeedController.hh | 20 + .../Motor/include/motorSpeedController.hh | 18 + trick_models/Wheelbot/Motor/makefile | 34 ++ trick_models/Wheelbot/Motor/src/DCMotor.cpp | 35 ++ .../Motor/src/DCMotorSpeedController.cpp | 29 ++ trick_models/Wheelbot/Motor/src/Motor.cpp | 5 + trick_models/Wheelbot/Motor/src/PWM.cpp | 29 ++ .../Wheelbot/Motor/src/ServoMotor.cpp | 35 ++ .../Motor/src/ServoSpeedController.cpp | 59 +++ .../Wheelbot/Motor/test/DCMotorTest.cpp | 83 ++++ trick_models/Wheelbot/Motor/test/PWMTest.cpp | 0 trick_models/Wheelbot/Motor/test/makefile | 48 +++ .../test/DifferentialDriveControllerTest.cpp | 172 ++++++++ .../Motor/test/test/VehicleControllerTest.cpp | 58 +++ .../Wheelbot/Motor/test/test/makefile | 52 +++ .../Wheelbot/Vehicle/include/vehicleOne.hh | 70 ++++ trick_models/Wheelbot/Vehicle/makefile | 33 ++ .../Wheelbot/Vehicle/src/vehicleOne.cpp | 235 +++++++++++ .../Modified_data/cross.waypoints | 12 + .../SIM_wheelbot/Modified_data/realtime.py | 10 + .../Modified_data/vehicleState.dr | 28 ++ trick_sims/SIM_wheelbot/README.md | 4 + trick_sims/SIM_wheelbot/RUN_test/input.py | 49 +++ trick_sims/SIM_wheelbot/S_define | 28 ++ trick_sims/SIM_wheelbot/S_overrides.mk | 2 + .../SIM_wheelbot/images/twoWheelRover.png | Bin 0 -> 2786 bytes trick_sims/SIM_wheelbot/images/wp0.png | Bin 0 -> 1472 bytes trick_sims/SIM_wheelbot/images/wp1.png | Bin 0 -> 1343 bytes trick_sims/SIM_wheelbot/images/wp10.png | Bin 0 -> 1501 bytes trick_sims/SIM_wheelbot/images/wp11.png | Bin 0 -> 1369 bytes trick_sims/SIM_wheelbot/images/wp2.png | Bin 0 -> 1439 bytes trick_sims/SIM_wheelbot/images/wp3.png | Bin 0 -> 1473 bytes trick_sims/SIM_wheelbot/images/wp4.png | Bin 0 -> 1406 bytes trick_sims/SIM_wheelbot/images/wp5.png | Bin 0 -> 1462 bytes trick_sims/SIM_wheelbot/images/wp6.png | Bin 0 -> 1493 bytes trick_sims/SIM_wheelbot/images/wp7.png | Bin 0 -> 1383 bytes trick_sims/SIM_wheelbot/images/wp8.png | Bin 0 -> 1486 bytes trick_sims/SIM_wheelbot/images/wp9.png | Bin 0 -> 1483 bytes 78 files changed, 3724 insertions(+) create mode 100644 trick_models/Wheelbot/Battery/.gitignore create mode 100644 trick_models/Wheelbot/Battery/include/DCBattery.hh create mode 100644 trick_models/Wheelbot/Battery/makefile create mode 100644 trick_models/Wheelbot/Battery/src/DCBattery.cpp create mode 100644 trick_models/Wheelbot/Battery/test/DCBatteryTest.cpp create mode 100644 trick_models/Wheelbot/Battery/test/makefile create mode 100644 trick_models/Wheelbot/Control/include/differentialDriveController.hh create mode 100644 trick_models/Wheelbot/Control/include/testMotorController.hh create mode 100644 trick_models/Wheelbot/Control/include/vehicleController.hh create mode 100644 trick_models/Wheelbot/Control/makefile create mode 100644 trick_models/Wheelbot/Control/src/differentialDriveController.cpp create mode 100644 trick_models/Wheelbot/Control/src/vehicleController.cpp create mode 100644 trick_models/Wheelbot/Control/test/DifferentialDriveControllerTest.cpp create mode 100644 trick_models/Wheelbot/Control/test/VehicleControllerTest.cpp create mode 100644 trick_models/Wheelbot/Control/test/makefile create mode 100644 trick_models/Wheelbot/Electrical/include/ElectricalCircuit.hh create mode 100644 trick_models/Wheelbot/Electrical/makefile create mode 100644 trick_models/Wheelbot/Electrical/src/ElectricalCircuit.cpp create mode 100644 trick_models/Wheelbot/Electrical/test/ElectricalCircuitTest.cpp create mode 100644 trick_models/Wheelbot/Electrical/test/makefile create mode 100644 trick_models/Wheelbot/Graphics/Makefile create mode 100644 trick_models/Wheelbot/Graphics/src/trick/EVDisplay.java create mode 100644 trick_models/Wheelbot/Guidance/include/arena.hh create mode 100644 trick_models/Wheelbot/Guidance/include/findpath.hh create mode 100644 trick_models/Wheelbot/Guidance/include/gridSquare.hh create mode 100644 trick_models/Wheelbot/Guidance/include/navigator.hh create mode 100644 trick_models/Wheelbot/Guidance/include/point.hh create mode 100644 trick_models/Wheelbot/Guidance/makefile create mode 100644 trick_models/Wheelbot/Guidance/src/arena.cpp create mode 100644 trick_models/Wheelbot/Guidance/src/findpath.cpp create mode 100644 trick_models/Wheelbot/Guidance/src/navigator.cpp create mode 100644 trick_models/Wheelbot/Guidance/test/ArenaTest.cpp create mode 100644 trick_models/Wheelbot/Guidance/test/FindPathTest.cpp create mode 100644 trick_models/Wheelbot/Guidance/test/NavigatorTest.cpp create mode 100644 trick_models/Wheelbot/Guidance/test/makefile create mode 100644 trick_models/Wheelbot/Motor/include/DCMotor.hh create mode 100644 trick_models/Wheelbot/Motor/include/DCMotorSpeedController.hh create mode 100644 trick_models/Wheelbot/Motor/include/Motor.hh create mode 100644 trick_models/Wheelbot/Motor/include/PWM.hh create mode 100644 trick_models/Wheelbot/Motor/include/ServoMotor.hh create mode 100644 trick_models/Wheelbot/Motor/include/ServoSpeedController.hh create mode 100644 trick_models/Wheelbot/Motor/include/motorSpeedController.hh create mode 100644 trick_models/Wheelbot/Motor/makefile create mode 100644 trick_models/Wheelbot/Motor/src/DCMotor.cpp create mode 100644 trick_models/Wheelbot/Motor/src/DCMotorSpeedController.cpp create mode 100644 trick_models/Wheelbot/Motor/src/Motor.cpp create mode 100644 trick_models/Wheelbot/Motor/src/PWM.cpp create mode 100644 trick_models/Wheelbot/Motor/src/ServoMotor.cpp create mode 100644 trick_models/Wheelbot/Motor/src/ServoSpeedController.cpp create mode 100644 trick_models/Wheelbot/Motor/test/DCMotorTest.cpp create mode 100644 trick_models/Wheelbot/Motor/test/PWMTest.cpp create mode 100644 trick_models/Wheelbot/Motor/test/makefile create mode 100644 trick_models/Wheelbot/Motor/test/test/DifferentialDriveControllerTest.cpp create mode 100644 trick_models/Wheelbot/Motor/test/test/VehicleControllerTest.cpp create mode 100644 trick_models/Wheelbot/Motor/test/test/makefile create mode 100644 trick_models/Wheelbot/Vehicle/include/vehicleOne.hh create mode 100644 trick_models/Wheelbot/Vehicle/makefile create mode 100644 trick_models/Wheelbot/Vehicle/src/vehicleOne.cpp create mode 100644 trick_sims/SIM_wheelbot/Modified_data/cross.waypoints create mode 100644 trick_sims/SIM_wheelbot/Modified_data/realtime.py create mode 100644 trick_sims/SIM_wheelbot/Modified_data/vehicleState.dr create mode 100644 trick_sims/SIM_wheelbot/README.md create mode 100644 trick_sims/SIM_wheelbot/RUN_test/input.py create mode 100644 trick_sims/SIM_wheelbot/S_define create mode 100644 trick_sims/SIM_wheelbot/S_overrides.mk create mode 100644 trick_sims/SIM_wheelbot/images/twoWheelRover.png create mode 100644 trick_sims/SIM_wheelbot/images/wp0.png create mode 100644 trick_sims/SIM_wheelbot/images/wp1.png create mode 100644 trick_sims/SIM_wheelbot/images/wp10.png create mode 100644 trick_sims/SIM_wheelbot/images/wp11.png create mode 100644 trick_sims/SIM_wheelbot/images/wp2.png create mode 100644 trick_sims/SIM_wheelbot/images/wp3.png create mode 100644 trick_sims/SIM_wheelbot/images/wp4.png create mode 100644 trick_sims/SIM_wheelbot/images/wp5.png create mode 100644 trick_sims/SIM_wheelbot/images/wp6.png create mode 100644 trick_sims/SIM_wheelbot/images/wp7.png create mode 100644 trick_sims/SIM_wheelbot/images/wp8.png create mode 100644 trick_sims/SIM_wheelbot/images/wp9.png 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 + + +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 +#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 +#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* waypointQueue, + Navigator& navigator, + DifferentialDriveController& driveController, + double arrival_distance); + + int getCurrentDestination(Point& currentDestination); + void setWayPointQueue( std::vector* waypointQueue ); + void printDestination(); + void update(); + + private: + // Do not allow the default constructor to be used. + VehicleController(); + + std::vector* waypointQueue; + std::vector::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 +#include +#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 + +VehicleController::VehicleController( std::vector* 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* 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 +#define private public +#include "Control/include/testMotorController.hh" +#include "Control/include/differentialDriveController.hh" +#include + +#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 +#define private public +#include "Control/include/testMotorController.hh" +#include "Control/include/vehicleController.hh" +#include + +#ifndef PI +#define PI 3.1415926535 +#endif +#define FLOAT_TOLERANCE 0.000001 + +/* +Test Fixture. +*/ +class vehicleControllerTest : public ::testing::Test { + protected: + std::vector 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 +#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 featureList; + private double unitsPerPixel; + + public ArenaMap(List 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 \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 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 +#include +#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 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 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 // sqrt() +#include // abs() +#include + +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> 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 Arena::getNeighbors( GridSquare* gridSquarePointer ) { + + std::vector 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 +#include // std::vector +#include // std::find +#include "findpath.hh" + +std::vector FindPath( GridSquare* origin, GridSquare* goal, Arena* arena) { + + std::vector openSet; + std::vector closedSet; + std::vector 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::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 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 neighbors = arena->getNeighbors( current ); + + // For each of the neighbors + std::vector::iterator neighborsIterator; + for ( neighborsIterator=neighbors.begin(); + neighborsIterator!=neighbors.end(); + ++neighborsIterator) { + + GridSquare* neighbor = *neighborsIterator; + + // Search for the neighbor in the closed set. + std::vector::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::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 // for: sqrt(), atan2(), cos(), and sin() +#include "navigator.hh" +#include + +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 +#define private public +#include "arena.hh" +#include + +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 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 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 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 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::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 +#include "findpath.hh" +#include "arena.hh" +#include +#include + +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 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 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 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 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 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 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 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 +#define private public +#include "navigator.hh" +#include + +#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 + // 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 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 + +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 +#include + + +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 + +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 +#include + +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 < +#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 +#define private public +#include "Control/include/testMotorController.hh" +#include "Control/include/differentialDriveController.hh" +#include + +#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 +#define private public +#include "Control/include/testMotorController.hh" +#include "Control/include/vehicleController.hh" +#include + +#ifndef PI +#define PI 3.1415926535 +#endif +#define FLOAT_TOLERANCE 0.000001 + +/* +Test Fixture. +*/ +class vehicleControllerTest : public ::testing::Test { + protected: + std::vector 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 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 +#include +#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 0000000000000000000000000000000000000000..5a2622d68dbe54574499a7470a8522b693f5402f GIT binary patch literal 2786 zcmV<83LW){P)4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER000JgNklA?rWbgNCDCyzK!rk;H^>Fwu<(97r@#0fhrZK6YSe zm~QK-nYZb#=^3hb%HhMVO*7r~@z(P`^;A{&TT2wDn1Psqn1TN>0|~>vyN2zBnKO#n zq%rsT^XJsp*GH|bt&~hA!@vjfUjCY;r*GNRoxKd?s}G4pg3iy+X?uH{rl+S-Yz((4 zL+--D0*#K2QYMp8@mG9)@xu{~e~_hjIvXn1Q42OUHfUmE0@+5eUq4JLg7Rl~Zf=go z#>OH!Uv5S(XJ}}M78e&$*~oM=K>!U53{X>3lb#0#Y7Gt!8n99{VT6G8_I6`{bPQpQ zF_nxEfJN%ACL;vgRp)^_Cg3iQo(b$J9V23!b4Bv1_V)JD=X zxtV5WX0-5W5pZ^PCXIsxEO-Y82VQ5l;pXJzB!G|c>R{6%fZ2ufAc3ECI;ck-#>4oc z>`Gb$Fr(Z-LqqV@6{N5_>ftc@@+TFaaRLg3g6xl-WXAmE6_C&81OD8$DWKvDfW78HO97XcSKsT9m%0mVZEaCcPY+p^C3lIiKGW%R z#WJQAVEo)FOZ-#+{ddc`H$_a7N2J-%~PG ze6du4JnMK$ov9S1#HEuE{P^p5mwwCt9;jaI-y!G1p^lCY0#Ue_0G%LAz@T{0Lc|gS z0PfUp=#STb(l@_+A+5>pBIOli_}Rx8#K19{Tz5=qLJe<8yzui~zWf$P({7 zpIm+_%TV|9^i)QmK!8eq&_MZ<76GgPG{-$g$e;4L;)@blB3E%9^=$i`rlzK7XJ^OX zjE?dSecPC%l5o5-M3$OA?(m)iH$r!=;}TCxyaZu@pe|Ne6cV%u@GHPi#uc#Q3sNG? zdr=9CCc;>}Uvs|?s%r@kYj@3@mR+=7W-lIg5kf0?bpr{~~v}DxC^Q3h1^YJk~em6sJKL`>4`|T%f z)YkGS&;%pz3i-QMK~5V&wFPF>F*RQ_v%6L$K4TOZ*^C~MhS_H6kVHllYOB0F5 z*|l7YGFFDNsU|5y$WbR*4S8y8ObdHK1ypJaN0X@p6)dqW3-S<%#q`?T+>}$`KU)M@ zVjut)I`Sv%#rOn8NF0Dvh4}0G@2v)#(oJa*P;K;QSG))LAQwa|EiIv*9EAS`_ML4zl3FP!J*Z{CTAB_gb6{Yg=~$ z;5aUP!k*?{)9&uB#1-WWF3gcP&8s^B019Uj#BfBI8^IP1Y25?~btM1};)26C&vQhW z+qUzPjGJ&CgHcxk&><2CKoG+bmu*7{S3%8s6Jdk^j>r_d z)zxank;+UEkeHpFrInQx(is^Jp5P>@qw@g`Jbol2%IaxTm={pV+S(eSKam1)%CP!6 zd0J6n4MPe1Z%u^DJ3Bk&sH%}$*x)(Ni=chpO?!VI(&axFffJ>$w%9cgj1~U_6sC+g zZ`2TsiR7-YuhahizPGRyv#4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER00043NklhXyk#CoYs7~|7eTCkJB}k_AGX|?#%)6Y}w`fyP(b+HssKXG&bKA7Vs|$hmqE@Bp zofNT-GKWS>lGK@OuIG{k^P%&mKPCDmyztk$FS#DDg1oLL3$l?DY{j$}`630RG1acO zoQl0g)Ao4<==*pEFVe35-)RC}ctjm%_a3Ri;vTwRyNPS~A2_#tA(tlfLfjKg1-bbj z(IvtT?87yv`*j$~`idZ0nB0ROv-q+zowr~YMq&yWGAQUG2YawJ=t9)v1mXnZ1mXlH adjh{lL|g)V{A4Hq00004Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER0002iNklV64iWBjfO-TrV;uRTy5*rNs0y17fIUitx*l2Pc@E?oqAR%N&V6zWHjGQRIupDG6 z)IsR(!mtosfE)+F%>a3qWG};YLl_h}0L?72xa6h*uw^8NHx?(769SCbf*mS`O$v*( z2qAJq0OSbt03g|Ngfm82qaiRF0;3@?8UiCZ1OQv76eqvnPy_$~002ovPDHLkV1h0D BZc_jN literal 0 HcmV?d00001 diff --git a/trick_sims/SIM_wheelbot/images/wp10.png b/trick_sims/SIM_wheelbot/images/wp10.png new file mode 100644 index 0000000000000000000000000000000000000000..504b111f7552f2b09f1b678e2d73375878ebc958 GIT binary patch literal 1501 zcmV<31tR*1P)4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER0004WNklY5QZ<}M-Usa5Cj{G2>yc< zejtdJ_7;+#U}a}%<^Ql2?5+F(3L=PD2!gQ?6$B9>J`-kP+1pLL-CX4kJnY@h>^raH z-K8i-6I;miU_T&ts`(GrPrW`4)c$#Un#wgegck@I*wfq*>o`opiSC6AlO7)1lt@z> zpWqWU*N&VRd56m}CsF10g5r6l6F4}_{sNpk`5*4@pq*mi8vz-;@CLqU>E^vBDFzxT z20WUjt!3SLEoF+LZ>nws6WVynW1!!=ZUfKSc*$d6PWI|H@G!uDJRjY<4cyY?26kWu zOvszD0q)yj@|D+JPRRGb_Z=`FqBHVs*n}(S!Xk_$1{shB2k-@AQh0<_ND3on)?s2z z3f5u8E^OF06|Q^I)eaS^tXKH}O-s#4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER0002+NklM0Lqo)k*7$? z6{!0^5Xa*%1?0dZKztu2&jMpmKyv~0ngFpS5EtVxM+GQu0K~CCYz@Q@fS78)9cU8B z*T@(>1dgN0!$W|H3eMOMG(#MU6LFa5g{0;xvKnec!7reJub_s0KsFqS{T_+^2Z_x< zjSxUKk7V|!14ct&z(atNYzQiNkO8QeB^i*V324iI9JZ5e9?2T8Sq}3y)G(6Fq$mx5 zY$C}vm`jmpiX4Ds4%tjf5+S(yBim-EMsh=NKocR%(>O8$OnQ_a4S~@R7!85Z5Eu;s b;z9rbTFW6Y(aasq00000NkvXXu0mjfV=sY| literal 0 HcmV?d00001 diff --git a/trick_sims/SIM_wheelbot/images/wp2.png b/trick_sims/SIM_wheelbot/images/wp2.png new file mode 100644 index 0000000000000000000000000000000000000000..ccbf4d449537e7ffb908382fa99a8176c131bac0 GIT binary patch literal 1439 zcmV;Q1z`G#P)4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER0003tNklgcEp%2lxQ*I;KJdEBJ!Xcs^&m0Pnh^!Vla)8RSL}6ng~4 zylP7JaY}^|F}ZT4XTG{PB6kf2D&lAsul0%wNw=`sCRu|`3(!Yc+2%mjo)wi7uu;wA zsl~$=_=fAejhmh&3fqlvFs+F?@jfi!6*^Ez>juc8g6`T*-e&Ozvi%mSP*`q3cWnw> zF_GO9I5ok2jO&87{hE_p7(Fr3?ez*Wwd=1~O(Ya2GWqnizrzqBNYgz*{S|kXxv?4T tL|qmpo;9A^2f{o!5F7{&1PA`F1HbULbWPSmIbHw&002ovPDHLkV1i8;vt4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER00044NklY?XGb3e{Af)r}6(1#1i!8zQ4#&iX- z;!hK(+4I-Ydo+3N3H_XO3iM(AM8pV#?U}1xFib0ng3kOtyu%adKj6bV;2rP|cn7=# bzYdH6J1b`?VTv1T00000NkvXXu0mjf&`Y~7 literal 0 HcmV?d00001 diff --git a/trick_sims/SIM_wheelbot/images/wp4.png b/trick_sims/SIM_wheelbot/images/wp4.png new file mode 100644 index 0000000000000000000000000000000000000000..74ea4681bfab460f4ba10856e26e1977b5b9393c GIT binary patch literal 1406 zcmV-^1%djBP)4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER0003MNkl!4ov=?sGG$89g5ck7HfD9&N4h`Aip4z#cUh?RkO6%dyIF$l0jC@Zl zm{*W(fr??1LRN;sCN~N|jzEukk}bz@&8Wa=2#kinXb6mkz=#Y10L%D6vM@5j6#xJL M07*qoM6N<$f?Yw0Y5)KL literal 0 HcmV?d00001 diff --git a/trick_sims/SIM_wheelbot/images/wp5.png b/trick_sims/SIM_wheelbot/images/wp5.png new file mode 100644 index 0000000000000000000000000000000000000000..f411fab66fd6d870f5607e0afbc12a5990ae0cb8 GIT binary patch literal 1462 zcmV;n1xfmeP)4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER0003^NklJ5QY~54H6QOH~_D-R8Xei z3e?aBg$r;6B*Y~+K<)s2x`YA>sDO$Ffe;0o@0aYAGaDD|qrxM7vOTtEX8#>I%aSDp z{<{K8Eeg;f_DLlE1phd)*O+^YNO^=ypJl~QS{QpS<`LtoiU01`Z5FUcl5ey?;L7bPLw%_ zrof?{u{aJL%-7)wK4BFq*<9kV5Te;47A3fL=k~Q(s|WOoMP9N^whlpGY;D6i)EAYk zP=MR|V!Kw*zT3OdA~4CWW_*K4?ZN5Dv`84ruQ}(1Oo{G6|60AU3kNU)of$waD-}UB zpWFxDW^(KNzPK;eLw`VHWf{0@6UJicLE3>Acz_>BmJ~<|Bn6TJNr6Iv8BghOgsQ+8 Q5dZ)H07*qoM6N<$f|($=5C8xG literal 0 HcmV?d00001 diff --git a/trick_sims/SIM_wheelbot/images/wp6.png b/trick_sims/SIM_wheelbot/images/wp6.png new file mode 100644 index 0000000000000000000000000000000000000000..beee900ef99565e44e389ab9533bf4040635b843 GIT binary patch literal 1493 zcmV;`1uFW9P)4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER0004ONklPoR4;Tf)B08g+1xoedQ?{Eh5Faulg25rb) zEh^}57h{zU@f)#NF77>9Prc0QLPE2N$an%p(k#Oz)S(9-a0UA?md&pX<%8=~!P3|b zXalMf2hah<^-&{fK$Zw(Q(~)44EcRn0jmWJb>$0|6wI;&Dx8ps*`WtB;hbmBoBWBb z&8a0=S3JuH=)GU*T!|Fjn%HB?n2w>O`yrED>v>y$9QSR~E{5&_ZAf1iIB9Wz&Y!j` zNcD)#TXqSQpl19g&nm3JiHRNY97&PF`Vg}D#clE(4Bh*7Tx~xFcMOLtAyfWWyx_20 vx(RhGbrES4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER0002~Nkl2tBUJF$88i@IcQH)Q`f1vFs7^oQJAbh|IC<#)Ng-;PNawrxfI|OP0E;BX( zC7u8=3oZr3io+cM)By4kIv<-k+Cag7K-_{&l2kEt3(@)5%$o@m1UWznnSVk^C%V%VfWg^?T(@5g2?xd{T~2z1|Kvm9inE7aoS p*c>n_HW~t>Aut*OqaiRF0ssPMT*qSqzGDCY002ovPDHLkV1iL1f35%k literal 0 HcmV?d00001 diff --git a/trick_sims/SIM_wheelbot/images/wp8.png b/trick_sims/SIM_wheelbot/images/wp8.png new file mode 100644 index 0000000000000000000000000000000000000000..e7f376bf84c9cba4e9fbf949bd9f0e97f114474a GIT binary patch literal 1486 zcmV;<1u^=GP)4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER0004HNkldCwB-UYvVl(ZUs+TW|^MP=yD00&Og}be2A$VIKqa zV2##qK|kbD8>bF%lew=`I{Lfm;?2P_*bVIwcdd@Z{zh=Lo0vB7NsvW%?%vFeX)f0# z)GLmMc4tY*@RsBu?gI4a%((6aBRjkCv*hcpa4Tx05}naRo`#hR1`jmZ&IWdKOk5~hl<6oRa0BJ8yc;~21%2p?MfD<>DVeH z9(p*dx19w`~g7O0}n_%Aq@s%d)fBDv`JHkDym6Hd+5XuAtvnwRpGmK zVkc9?T=n|PIo~X-eVh__(Z?q}P9Z-Dj?gOW6|D%o20XmjW-qs4UjrD(li^iv8@eK9k+ZFm zVRFymFOPAzG5-%Pn|1W;U4vNroTa&AxDScmEA~{ri9gr1^c?U@uwSpaNnw8l_>cP1 zd;)kMQS_;jeRSUEM_*s96y65j1$)tOrwdK{YIQMt92l|D^(E_=$Rjw{b!QT@q!)ni zR`|5oW9X5n$Wv+HVc@|^eX5yXnsHX8PF3UX~a6)MwxDE0HaPjyrlI!;jX{6Kvuh*8ej?;85ekN$?5uuCiS zBTvvVG+XTxAO{m@bvM#Jr)z6J><&E22D|vq?Y?Vkbo_DijopiF$2PET#mZ8eu=y$(ArYkv7@Ex`GL?QCc!_*KFrd&;n1r7 zqW-CFs9&fT)ZaU5gc&=gBz-DaCw(vdOp0__x+47~U6sC(E(JNe@4cTT*n6*E zVH4eoU1-&7pEV~_PRe`a7v+@vy!^5}8?Y3)UmlaER0004ENkl2im;@T{$$MpBk&M2IKGqkC01DQso*d@C+aJyvdqNoKoclHlP=ZunRiK zBD6Ggd)@rNRT(DN0lxK4xA~vxgqa;W(>wBWn3mqHpBBZv`Kl8>1`srmJ$|E+TD)a7baM-e?8k4L;!n%J2$jpuc01Wd>CEN0{cf_IPm? zwn2rDU=D8K3Nl6Y=6tP!J`bk4TmgQYvoiV)WC~x<=fQN82OV%L2(~VnUUK@M(`rH7 zQ_p5xYji>K01KdrpNAXRho&M`SO>vP_?SYt1np@M-{BaBpy>!j!&=W!GcW_oFj()U lsK*Y(4#W<`4zzCvz5r)KcgEG|hLZpQ002ovPDHLkV1f#=#Cre$ literal 0 HcmV?d00001