RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Updated spherical direction and unit tests

Browse Source
This commit is contained in:
Pascal Serrarens 2024-09-24 12:26:03 +02:00
parent 84654af2c3
commit dc9d1cd80f
9 changed files with 47 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
DifferentialDrive.cpp
View File

@ -14,10 +14,10 @@ DifferentialDrive::DifferentialDrive(Motor* leftMotor, Motor* rightMotor) {
float distance = this->wheelSeparation / 2; float distance = this->wheelSeparation / 2;
leftMotor->direction = Motor::Direction::CounterClockwise; leftMotor->direction = Motor::Direction::CounterClockwise;
leftMotor->position.horizontal = Angle16::Degrees(-90); leftMotor->position.direction.horizontal = Angle16::Degrees(-90);
leftMotor->position.distance = distance; leftMotor->position.distance = distance;
rightMotor->direction = Motor::Direction::Clockwise; rightMotor->direction = Motor::Direction::Clockwise;
rightMotor->position.horizontal = Angle16::Degrees(90); rightMotor->position.direction.horizontal = Angle16::Degrees(90);
rightMotor->position.distance = distance; rightMotor->position.distance = distance;
} }
@ -39,7 +39,8 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
if (motor == nullptr) if (motor == nullptr)
continue; continue;
float xPosition = motors[motorIx]->position.horizontal.InDegrees(); float xPosition =
motors[motorIx]->position.direction.horizontal.InDegrees();
if (xPosition < 0) if (xPosition < 0)
motor->SetTargetSpeed(leftSpeed); motor->SetTargetSpeed(leftSpeed);
else if (xPosition > 0) else if (xPosition > 0)

2
LinearAlgebra

@ -1 +1 @@
Subproject commit 136e44e0001f382b6378cefa4cf17573d0b0dd98 Subproject commit 9eca3189918ecfbaae462427619c263f636b6d9d

34
Perception.cpp
View File

