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;
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)

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 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,

2
Perception.h
View File

@ -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

3
Roboid.cpp
View File

@ -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;

2
ServoMotor.h
View File

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

8
TrackedObject.cpp
View File

@ -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;

2
TrackedObject.h
View File

@ -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"

26
test/BB2B_Test.cc
View File

@ -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);