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) {
this->wheelDiameter = wheelDiameter;
this->wheelSeparation = wheelSeparation;
this->rpsToMs = wheelDiameter * Angle::pi;
this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
float distance = this->wheelSeparation / 2;
this->motors[0]->position.distance = distance;
@ -39,7 +39,7 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
if (motor == nullptr)
continue;
float xPosition = motors[motorIx]->position.horizontalAngle;
float xPosition = motors[motorIx]->position.horizontalAngle.ToFloat();
if (xPosition < 0)
motor->SetTargetSpeed(leftSpeed);
else if (xPosition > 0)
@ -63,13 +63,15 @@ void DifferentialDrive::SetTwistSpeed(Vector2 linear, float 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) {
SetTwistSpeed(linear.Forward(), yaw);
}
void DifferentialDrive::SetVelocity(Polar velocity) {
SetTwistSpeed(velocity.distance, velocity.angle);
SetTwistSpeed(velocity.distance, velocity.angle.ToFloat());
}
Polar DifferentialDrive::GetVelocity() {
@ -98,9 +100,9 @@ float DifferentialDrive::GetAngularVelocity() {
rightSpeed = rightSpeed * rpsToMs; // in meters per second
float angularSpeed = (leftSpeed - rightSpeed) / 2;
float angularDistance = wheelSeparation / 2 * Angle::pi;
float angularDistance = wheelSeparation / 2 * Passer::LinearAlgebra::pi;
float rotationsPerSecond = angularSpeed / angularDistance;
float degreesPerSecond = Angle::Normalize(360 * rotationsPerSecond);
float degreesPerSecond = Angle::Normalize(360 * rotationsPerSecond).ToFloat();
float angularVelocity = degreesPerSecond;
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,
unsigned char* startIndex,
Polar p) {
SendAngle8(data, *startIndex, (const float)p.angle);
SendAngle8(data, *startIndex, (const float)p.angle.ToFloat());
SendSingle100(data, (*startIndex) + 1, p.distance);
}
void NetworkSync::SendSpherical(unsigned char* data,
unsigned char* startIndex,
Spherical s) {
SendAngle8(data, (*startIndex)++, s.horizontalAngle);
SendAngle8(data, (*startIndex)++, s.verticalAngle);
SendAngle8(data, (*startIndex)++, s.horizontalAngle.ToFloat());
SendAngle8(data, (*startIndex)++, s.verticalAngle.ToFloat());
// SendAngle8(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;
}
unsigned int Perception::GetSensorCount() { return this->sensorCount; }
unsigned int Perception::GetSensorCount() {
return this->sensorCount;
}
Sensor* Perception::GetSensor(unsigned int sensorId) {
if (sensorId >= this->sensorCount)
@ -69,11 +71,14 @@ Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
return nullptr;
}
float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
float GetPlaneDistance(InterestingThing* plane,
float horizontalAngle,
float range) {
float distance = plane->position.distance;
float deltaAngle = Angle::Normalize((float)plane->position.horizontalAngle -
horizontalAngle);
float deltaAngle =
Angle::Normalize((float)plane->position.horizontalAngle.ToFloat() -
horizontalAngle)
.ToFloat();
if (fabsf(deltaAngle) < fabsf(range)) {
// distance = 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,
// angle, cosf(angle * Angle::Deg2Rad));
if (angle > -90)
distance = distance / cosf(angle * Angle::Deg2Rad);
distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad);
else
distance = 9999; // infinity?
@ -96,7 +101,7 @@ float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
// (float)plane->position.horizontalAngle, horizontalAngle, range,
// angle, cosf(angle * Angle::Deg2Rad));
if (angle < 90)
distance = distance / cosf(angle * Angle::Deg2Rad);
distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad);
else
distance = 9999; // infinity?
}
@ -117,9 +122,10 @@ 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.horizontalAngle > horizontalDirection - range &&
obj->position.horizontalAngle < horizontalDirection + range) {
} else if (obj->position.horizontalAngle.ToFloat() >
horizontalDirection - range &&
obj->position.horizontalAngle.ToFloat() <
horizontalDirection + range) {
minDistance = fminf(minDistance, obj->position.distance);
}
}
@ -127,7 +133,8 @@ float Perception::GetDistance(float horizontalDirection, float range) {
}
float Perception::GetDistanceOfType(unsigned char thingType,
float horizontalAngle, float range) {
float horizontalAngle,
float range) {
float minDistance = INFINITY;
if (range < 0)
range = -range;
@ -142,9 +149,10 @@ 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.horizontalAngle > horizontalAngle - range &&
thing->position.horizontalAngle < horizontalAngle + range) {
} else if (thing->position.horizontalAngle.ToFloat() >
horizontalAngle - range &&
thing->position.horizontalAngle.ToFloat() <
horizontalAngle + range) {
minDistance = fminf(minDistance, thing->position.distance);
}
}
@ -152,7 +160,8 @@ float Perception::GetDistanceOfType(unsigned char thingType,
}
float Perception::GetDistance(float horizontalDirection,
float verticalDirection, float range) {
float verticalDirection,
float range) {
float minDistance = INFINITY;
if (range < 0)
range = -range;
@ -161,9 +170,8 @@ float Perception::GetDistance(float horizontalDirection,
InterestingThing* obj = trackedObjects[objIx];
if (obj == nullptr)
continue;
if (obj->position.horizontalAngle > horizontalDirection - range &&
obj->position.horizontalAngle < horizontalDirection + range) {
if (obj->position.horizontalAngle.ToFloat() > horizontalDirection - range &&
obj->position.horizontalAngle.ToFloat() < horizontalDirection + range) {
minDistance = fminf(minDistance, obj->position.distance);
}
}
@ -179,8 +187,8 @@ bool Perception::ObjectNearby(float direction, float range) {
if (obj == nullptr)
continue;
if (obj->position.horizontalAngle > direction - range &&
obj->position.horizontalAngle < direction + range) {
if (obj->position.horizontalAngle.ToFloat() > direction - range &&
obj->position.horizontalAngle.ToFloat() < direction + range) {
if (obj->position.distance <= nearbyDistance)
return true;
}
@ -190,7 +198,8 @@ bool Perception::ObjectNearby(float direction, float range) {
// #include <WifiSync.h>
// This function is deprecated
void Perception::AddTrackedObject(Sensor *sensor, Polar position,
void Perception::AddTrackedObject(Sensor* sensor,
Polar position,
unsigned char thingType,
unsigned char networkId) {
Spherical sPos = Spherical(position);
@ -272,10 +281,12 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
*/
}
InterestingThing *
Perception::AddTrackedObject(Sensor *sensor, Spherical position,
Quaternion orientation, unsigned char thingType,
unsigned char thingId, unsigned networkId) {
InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
Spherical position,
Quaternion orientation,
unsigned char thingType,
unsigned char thingId,
unsigned networkId) {
InterestingThing* thing = new InterestingThing(sensor, position, orientation);
if (thingId != 0x00)
thing->id = thingId;
@ -472,7 +483,7 @@ void Perception::Update(unsigned long currentTimeMs) {
float distance = distanceSensor->GetDistance();
if (distance >= 0) {
float angle = sensor->position.horizontalAngle;
Angle angle = sensor->position.horizontalAngle;
// Polar position = Polar(angle, distance);
Polar position = Polar(distance, angle);
AddTrackedObject(distanceSensor, position);
@ -520,10 +531,12 @@ 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.horizontalAngle + translation.angle;
float angle = (float)thing->position.horizontalAngle.ToFloat() +
translation.angle.ToFloat();
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,
// (float)angle, deltaDistance);
thing->position.distance -= deltaDistance;
@ -557,9 +570,9 @@ void Perception::UpdatePose(Quaternion rotation) {
// (float)thing->position.horizontalAngle,
// (float)thing->position.verticalAngle);
// printf("| rotate %f | ", rotationAngle);
float updatedAngle =
Angle::Normalize(thing->position.horizontalAngle - rotationAngle);
thing->position.horizontalAngle = updatedAngle;
thing->position.horizontalAngle = Angle::Normalize(
thing->position.horizontalAngle.ToFloat() - rotationAngle);
// printf("-> %f (%f %f) \n", thing->position.distance,
// (float)thing->position.horizontalAngle,

15
Roboid.cpp
View File

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

13
ServoMotor.cpp
View File

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

11
TrackedObject.cpp
View File

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