diff --git a/test/BB2B_Test.cc b/test/BB2B_Test.cc index 241dc38..742a213 100644 --- a/test/BB2B_Test.cc +++ b/test/BB2B_Test.cc @@ -1,5 +1,7 @@ // #if GTEST +// #include +// not supported using Visual Studio 2022 compiler... #include #include "../DifferentialDrive.h" @@ -312,4 +314,102 @@ TEST(BB2B, ObstacleRight) { #pragma endregion } +TEST(BB2B, ObstacleBoth) { +#pragma region Setup + + Motor *motorLeft = new Motor(); + Motor *motorRight = new Motor(); + + MockDistanceSensor *sensorLeft = new MockDistanceSensor(); + sensorLeft->position.angle = -30; + MockDistanceSensor *sensorRight = new MockDistanceSensor(); + sensorRight->position.angle = 30; + + Sensor *sensors[] = {sensorLeft, sensorRight}; + + 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 + 8 / Angle::pi); // we use this, such that angular velocity will be 90 + // degrees per second + + Roboid *roboid = new Roboid(perception, propulsion); + +#pragma endregion + +#pragma region Sensor Measurement - left and right + + sensorLeft->SimulateMeasurement(0.1F); + sensorRight->SimulateMeasurement(0.1F); + + roboid->Update(0.1F); + +#pragma endregion + +#pragma region Test Sensor output + // Distance + float distanceLeft = sensorLeft->GetDistance(); + float distanceRight = sensorRight->GetDistance(); + + EXPECT_FLOAT_EQ(distanceLeft, 0.1F); + EXPECT_FLOAT_EQ(distanceRight, 0.1F); + + // Obstacle + bool obstacleLeft = roboid->perception->ObjectNearby(-30); + bool obstacleRight = roboid->perception->ObjectNearby(30); + + EXPECT_TRUE(obstacleLeft); + EXPECT_TRUE(obstacleRight); + + // Tracked objects + int trackedObjectCount = roboid->perception->TrackedObjectCount(); + EXPECT_EQ(trackedObjectCount, 2); + + // Find the single tracked object + TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects(); + for (int i = 0; i < roboid->perception->maxObjectCount; i++) { + if (trackedObjects[i] != nullptr) { + EXPECT_FLOAT_EQ(trackedObjects[i]->position.distance, 0.1F); + // EXPECT_THAT(trackedObjects[i] ->position.angle, AnyOf(FloatEq(-30), + // FloatEq(30)); + } + } + +#pragma endregion + +#pragma region Motor Control + + // Motor speeds + 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); + +#pragma endregion + +#pragma region Test motor output results + + float leftActualSpeed = motorLeft->GetActualSpeed(); + float rightActualSpeed = motorRight->GetActualSpeed(); + + EXPECT_FLOAT_EQ(leftActualSpeed, -1.0F); + EXPECT_FLOAT_EQ(rightActualSpeed, -1.0F); + + // Roboid velocity + Polar velocity = diffDrive->GetVelocity(); + EXPECT_FLOAT_EQ(velocity.distance, 1.0F); + EXPECT_FLOAT_EQ(velocity.angle, 180.0F); + + float angularVelocity = diffDrive->GetAngularVelocity(); + EXPECT_FLOAT_EQ(angularVelocity, 0.0F); + +#pragma endregion +} + // #endif