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

Bugfixes

Browse Source
This commit is contained in:
Pascal Serrarens 2024-05-14 11:58:42 +02:00
parent aa3f7a3351
commit b81b236ef4
3 changed files with 16 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LinearAlgebra

@ -1 +1 @@
Subproject commit ee62cbae3ec63b7847df242e02b453a43defa5de Subproject commit 78468e38cd8d398f36213f8205b87ad41f798f74

22
Perception.cpp
View File

@ -74,7 +74,7 @@ float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
float distance = plane->position.distance; float distance = plane->position.distance;
float deltaAngle = Angle::Normalize((float)plane->position.horizontalAngle - float deltaAngle = Angle::Normalize((float)plane->position.horizontalAngle -
horizontalAngle); horizontalAngle);
if (abs(deltaAngle) < abs(range)) { if (fabsf(deltaAngle) < fabsf(range)) {
// distance = distance // distance = distance
// printf(" plane distance = %f (%f-%f)+%f=%f", distance, // printf(" plane distance = %f (%f-%f)+%f=%f", distance,
// (float)plane->position.horizontalAngle, horizontalAngle, range, // (float)plane->position.horizontalAngle, horizontalAngle, range,
@ -188,7 +188,7 @@ bool Perception::ObjectNearby(float direction, float range) {
return false; return false;
} }
#include <WifiSync.h> // #include <WifiSync.h>
void Perception::AddTrackedObject(Sensor *sensor, Polar position, void Perception::AddTrackedObject(Sensor *sensor, Polar position,
unsigned char thingType, unsigned char thingType,
unsigned char networkId) { unsigned char networkId) {
@ -235,8 +235,10 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
printf("%d: new tracked object [%d/%d]\n", availableSlotIx, obj->networkId, printf("%d: new tracked object [%d/%d]\n", availableSlotIx, obj->networkId,
obj->id); obj->id);
#endif #endif
if (roboid->networkSync != nullptr) {
roboid->networkSync->NewObject(obj); roboid->networkSync->NewObject(obj);
((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj); // ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
}
} }
// If this object is closer than the farthest object, then replace it // If this object is closer than the farthest object, then replace it
else if (obj->position.distance < else if (obj->position.distance <
@ -250,8 +252,10 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
// Serial.println(": replaced tracked object"); // Serial.println(": replaced tracked object");
printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id); printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id);
#endif #endif
if (roboid->networkSync != nullptr) {
roboid->networkSync->NewObject(obj); roboid->networkSync->NewObject(obj);
((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj); // ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
}
} else { } else {
#ifdef RC_DEBUG2 #ifdef RC_DEBUG2
// Serial.print((int)obj->id); // Serial.print((int)obj->id);
@ -418,17 +422,17 @@ unsigned char Perception::ThingsOfType(unsigned char thingType,
if (thing == nullptr) if (thing == nullptr)
continue; continue;
printf("[%d/%d]%d ", thing->networkId, thing->id, thing->type); // printf("[%d/%d]%d ", thing->networkId, thing->id, thing->type);
if (thing->type == thingType) { if (thing->type == thingType) {
buffer[thingCount] = thing; buffer[thingCount] = thing;
thingCount++; thingCount++;
if (thingCount >= bufferSize) { if (thingCount >= bufferSize) {
printf("\n"); // printf("\n");
return bufferSize; return bufferSize;
} }
} }
} }
printf("\n"); // printf("\n");
return thingCount; return thingCount;
} }
@ -480,7 +484,7 @@ void Perception::Update(float currentTimeMs) {
float angle = sensor->position.angle; float angle = sensor->position.angle;
// Polar position = Polar(angle, distance); // Polar position = Polar(angle, distance);
Polar position = Polar(distance, angle); Polar position = Polar(distance, angle);
// AddTrackedObject(distanceSensor, position); AddTrackedObject(distanceSensor, position);
} }
} else if (sensor->type == Thing::SwitchType) { } else if (sensor->type == Thing::SwitchType) {
@ -525,7 +529,7 @@ void Perception::UpdatePose(Polar translation) {
// (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.horizontalAngle + translation.angle; float angle = (float)thing->position.horizontalAngle + translation.angle;
angle = abs(angle); angle = fabsf(angle);
float deltaDistance = translation.distance * cosf(angle * Angle::Deg2Rad); float deltaDistance = translation.distance * cosf(angle * Angle::Deg2Rad);
// printf(" | translate %f %f %f | ", (float)translation.distance, // printf(" | translate %f %f %f | ", (float)translation.distance,

1
Roboid.cpp
View File

@ -75,7 +75,6 @@ Vector2 Roboid::GetPosition2D() {
return Vector2(this->worldPosition.Right(), this->worldPosition.Forward()); return Vector2(this->worldPosition.Right(), this->worldPosition.Forward());
} }
#include <Arduino.h>
Quaternion Roboid::GetOrientation() { Quaternion Roboid::GetOrientation() {
Vector3 axis = this->worldAngleAxis.axis.ToVector3(); Vector3 axis = this->worldAngleAxis.axis.ToVector3();
Quaternion q = Quaternion::AngleAxis(this->worldAngleAxis.angle, axis); Quaternion q = Quaternion::AngleAxis(this->worldAngleAxis.angle, axis);