@ -79,8 +79,9 @@ float GetPlaneDistance(InterestingThing* plane,
float range) { float range) {
float distance = plane->position.distance; float distance = plane->position.distance;
float deltaAngle = float deltaAngle =
Angle::Normalize(Angle::Degrees(plane->position.horizontal.InDegrees() - Angle::Normalize(
horizontalAngle)) Angle::Degrees(plane->position.direction.horizontal.InDegrees() -
horizontalAngle))
.InDegrees(); .InDegrees();
if (fabsf(deltaAngle) < fabsf(range)) { if (fabsf(deltaAngle) < fabsf(range)) {
// distance = distance // distance = distance
@ -125,9 +126,9 @@ float Perception::GetDistance(float horizontalDirection, float range) {
if (obj->type == 0x080) { // plane if (obj->type == 0x080) { // plane
float planeDistance = GetPlaneDistance(obj, horizontalDirection, range); float planeDistance = GetPlaneDistance(obj, horizontalDirection, range);
minDistance = fminf(minDistance, planeDistance); minDistance = fminf(minDistance, planeDistance);
} else if (obj->position.horizontal.InDegrees() > } else if (obj->position.direction.horizontal.InDegrees() >
horizontalDirection - range && horizontalDirection - range &&
obj->position.horizontal.InDegrees() < obj->position.direction.horizontal.InDegrees() <
horizontalDirection + range) { horizontalDirection + range) {
minDistance = fminf(minDistance, obj->position.distance); minDistance = fminf(minDistance, obj->position.distance);
} }
@ -152,9 +153,9 @@ float Perception::GetDistanceOfType(unsigned char thingType,
if (thing->type == 0x080) { // plane if (thing->type == 0x080) { // plane
float planeDistance = GetPlaneDistance(thing, horizontalAngle, range); float planeDistance = GetPlaneDistance(thing, horizontalAngle, range);
minDistance = fminf(minDistance, planeDistance); minDistance = fminf(minDistance, planeDistance);
} else if (thing->position.horizontal.InDegrees() > } else if (thing->position.direction.horizontal.InDegrees() >
horizontalAngle - range && horizontalAngle - range &&
thing->position.horizontal.InDegrees() < thing->position.direction.horizontal.InDegrees() <
horizontalAngle + range) { horizontalAngle + range) {
minDistance = fminf(minDistance, thing->position.distance); minDistance = fminf(minDistance, thing->position.distance);
} }
@ -173,8 +174,10 @@ float Perception::GetDistance(float horizontalDirection,
InterestingThing* obj = trackedObjects[objIx]; InterestingThing* obj = trackedObjects[objIx];
if (obj == nullptr) if (obj == nullptr)
continue; continue;
if (obj->position.horizontal.InDegrees() > horizontalDirection - range && if (obj->position.direction.horizontal.InDegrees() >
obj->position.horizontal.InDegrees() < horizontalDirection + range) { horizontalDirection - range &&
obj->position.direction.horizontal.InDegrees() <
horizontalDirection + range) {
minDistance = fminf(minDistance, obj->position.distance); minDistance = fminf(minDistance, obj->position.distance);
} }
} }
@ -190,8 +193,8 @@ bool Perception::ObjectNearby(float direction, float range) {
if (obj == nullptr) if (obj == nullptr)
continue; continue;
if (obj->position.horizontal.InDegrees() > direction - range && if (obj->position.direction.horizontal.InDegrees() > direction - range &&
obj->position.horizontal.InDegrees() < direction + range) { obj->position.direction.horizontal.InDegrees() < direction + range) {
if (obj->position.distance <= nearbyDistance) if (obj->position.distance <= nearbyDistance)
return true; return true;
} }
@ -402,8 +405,8 @@ void Perception::Update(unsigned long currentTimeMs) {
// Polar position = Polar(angle, distance); // Polar position = Polar(angle, distance);
// Polar position = Polar(distance, angle.ToFloat()); // Polar position = Polar(distance, angle.ToFloat());
// AddTrackedObject(distanceSensor, position); // AddTrackedObject(distanceSensor, position);
Spherical16 position = Spherical16( Spherical16 position =
distance, sensor->position.horizontal, sensor->position.vertical); Spherical16(distance, sensor->position.direction);
AddTrackedObject(distanceSensor, position); 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++) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing* thing = trackedObjects[thingIx]; InterestingThing* thing = trackedObjects[thingIx];
if (thing == nullptr) if (thing == nullptr)
@ -449,7 +452,7 @@ void Perception::UpdatePose(Polar translation) {
// (float)thing->position.horizontalAngle, // (float)thing->position.horizontalAngle,
// (float)thing->position.verticalAngle); // (float)thing->position.verticalAngle);
// Update the closest point to the plane // 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(); translation.angle.InDegrees();
angle = fabsf(angle); angle = fabsf(angle);
@ -495,7 +498,8 @@ void Perception::UpdatePose(SwingTwist16 rotation) {
// (float)thing->position.verticalAngle); // (float)thing->position.verticalAngle);
// printf("| rotate %f | ", rotationAngle); // 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, // printf("-> %f (%f %f) \n", thing->position.distance,
// (float)thing->position.horizontalAngle, // (float)thing->position.horizontalAngle,

2
Perception.h
View File

@ -149,7 +149,7 @@ class Perception {
/// @details This function will be called through Roboid::SetPosition. It /// @details This function will be called through Roboid::SetPosition. It
/// is advised to use that function to update the roboid position instead of /// is advised to use that function to update the roboid position instead of
/// this function. /// this function.
void UpdatePose(Polar translation); void UpdatePose(Polar16 translation);
/// @brief Update the orientation of the perceived objecst from the given /// @brief Update the orientation of the perceived objecst from the given
/// roboid rotation /// roboid rotation
/// @param rotation The rotation of the roboid in world space /// @param rotation The rotation of the roboid in world space

3
Roboid.cpp
View File

@ -99,7 +99,8 @@ void Roboid::SetPosition(Spherical16 newWorldPosition) {
float distance = translation.distance; float distance = translation.distance;
Angle16 angle = Spherical16::SignedAngleBetween( Angle16 angle = Spherical16::SignedAngleBetween(
roboidOrientation * Spherical16::forward, translation, Spherical16::up); 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) if (perception != nullptr)
perception->UpdatePose(polarTranslation); perception->UpdatePose(polarTranslation);
this->worldPosition = newWorldPosition; this->worldPosition = newWorldPosition;

2
ServoMotor.h
View File

@ -1,7 +1,7 @@
#pragma once #pragma once
#include "ControlledMotor.h" #include "ControlledMotor.h"
#include "LinearAlgebra/Angle16.h" #include "LinearAlgebra/Angle.h"
namespace Passer { namespace Passer {
namespace RoboidContol { namespace RoboidContol {

8
TrackedObject.cpp
View File

@ -46,11 +46,11 @@ bool InterestingThing::IsTheSameAs(InterestingThing* otherObj) {
// (float)position.horizontalAngle, (float)position.verticalAngle); // (float)position.horizontalAngle, (float)position.verticalAngle);
if (fabsf(position.distance - otherObj->position.distance) > equalDistance) if (fabsf(position.distance - otherObj->position.distance) > equalDistance)
return false; return false;
if (fabsf(position.horizontal.InDegrees() - if (fabsf(position.direction.horizontal.InDegrees() -
otherObj->position.horizontal.InDegrees()) > equalAngle) otherObj->position.direction.horizontal.InDegrees()) > equalAngle)
return false; return false;
if (fabsf(position.vertical.InDegrees() - if (fabsf(position.direction.vertical.InDegrees() -
otherObj->position.vertical.InDegrees()) > equalAngle) otherObj->position.direction.vertical.InDegrees()) > equalAngle)
return false; return false;
// printf(" -> yes "); // printf(" -> yes ");
return true; return true;

2
TrackedObject.h
View File

@ -1,6 +1,6 @@
#pragma once #pragma once
#include "LinearAlgebra/Angle16.h" #include "LinearAlgebra/Angle.h"
#include "LinearAlgebra/AngleAxis.h" #include "LinearAlgebra/AngleAxis.h"
#include "LinearAlgebra/Polar.h" #include "LinearAlgebra/Polar.h"
#include "LinearAlgebra/Quaternion.h" #include "LinearAlgebra/Quaternion.h"

26
test/BB2B_Test.cc
View File

@ -35,11 +35,11 @@ TEST(BB2B, NoObstacle) {
Roboid* roboid = new Roboid(propulsion); Roboid* roboid = new Roboid(propulsion);
MockDistanceSensor* sensorLeft = new MockDistanceSensor(10.0F); MockDistanceSensor* sensorLeft = new MockDistanceSensor(10.0F);
sensorLeft->position.horizontal = -30; sensorLeft->position.direction.horizontal = Angle16::Degrees(-30);
roboid->AddChild(sensorLeft); roboid->AddChild(sensorLeft);
MockDistanceSensor* sensorRight = new MockDistanceSensor(10.0F); MockDistanceSensor* sensorRight = new MockDistanceSensor(10.0F);
sensorRight->SetParent(roboid); sensorRight->SetParent(roboid);
sensorRight->position.horizontal = 30; sensorRight->position.direction.horizontal = Angle16::Degrees(30);
roboid->perception->nearbyDistance = 0.2f; roboid->perception->nearbyDistance = 0.2f;
@ -100,7 +100,7 @@ TEST(BB2B, NoObstacle) {
Polar velocity = Polar velocity =
diffDrive->GetVelocity(); // this depends on the wheel diameter. diffDrive->GetVelocity(); // this depends on the wheel diameter.
EXPECT_FLOAT_EQ(velocity.distance, 1.0F); 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(); trackedObjectCount = roboid->perception->TrackedObjectCount();
EXPECT_EQ(trackedObjectCount, 0); EXPECT_EQ(trackedObjectCount, 0);
@ -132,11 +132,11 @@ TEST(BB2B, ObstacleLeft) {
Roboid* roboid = new Roboid(propulsion); Roboid* roboid = new Roboid(propulsion);
MockDistanceSensor* sensorLeft = new MockDistanceSensor(); MockDistanceSensor* sensorLeft = new MockDistanceSensor();
sensorLeft->position.horizontal = -30; sensorLeft->position.direction.horizontal = Angle16::Degrees(-30);
roboid->AddChild(sensorLeft); roboid->AddChild(sensorLeft);
MockDistanceSensor* sensorRight = new MockDistanceSensor(); MockDistanceSensor* sensorRight = new MockDistanceSensor();
sensorRight->SetParent(roboid); sensorRight->SetParent(roboid);
sensorRight->position.horizontal = 30; sensorRight->position.direction.horizontal = Angle16::Degrees(30);
roboid->perception->nearbyDistance = 0.2f; roboid->perception->nearbyDistance = 0.2f;
@ -180,7 +180,8 @@ TEST(BB2B, ObstacleLeft) {
ASSERT_FALSE(trackedObject == nullptr); ASSERT_FALSE(trackedObject == nullptr);
EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F); 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 #pragma endregion
@ -235,11 +236,11 @@ TEST(BB2B, ObstacleRight) {
Roboid* roboid = new Roboid(propulsion); Roboid* roboid = new Roboid(propulsion);
MockDistanceSensor* sensorLeft = new MockDistanceSensor(); MockDistanceSensor* sensorLeft = new MockDistanceSensor();
sensorLeft->position.horizontal = -30; sensorLeft->position.direction.horizontal = Angle16::Degrees(-30);
roboid->AddChild(sensorLeft); roboid->AddChild(sensorLeft);
MockDistanceSensor* sensorRight = new MockDistanceSensor(); MockDistanceSensor* sensorRight = new MockDistanceSensor();
sensorRight->SetParent(roboid); sensorRight->SetParent(roboid);
sensorRight->position.horizontal = 30; sensorRight->position.direction.horizontal = Angle16::Degrees(30);
roboid->perception->nearbyDistance = 0.2f; roboid->perception->nearbyDistance = 0.2f;
@ -283,7 +284,8 @@ TEST(BB2B, ObstacleRight) {
ASSERT_FALSE(trackedObject == nullptr); ASSERT_FALSE(trackedObject == nullptr);
EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F); 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 #pragma endregion
@ -334,11 +336,11 @@ TEST(BB2B, ObstacleBoth) {
Roboid* roboid = new Roboid(propulsion); Roboid* roboid = new Roboid(propulsion);
MockDistanceSensor* sensorLeft = new MockDistanceSensor(); MockDistanceSensor* sensorLeft = new MockDistanceSensor();
sensorLeft->position.horizontal = -30; sensorLeft->position.direction.horizontal = Angle16::Degrees(-30);
roboid->AddChild(sensorLeft); roboid->AddChild(sensorLeft);
MockDistanceSensor* sensorRight = new MockDistanceSensor(); MockDistanceSensor* sensorRight = new MockDistanceSensor();
sensorRight->SetParent(roboid); sensorRight->SetParent(roboid);
sensorRight->position.horizontal = 30; sensorRight->position.direction.horizontal = Angle16::Degrees(30);
roboid->perception->nearbyDistance = 0.2f; roboid->perception->nearbyDistance = 0.2f;
@ -409,7 +411,7 @@ TEST(BB2B, ObstacleBoth) {
// Roboid velocity // Roboid velocity
Polar velocity = diffDrive->GetVelocity(); Polar velocity = diffDrive->GetVelocity();
EXPECT_FLOAT_EQ(velocity.distance, 1.0F); 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(); float angularVelocity = diffDrive->GetAngularVelocity();
EXPECT_FLOAT_EQ(angularVelocity, 0.0F); EXPECT_FLOAT_EQ(angularVelocity, 0.0F);