From 723c5a1cd781cec3f748ea27af241e4cc7c43451 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Tue, 16 Jan 2024 17:10:51 +0100
Subject: [PATCH] Improved testing setup

---
 CMakeLists.txt                                | 24 +++++++++---------
 test/BB2B_Test.cc                             | 24 +++++++++++++++---
 ...CMakeLists_disabled.txt => CMakeLists.txt} | 25 ++++++++++++-------
 3 files changed, 48 insertions(+), 25 deletions(-)
 rename test/{CMakeLists_disabled.txt => CMakeLists.txt} (66%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ca357b0..07706d3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,7 +19,7 @@ endif()
 
 project(RoboidControl)
 add_subdirectory(VectorAlgebra)
-#add_subdirectory(test)
+add_subdirectory(test)
 
 set(CMAKE_CXX_STANDARD 11)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
@@ -56,16 +56,16 @@ add_library(RoboidControl STATIC
 
 enable_testing()
 
-add_executable(
-    RoboidControlTest
-    "test/BB2B_Test.cc"
-)
-target_link_libraries(
-    RoboidControlTest
-    gtest_main
-    RoboidControl
-    VectorAlgebra
-)
+# add_executable(
+#     RoboidControlTest
+#     "test/BB2B_Test.cc"
+# )
+# target_link_libraries(
+#     RoboidControlTest
+#     gtest_main
+#     RoboidControl
+#     VectorAlgebra
+# )
 
 include(GoogleTest)
-gtest_discover_tests(RoboidControlTest)
+# gtest_discover_tests(RoboidControlTest)
diff --git a/test/BB2B_Test.cc b/test/BB2B_Test.cc
index 4b89d9c..b077998 100644
--- a/test/BB2B_Test.cc
+++ b/test/BB2B_Test.cc
@@ -1,9 +1,10 @@
 // #if GTEST
 #include <gtest/gtest.h>
 
-#include "DifferentialDrive.h"
-#include "DistanceSensor.h"
-#include "Roboid.h"
+#include "../DifferentialDrive.h"
+#include "../DistanceSensor.h"
+#include "../Roboid.h"
+#include "../VectorAlgebra/Angle.h"
 
 TEST(BB2B, Basics) {
   Motor *motorLeft = new Motor();
@@ -19,6 +20,9 @@ TEST(BB2B, Basics) {
   Perception *perception = new Perception(sensors, 2);
 
   DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
+  propulsion->SetDimensions(
+      1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s
+      1);
 
   Roboid *roboid = new Roboid(perception, propulsion);
 
@@ -31,9 +35,21 @@ TEST(BB2B, Basics) {
   float leftMotorSpeed = obstacleRight ? -1.0F : 1.0F;
   float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F;
 
+  EXPECT_FLOAT_EQ(leftMotorSpeed, 1.0F);
+  EXPECT_FLOAT_EQ(rightMotorSpeed, 1.0F);
+
   DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
   diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
 
-  // cannot chek verlocity in this branch?
+  float leftActualSpeed = motorLeft->GetActualSpeed();
+  float rightActualSpeed = motorRight->GetActualSpeed();
+
+  EXPECT_FLOAT_EQ(leftActualSpeed, 1.0F);
+  EXPECT_FLOAT_EQ(rightActualSpeed, 1.0F);
+
+  Polar velocity =
+      diffDrive->GetVelocity(); // this depends on the wheel diameter.
+  EXPECT_FLOAT_EQ(velocity.distance, 1.0F);
+  EXPECT_FLOAT_EQ(velocity.angle, 0.0F);
 }
 // #endif
diff --git a/test/CMakeLists_disabled.txt b/test/CMakeLists.txt
similarity index 66%
rename from test/CMakeLists_disabled.txt
rename to test/CMakeLists.txt
index 5cf6e5a..8a0c919 100644
--- a/test/CMakeLists_disabled.txt
+++ b/test/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.13)  # CMake version check
-Project(test)
+Project(RoboidControlTestNew)
 
 set(CMAKE_CXX_STANDARD 11)            # Enable c++11 standard
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
@@ -14,17 +14,24 @@ FetchContent_Declare(
 # For Windows: Prevent overriding the parent project's compiler/linker settings
 set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
 FetchContent_MakeAvailable(googletest)
-include_directories(include)
 
+include_directories(
+    . 
+    ..
+)
 enable_testing()
 
 add_executable(
-    VectorAlgebraTest
-    "test/Angle_test.cc"
-    "test/FloatSingle_test.cc"
-    "test/Vector2_test.cc"
-    "test/Vector3_test.cc"
-    "test/Quaternion_test.cc"
+    RoboidControlTestNew
+    "BB2B_test.cc"
 )
 
-include(GoogleTest)
\ No newline at end of file
+target_link_libraries(
+    RoboidControlTestNew
+    gtest_main
+    RoboidControl
+    VectorAlgebra
+)
+
+include(GoogleTest)
+gtest_discover_tests(RoboidControlTestNew)