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

Fixed explicit Angle->float

Browse Source
This commit is contained in:
Pascal Serrarens 2024-08-05 10:21:39 +02:00
parent 246929a81d
commit ed715ed610
7 changed files with 153 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
DifferentialDrive.cpp
View File

@ -25,7 +25,7 @@ void DifferentialDrive::SetDimensions(float wheelDiameter,
float wheelSeparation) { float wheelSeparation) {
this->wheelDiameter = wheelDiameter; this->wheelDiameter = wheelDiameter;
this->wheelSeparation = wheelSeparation; this->wheelSeparation = wheelSeparation;
this->rpsToMs = wheelDiameter * Angle::pi; this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
float distance = this->wheelSeparation / 2; float distance = this->wheelSeparation / 2;
this->motors[0]->position.distance = distance; this->motors[0]->position.distance = distance;
@ -39,7 +39,7 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
if (motor == nullptr) if (motor == nullptr)
continue; continue;
float xPosition = motors[motorIx]->position.horizontalAngle; float xPosition = motors[motorIx]->position.horizontalAngle.ToFloat();
if (xPosition < 0) if (xPosition < 0)
motor->SetTargetSpeed(leftSpeed); motor->SetTargetSpeed(leftSpeed);
else if (xPosition > 0) else if (xPosition > 0)
@ -63,13 +63,15 @@ void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
SetTwistSpeed(linear.y, yaw); SetTwistSpeed(linear.y, yaw);
} }
void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch, void DifferentialDrive::SetTwistSpeed(Vector3 linear,
float yaw,
float pitch,
float roll) { float roll) {
SetTwistSpeed(linear.Forward(), yaw); SetTwistSpeed(linear.Forward(), yaw);
} }
void DifferentialDrive::SetVelocity(Polar velocity) { void DifferentialDrive::SetVelocity(Polar velocity) {
SetTwistSpeed(velocity.distance, velocity.angle); SetTwistSpeed(velocity.distance, velocity.angle.ToFloat());
} }
Polar DifferentialDrive::GetVelocity() { Polar DifferentialDrive::GetVelocity() {
@ -98,9 +100,9 @@ float DifferentialDrive::GetAngularVelocity() {
rightSpeed = rightSpeed * rpsToMs; // in meters per second rightSpeed = rightSpeed * rpsToMs; // in meters per second
float angularSpeed = (leftSpeed - rightSpeed) / 2; float angularSpeed = (leftSpeed - rightSpeed) / 2;
float angularDistance = wheelSeparation / 2 * Angle::pi; float angularDistance = wheelSeparation / 2 * Passer::LinearAlgebra::pi;
float rotationsPerSecond = angularSpeed / angularDistance; float rotationsPerSecond = angularSpeed / angularDistance;
float degreesPerSecond = Angle::Normalize(360 * rotationsPerSecond); float degreesPerSecond = Angle::Normalize(360 * rotationsPerSecond).ToFloat();
float angularVelocity = degreesPerSecond; float angularVelocity = degreesPerSecond;
return angularVelocity; return angularVelocity;

2
LinearAlgebra

@ -1 +1 @@
Subproject commit 608c45c1a7b7f5469cfdcd1a8ff30036111aefb2 Subproject commit 49c67405fcbf15b3470080e67ab203c835f1f9b8

6
NetworkSync.cpp
View File

@ -345,15 +345,15 @@ void NetworkSync::SendQuaternion(unsigned char* data,
void NetworkSync::SendPolar(unsigned char* data, void NetworkSync::SendPolar(unsigned char* data,
unsigned char* startIndex, unsigned char* startIndex,
Polar p) { Polar p) {
SendAngle8(data, *startIndex, (const float)p.angle); SendAngle8(data, *startIndex, (const float)p.angle.ToFloat());
SendSingle100(data, (*startIndex) + 1, p.distance); SendSingle100(data, (*startIndex) + 1, p.distance);
} }
void NetworkSync::SendSpherical(unsigned char* data, void NetworkSync::SendSpherical(unsigned char* data,
unsigned char* startIndex, unsigned char* startIndex,
Spherical s) { Spherical s) {
SendAngle8(data, (*startIndex)++, s.horizontalAngle); SendAngle8(data, (*startIndex)++, s.horizontalAngle.ToFloat());
SendAngle8(data, (*startIndex)++, s.verticalAngle); SendAngle8(data, (*startIndex)++, s.verticalAngle.ToFloat());
// SendAngle8(data, startIndex++, s.distance); // SendAngle8(data, startIndex++, s.distance);
SendFloat16(data, startIndex, s.distance); SendFloat16(data, startIndex, s.distance);
} }

73
Perception.cpp
View File

@ -33,7 +33,9 @@ Perception::Perception(Sensor **sensors, unsigned int sensorCount)
this->trackedObjects[objIx] = nullptr; this->trackedObjects[objIx] = nullptr;
} }
unsigned int Perception::GetSensorCount() { return this->sensorCount; } unsigned int Perception::GetSensorCount() {
return this->sensorCount;
}
Sensor* Perception::GetSensor(unsigned int sensorId) { Sensor* Perception::GetSensor(unsigned int sensorId) {
if (sensorId >= this->sensorCount) if (sensorId >= this->sensorCount)
@ -69,11 +71,14 @@ Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
return nullptr; return nullptr;
} }
float GetPlaneDistance(InterestingThing *plane, float horizontalAngle, float GetPlaneDistance(InterestingThing* plane,
float horizontalAngle,
float range) { float range) {
float distance = plane->position.distance; float distance = plane->position.distance;
float deltaAngle = Angle::Normalize((float)plane->position.horizontalAngle - float deltaAngle =
horizontalAngle); Angle::Normalize((float)plane->position.horizontalAngle.ToFloat() -
horizontalAngle)
.ToFloat();
if (fabsf(deltaAngle) < fabsf(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,
@ -86,7 +91,7 @@ float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
// (float)plane->position.horizontalAngle, horizontalAngle, range, // (float)plane->position.horizontalAngle, horizontalAngle, range,
// angle, cosf(angle * Angle::Deg2Rad)); // angle, cosf(angle * Angle::Deg2Rad));
if (angle > -90) if (angle > -90)
distance = distance / cosf(angle * Angle::Deg2Rad); distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad);
else else
distance = 9999; // infinity? distance = 9999; // infinity?
@ -96,7 +101,7 @@ float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
// (float)plane->position.horizontalAngle, horizontalAngle, range, // (float)plane->position.horizontalAngle, horizontalAngle, range,
// angle, cosf(angle * Angle::Deg2Rad)); // angle, cosf(angle * Angle::Deg2Rad));
if (angle < 90) if (angle < 90)
distance = distance / cosf(angle * Angle::Deg2Rad); distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad);
else else
distance = 9999; // infinity? distance = 9999; // infinity?
} }
@ -117,9 +122,10 @@ 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.horizontalAngle > horizontalDirection - range && } else if (obj->position.horizontalAngle.ToFloat() >
obj->position.horizontalAngle < horizontalDirection + range) { horizontalDirection - range &&
obj->position.horizontalAngle.ToFloat() <
horizontalDirection + range) {
minDistance = fminf(minDistance, obj->position.distance); minDistance = fminf(minDistance, obj->position.distance);
} }
} }
@ -127,7 +133,8 @@ float Perception::GetDistance(float horizontalDirection, float range) {
} }
float Perception::GetDistanceOfType(unsigned char thingType, float Perception::GetDistanceOfType(unsigned char thingType,
float horizontalAngle, float range) { float horizontalAngle,
float range) {
float minDistance = INFINITY; float minDistance = INFINITY;
if (range < 0) if (range < 0)
range = -range; range = -range;
@ -142,9 +149,10 @@ 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.horizontalAngle > horizontalAngle - range && } else if (thing->position.horizontalAngle.ToFloat() >
thing->position.horizontalAngle < horizontalAngle + range) { horizontalAngle - range &&
thing->position.horizontalAngle.ToFloat() <
horizontalAngle + range) {
minDistance = fminf(minDistance, thing->position.distance); minDistance = fminf(minDistance, thing->position.distance);
} }
} }
@ -152,7 +160,8 @@ float Perception::GetDistanceOfType(unsigned char thingType,
} }
float Perception::GetDistance(float horizontalDirection, float Perception::GetDistance(float horizontalDirection,
float verticalDirection, float range) { float verticalDirection,
float range) {
float minDistance = INFINITY; float minDistance = INFINITY;
if (range < 0) if (range < 0)
range = -range; range = -range;
@ -161,9 +170,8 @@ float Perception::GetDistance(float horizontalDirection,
InterestingThing* obj = trackedObjects[objIx]; InterestingThing* obj = trackedObjects[objIx];
if (obj == nullptr) if (obj == nullptr)
continue; continue;
if (obj->position.horizontalAngle > horizontalDirection - range && if (obj->position.horizontalAngle.ToFloat() > horizontalDirection - range &&
obj->position.horizontalAngle < horizontalDirection + range) { obj->position.horizontalAngle.ToFloat() < horizontalDirection + range) {
minDistance = fminf(minDistance, obj->position.distance); minDistance = fminf(minDistance, obj->position.distance);
} }
} }
@ -179,8 +187,8 @@ bool Perception::ObjectNearby(float direction, float range) {
if (obj == nullptr) if (obj == nullptr)
continue; continue;
if (obj->position.horizontalAngle > direction - range && if (obj->position.horizontalAngle.ToFloat() > direction - range &&
obj->position.horizontalAngle < direction + range) { obj->position.horizontalAngle.ToFloat() < direction + range) {
if (obj->position.distance <= nearbyDistance) if (obj->position.distance <= nearbyDistance)
return true; return true;
} }
@ -190,7 +198,8 @@ bool Perception::ObjectNearby(float direction, float range) {
// #include <WifiSync.h> // #include <WifiSync.h>
// This function is deprecated // This function is deprecated
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) {
Spherical sPos = Spherical(position); Spherical sPos = Spherical(position);
@ -272,10 +281,12 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
*/ */
} }
InterestingThing * InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
Perception::AddTrackedObject(Sensor *sensor, Spherical position, Spherical position,
Quaternion orientation, unsigned char thingType, Quaternion orientation,
unsigned char thingId, unsigned networkId) { unsigned char thingType,
unsigned char thingId,
unsigned networkId) {
InterestingThing* thing = new InterestingThing(sensor, position, orientation); InterestingThing* thing = new InterestingThing(sensor, position, orientation);
if (thingId != 0x00) if (thingId != 0x00)
thing->id = thingId; thing->id = thingId;
@ -472,7 +483,7 @@ void Perception::Update(unsigned long currentTimeMs) {
float distance = distanceSensor->GetDistance(); float distance = distanceSensor->GetDistance();
if (distance >= 0) { if (distance >= 0) {
float angle = sensor->position.horizontalAngle; Angle angle = sensor->position.horizontalAngle;
// 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);
@ -520,10 +531,12 @@ 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.horizontalAngle + translation.angle; float angle = (float)thing->position.horizontalAngle.ToFloat() +
translation.angle.ToFloat();
angle = fabsf(angle); angle = fabsf(angle);
float deltaDistance = translation.distance * cosf(angle * Angle::Deg2Rad); float deltaDistance =
translation.distance * cosf(angle * Passer::LinearAlgebra::Deg2Rad);
// printf(" | translate %f %f %f | ", (float)translation.distance, // printf(" | translate %f %f %f | ", (float)translation.distance,
// (float)angle, deltaDistance); // (float)angle, deltaDistance);
thing->position.distance -= deltaDistance; thing->position.distance -= deltaDistance;
@ -557,9 +570,9 @@ void Perception::UpdatePose(Quaternion rotation) {
// (float)thing->position.horizontalAngle, // (float)thing->position.horizontalAngle,
// (float)thing->position.verticalAngle); // (float)thing->position.verticalAngle);
// printf("| rotate %f | ", rotationAngle); // printf("| rotate %f | ", rotationAngle);
float updatedAngle =
Angle::Normalize(thing->position.horizontalAngle - rotationAngle); thing->position.horizontalAngle = Angle::Normalize(
thing->position.horizontalAngle = updatedAngle; thing->position.horizontalAngle.ToFloat() - 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,

15
Roboid.cpp
View File

@ -61,14 +61,17 @@ void Roboid::Update(unsigned long currentTimeMs) {
lastUpdateTimeMs = currentTimeMs; lastUpdateTimeMs = currentTimeMs;
} }
Vector3 Roboid::GetPosition() { return this->worldPosition; } Vector3 Roboid::GetPosition() {
return this->worldPosition;
}
Vector2 Roboid::GetPosition2D() { Vector2 Roboid::GetPosition2D() {
return Vector2(this->worldPosition.Right(), this->worldPosition.Forward()); return Vector2(this->worldPosition.Right(), this->worldPosition.Forward());
} }
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.ToFloat(), axis);
return q; return q;
} }
@ -76,10 +79,10 @@ float Roboid::GetOrientation2D() {
float maxAngle = 90 - Float::epsilon; // note: range vertical angle = -90..90 float maxAngle = 90 - Float::epsilon; // note: range vertical angle = -90..90
// rotation axis is vertical, so we have a simple 2D orientation // rotation axis is vertical, so we have a simple 2D orientation
if (this->worldAngleAxis.axis.verticalAngle > maxAngle) if (this->worldAngleAxis.axis.verticalAngle.ToFloat() > maxAngle)
return this->worldAngleAxis.angle; return this->worldAngleAxis.angle.ToFloat();
if (this->worldAngleAxis.axis.verticalAngle < -maxAngle) if (this->worldAngleAxis.axis.verticalAngle.ToFloat() < -maxAngle)
return -this->worldAngleAxis.angle; return -this->worldAngleAxis.angle.ToFloat();
Quaternion q = GetOrientation(); Quaternion q = GetOrientation();
return Quaternion::GetAngleAround(Vector3::up, q); return Quaternion::GetAngleAround(Vector3::up, q);

13
ServoMotor.cpp
View File

@ -11,7 +11,7 @@ ServoMotor::ServoMotor()
} }
void ServoMotor::SetTargetAngle(Angle16 angle) { void ServoMotor::SetTargetAngle(Angle16 angle) {
angle = Float::Clamp(angle, minAngle, maxAngle); angle = Float::Clamp(angle.ToFloat(), minAngle.ToFloat(), maxAngle.ToFloat());
if (maxVelocity == 0.0F || hasTargetAngle == false) { if (maxVelocity == 0.0F || hasTargetAngle == false) {
SetAngle(angle); SetAngle(angle);
@ -68,7 +68,8 @@ void ServoMotor::Update(unsigned long currentTimeMs) {
} else { } else {
float angleStep = maxVelocity * deltaTime; float angleStep = maxVelocity * deltaTime;
float deltaAngle = this->targetAngle - this->limitedTargetAngle; float deltaAngle =
this->targetAngle.ToFloat() - this->limitedTargetAngle.ToFloat();
float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle; float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle;
if (absDeltaAngle < angleStep) { if (absDeltaAngle < angleStep) {
@ -76,9 +77,9 @@ void ServoMotor::Update(unsigned long currentTimeMs) {
SetAngle(targetAngle); SetAngle(targetAngle);
} else { } else {
if (deltaAngle < 0) if (deltaAngle < 0)
limitedTargetAngle = limitedTargetAngle - angleStep; limitedTargetAngle = limitedTargetAngle.ToFloat() - angleStep;
else else
limitedTargetAngle = limitedTargetAngle + angleStep; limitedTargetAngle = limitedTargetAngle.ToFloat() + angleStep;
} }
SetAngle(limitedTargetAngle); SetAngle(limitedTargetAngle);
@ -88,8 +89,8 @@ void ServoMotor::Update(unsigned long currentTimeMs) {
// Velocity control // Velocity control
} else { } else {
float newAngle = this->targetAngle + targetVelocity * deltaTime; float newAngle = this->targetAngle.ToFloat() + targetVelocity * deltaTime;
newAngle = Float::Clamp(newAngle, minAngle, maxAngle); newAngle = Float::Clamp(newAngle, minAngle.ToFloat(), maxAngle.ToFloat());
ServoMotor::SetTargetAngle(newAngle); ServoMotor::SetTargetAngle(newAngle);
SetAngle(newAngle); SetAngle(newAngle);

11
TrackedObject.cpp
View File

@ -10,7 +10,8 @@ InterestingThing::InterestingThing(Sensor *sensor, Polar position) {
this->updated = true; this->updated = true;
} }
InterestingThing::InterestingThing(Sensor *sensor, Spherical position, InterestingThing::InterestingThing(Sensor* sensor,
Spherical position,
Quaternion orientation) { Quaternion orientation) {
this->id = 0; this->id = 0;
this->confidence = maxConfidence; this->confidence = maxConfidence;
@ -40,11 +41,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.horizontalAngle - otherObj->position.horizontalAngle) > if (fabsf(position.horizontalAngle.ToFloat() -
equalAngle) otherObj->position.horizontalAngle.ToFloat()) > equalAngle)
return false; return false;
if (fabsf(position.verticalAngle - otherObj->position.verticalAngle) > if (fabsf(position.verticalAngle.ToFloat() -
equalAngle) otherObj->position.verticalAngle.ToFloat()) > equalAngle)
return false; return false;
// printf(" -> yes "); // printf(" -> yes ");
return true; return true;