From dc9d1cd80fe37285f1c83987bb8dffe9f1cebf44 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 24 Sep 2024 12:26:03 +0200 Subject: [PATCH] Updated spherical direction and unit tests --- DifferentialDrive.cpp | 7 ++++--- LinearAlgebra | 2 +- Perception.cpp | 34 +++++++++++++++++++--------------- Perception.h | 2 +- Roboid.cpp | 3 ++- ServoMotor.h | 2 +- TrackedObject.cpp | 8 ++++---- TrackedObject.h | 2 +- test/BB2B_Test.cc | 26 ++++++++++++++------------ 9 files changed, 47 insertions(+), 39 deletions(-) diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index db67a68..03e7bfd 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -14,10 +14,10 @@ DifferentialDrive::DifferentialDrive(Motor* leftMotor, Motor* rightMotor) { float distance = this->wheelSeparation / 2; leftMotor->direction = Motor::Direction::CounterClockwise; - leftMotor->position.horizontal = Angle16::Degrees(-90); + leftMotor->position.direction.horizontal = Angle16::Degrees(-90); leftMotor->position.distance = distance; rightMotor->direction = Motor::Direction::Clockwise; - rightMotor->position.horizontal = Angle16::Degrees(90); + rightMotor->position.direction.horizontal = Angle16::Degrees(90); rightMotor->position.distance = distance; } @@ -39,7 +39,8 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, if (motor == nullptr) continue; - float xPosition = motors[motorIx]->position.horizontal.InDegrees(); + float xPosition = + motors[motorIx]->position.direction.horizontal.InDegrees(); if (xPosition < 0) motor->SetTargetSpeed(leftSpeed); else if (xPosition > 0) diff --git a/LinearAlgebra b/LinearAlgebra index 136e44e..9eca318 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit 136e44e0001f382b6378cefa4cf17573d0b0dd98 +Subproject commit 9eca3189918ecfbaae462427619c263f636b6d9d diff --git a/Perception.cpp b/Perception.cpp index b75639d..98d878e 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -79,8 +79,9 @@ float GetPlaneDistance(InterestingThing* plane, float range) { float distance = plane->position.distance; float deltaAngle = - Angle::Normalize(Angle::Degrees(plane->position.horizontal.InDegrees() - - horizontalAngle)) + Angle::Normalize( + Angle::Degrees(plane->position.direction.horizontal.InDegrees() - + horizontalAngle)) .InDegrees(); if (fabsf(deltaAngle) < fabsf(range)) { // distance = distance @@ -125,9 +126,9 @@ float Perception::GetDistance(float horizontalDirection, float range) { if (obj->type == 0x080) { // plane float planeDistance = GetPlaneDistance(obj, horizontalDirection, range); minDistance = fminf(minDistance, planeDistance); - } else if (obj->position.horizontal.InDegrees() > + } else if (obj->position.direction.horizontal.InDegrees() > horizontalDirection - range && - obj->position.horizontal.InDegrees() < + obj->position.direction.horizontal.InDegrees() < horizontalDirection + range) { minDistance = fminf(minDistance, obj->position.distance); } @@ -152,9 +153,9 @@ float Perception::GetDistanceOfType(unsigned char thingType, if (thing->type == 0x080) { // plane float planeDistance = GetPlaneDistance(thing, horizontalAngle, range); minDistance = fminf(minDistance, planeDistance); - } else if (thing->position.horizontal.InDegrees() > + } else if (thing->position.direction.horizontal.InDegrees() > horizontalAngle - range && - thing->position.horizontal.InDegrees() < + thing->position.direction.horizontal.InDegrees() < horizontalAngle + range) { minDistance = fminf(minDistance, thing->position.distance); } @@ -173,8 +174,10 @@ float Perception::GetDistance(float horizontalDirection, InterestingThing* obj = trackedObjects[objIx]; if (obj == nullptr) continue; - if (obj->position.horizontal.InDegrees() > horizontalDirection - range && - obj->position.horizontal.InDegrees() < horizontalDirection + range) { + if (obj->position.direction.horizontal.InDegrees() > + horizontalDirection - range && + obj->position.direction.horizontal.InDegrees() < + horizontalDirection + range) { minDistance = fminf(minDistance, obj->position.distance); } } @@ -190,8 +193,8 @@ bool Perception::ObjectNearby(float direction, float range) { if (obj == nullptr) continue; - if (obj->position.horizontal.InDegrees() > direction - range && - obj->position.horizontal.InDegrees() < direction + range) { + if (obj->position.direction.horizontal.InDegrees() > direction - range && + obj->position.direction.horizontal.InDegrees() < direction + range) { if (obj->position.distance <= nearbyDistance) return true; } @@ -402,8 +405,8 @@ void Perception::Update(unsigned long currentTimeMs) { // Polar position = Polar(angle, distance); // Polar position = Polar(distance, angle.ToFloat()); // AddTrackedObject(distanceSensor, position); - Spherical16 position = Spherical16( - distance, sensor->position.horizontal, sensor->position.vertical); + Spherical16 position = + Spherical16(distance, sensor->position.direction); AddTrackedObject(distanceSensor, position); } @@ -436,7 +439,7 @@ void Perception::Update(unsigned long currentTimeMs) { } } -void Perception::UpdatePose(Polar translation) { +void Perception::UpdatePose(Polar16 translation) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { InterestingThing* thing = trackedObjects[thingIx]; if (thing == nullptr) @@ -449,7 +452,7 @@ void Perception::UpdatePose(Polar translation) { // (float)thing->position.horizontalAngle, // (float)thing->position.verticalAngle); // Update the closest point to the plane - float angle = (float)thing->position.horizontal.InDegrees() + + float angle = (float)thing->position.direction.horizontal.InDegrees() + translation.angle.InDegrees(); angle = fabsf(angle); @@ -495,7 +498,8 @@ void Perception::UpdatePose(SwingTwist16 rotation) { // (float)thing->position.verticalAngle); // printf("| rotate %f | ", rotationAngle); - thing->position.horizontal = thing->position.horizontal - rotationAngle; + thing->position.direction.horizontal = + thing->position.direction.horizontal - rotationAngle; // printf("-> %f (%f %f) \n", thing->position.distance, // (float)thing->position.horizontalAngle, diff --git a/Perception.h b/Perception.h index 5b8d961..44dbd1d 100644 --- a/Perception.h +++ b/Perception.h @@ -149,7 +149,7 @@ class Perception { /// @details This function will be called through Roboid::SetPosition. It /// is advised to use that function to update the roboid position instead of /// this function. - void UpdatePose(Polar translation); + void UpdatePose(Polar16 translation); /// @brief Update the orientation of the perceived objecst from the given /// roboid rotation /// @param rotation The rotation of the roboid in world space diff --git a/Roboid.cpp b/Roboid.cpp index 031c7af..766b056 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -99,7 +99,8 @@ void Roboid::SetPosition(Spherical16 newWorldPosition) { float distance = translation.distance; Angle16 angle = Spherical16::SignedAngleBetween( roboidOrientation * Spherical16::forward, translation, Spherical16::up); - Polar polarTranslation = Polar(angle.InDegrees(), Angle::Degrees(distance)); + Polar16 polarTranslation = Polar16( + distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance)); if (perception != nullptr) perception->UpdatePose(polarTranslation); this->worldPosition = newWorldPosition; diff --git a/ServoMotor.h b/ServoMotor.h index 4282818..569338f 100644 --- a/ServoMotor.h +++ b/ServoMotor.h @@ -1,7 +1,7 @@ #pragma once #include "ControlledMotor.h" -#include "LinearAlgebra/Angle16.h" +#include "LinearAlgebra/Angle.h" namespace Passer { namespace RoboidContol { diff --git a/TrackedObject.cpp b/TrackedObject.cpp index 03652fe..69cae01 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -46,11 +46,11 @@ bool InterestingThing::IsTheSameAs(InterestingThing* otherObj) { // (float)position.horizontalAngle, (float)position.verticalAngle); if (fabsf(position.distance - otherObj->position.distance) > equalDistance) return false; - if (fabsf(position.horizontal.InDegrees() - - otherObj->position.horizontal.InDegrees()) > equalAngle) + if (fabsf(position.direction.horizontal.InDegrees() - + otherObj->position.direction.horizontal.InDegrees()) > equalAngle) return false; - if (fabsf(position.vertical.InDegrees() - - otherObj->position.vertical.InDegrees()) > equalAngle) + if (fabsf(position.direction.vertical.InDegrees() - + otherObj->position.direction.vertical.InDegrees()) > equalAngle) return false; // printf(" -> yes "); return true; diff --git a/TrackedObject.h b/TrackedObject.h index 06da9c7..bf6dcaf 100644 --- a/TrackedObject.h +++ b/TrackedObject.h @@ -1,6 +1,6 @@ #pragma once -#include "LinearAlgebra/Angle16.h" +#include "LinearAlgebra/Angle.h" #include "LinearAlgebra/AngleAxis.h" #include "LinearAlgebra/Polar.h" #include "LinearAlgebra/Quaternion.h" diff --git a/test/BB2B_Test.cc b/test/BB2B_Test.cc index fe3ef89..0cbb919 100644 --- a/test/BB2B_Test.cc +++ b/test/BB2B_Test.cc @@ -35,11 +35,11 @@ TEST(BB2B, NoObstacle) { Roboid* roboid = new Roboid(propulsion); MockDistanceSensor* sensorLeft = new MockDistanceSensor(10.0F); - sensorLeft->position.horizontal = -30; + sensorLeft->position.direction.horizontal = Angle16::Degrees(-30); roboid->AddChild(sensorLeft); MockDistanceSensor* sensorRight = new MockDistanceSensor(10.0F); sensorRight->SetParent(roboid); - sensorRight->position.horizontal = 30; + sensorRight->position.direction.horizontal = Angle16::Degrees(30); roboid->perception->nearbyDistance = 0.2f; @@ -100,7 +100,7 @@ TEST(BB2B, NoObstacle) { Polar velocity = diffDrive->GetVelocity(); // this depends on the wheel diameter. EXPECT_FLOAT_EQ(velocity.distance, 1.0F); - EXPECT_FLOAT_EQ(velocity.angle.ToFloat(), 0.0F); + EXPECT_FLOAT_EQ(velocity.angle.InDegrees(), 0.0F); trackedObjectCount = roboid->perception->TrackedObjectCount(); EXPECT_EQ(trackedObjectCount, 0); @@ -132,11 +132,11 @@ TEST(BB2B, ObstacleLeft) { Roboid* roboid = new Roboid(propulsion); MockDistanceSensor* sensorLeft = new MockDistanceSensor(); - sensorLeft->position.horizontal = -30; + sensorLeft->position.direction.horizontal = Angle16::Degrees(-30); roboid->AddChild(sensorLeft); MockDistanceSensor* sensorRight = new MockDistanceSensor(); sensorRight->SetParent(roboid); - sensorRight->position.horizontal = 30; + sensorRight->position.direction.horizontal = Angle16::Degrees(30); roboid->perception->nearbyDistance = 0.2f; @@ -180,7 +180,8 @@ TEST(BB2B, ObstacleLeft) { ASSERT_FALSE(trackedObject == nullptr); EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F); - EXPECT_NEAR(trackedObject->position.horizontal.ToFloat(), -30, 1.0e-02); + EXPECT_NEAR(trackedObject->position.direction.horizontal.InDegrees(), -30, + 1.0e-02); #pragma endregion @@ -235,11 +236,11 @@ TEST(BB2B, ObstacleRight) { Roboid* roboid = new Roboid(propulsion); MockDistanceSensor* sensorLeft = new MockDistanceSensor(); - sensorLeft->position.horizontal = -30; + sensorLeft->position.direction.horizontal = Angle16::Degrees(-30); roboid->AddChild(sensorLeft); MockDistanceSensor* sensorRight = new MockDistanceSensor(); sensorRight->SetParent(roboid); - sensorRight->position.horizontal = 30; + sensorRight->position.direction.horizontal = Angle16::Degrees(30); roboid->perception->nearbyDistance = 0.2f; @@ -283,7 +284,8 @@ TEST(BB2B, ObstacleRight) { ASSERT_FALSE(trackedObject == nullptr); EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F); - EXPECT_NEAR(trackedObject->position.horizontal.ToFloat(), 30, 1.0e-02); + EXPECT_NEAR(trackedObject->position.direction.horizontal.InDegrees(), 30, + 1.0e-02); #pragma endregion @@ -334,11 +336,11 @@ TEST(BB2B, ObstacleBoth) { Roboid* roboid = new Roboid(propulsion); MockDistanceSensor* sensorLeft = new MockDistanceSensor(); - sensorLeft->position.horizontal = -30; + sensorLeft->position.direction.horizontal = Angle16::Degrees(-30); roboid->AddChild(sensorLeft); MockDistanceSensor* sensorRight = new MockDistanceSensor(); sensorRight->SetParent(roboid); - sensorRight->position.horizontal = 30; + sensorRight->position.direction.horizontal = Angle16::Degrees(30); roboid->perception->nearbyDistance = 0.2f; @@ -409,7 +411,7 @@ TEST(BB2B, ObstacleBoth) { // Roboid velocity Polar velocity = diffDrive->GetVelocity(); EXPECT_FLOAT_EQ(velocity.distance, 1.0F); - EXPECT_FLOAT_EQ(velocity.angle.ToFloat(), 180.0F); + EXPECT_FLOAT_EQ(velocity.angle.InDegrees(), 180.0F); float angularVelocity = diffDrive->GetAngularVelocity(); EXPECT_FLOAT_EQ(angularVelocity, 0.0F);