RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki 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

56
DifferentialDrive.cpp
View File

@ -4,11 +4,11 @@
#include <math.h>
DifferentialDrive::DifferentialDrive(){};
DifferentialDrive::DifferentialDrive() {};
DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor) {
DifferentialDrive::DifferentialDrive(Motor* leftMotor, Motor* rightMotor) {
this->motorCount = 2;
this->motors = new Motor *[2];
this->motors = new Motor*[2];
this->motors[0] = leftMotor;
this->motors[1] = rightMotor;
@ -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;
@ -35,11 +35,11 @@ void DifferentialDrive::SetDimensions(float wheelDiameter,
void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
float rightSpeed) {
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Motor *motor = motors[motorIx];
Motor* motor = motors[motorIx];
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)
@ -49,12 +49,12 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
float leftSpeed =
Float::Clamp(forward + yaw, -1, 1); // revolutions per second
Float::Clamp(forward + yaw, -1, 1); // revolutions per second
float rightSpeed =
Float::Clamp(forward - yaw, -1, 1); // revolutions per second
Float::Clamp(forward - yaw, -1, 1); // revolutions per second
float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
}
@ -63,44 +63,46 @@ 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() {
Motor *leftMotor = motors[0];
Motor *rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
Motor* leftMotor = motors[0];
Motor* rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
leftSpeed = leftSpeed * rpsToMs; // in meters per second
rightSpeed = rightSpeed * rpsToMs; // in meters per second
leftSpeed = leftSpeed * rpsToMs; // in meters per second
rightSpeed = rightSpeed * rpsToMs; // in meters per second
float speed = (leftSpeed + rightSpeed) / 2;
float direction = speed >= 0 ? 0.0F : 180.0F;
float magnitude = fabsf(speed);
Polar velocity = Polar(magnitude, direction); // Polar(direction, magnitude);
Polar velocity = Polar(magnitude, direction); // Polar(direction, magnitude);
return velocity;
}
float DifferentialDrive::GetAngularVelocity() {
Motor *leftMotor = motors[0];
Motor *rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
Motor* leftMotor = motors[0];
Motor* rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
leftSpeed = leftSpeed * rpsToMs; // in meters per second
rightSpeed = rightSpeed * rpsToMs; // in meters per second
leftSpeed = leftSpeed * rpsToMs; // in meters per second
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);
}

171
Perception.cpp
View File

@ -11,57 +11,59 @@
#include <Arduino.h>
#endif
unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
// number of object which can
// be tracked by a human
unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
// number of object which can
// be tracked by a human
Perception::Perception() {
this->trackedObjects = new InterestingThing *[maxObjectCount];
this->trackedObjects = new InterestingThing*[maxObjectCount];
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
this->trackedObjects[objIx] = nullptr;
}
Perception::Perception(Sensor **sensors, unsigned int sensorCount)
Perception::Perception(Sensor** sensors, unsigned int sensorCount)
: Perception() {
this->sensorCount = sensorCount;
this->sensors = new Sensor *[this->sensorCount];
this->sensors = new Sensor*[this->sensorCount];
for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
this->sensors[sensorIx] = sensors[sensorIx];
this->trackedObjects = new InterestingThing *[maxObjectCount];
this->trackedObjects = new InterestingThing*[maxObjectCount];
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
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)
return nullptr;
Sensor *sensor = this->sensors[sensorId];
Sensor* sensor = this->sensors[sensorId];
return sensor;
}
unsigned int Perception::AddSensor(Sensor *newSensor) {
unsigned int Perception::AddSensor(Sensor* newSensor) {
unsigned int newSensorCount = this->sensorCount + 1;
Sensor **newSensors = new Sensor *[newSensorCount];
Sensor** newSensors = new Sensor*[newSensorCount];
for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
newSensors[sensorIx] = sensors[sensorIx];
unsigned int sensorId = this->sensorCount;
newSensors[sensorId] = newSensor;
Sensor **oldSensors = this->sensors;
Sensor** oldSensors = this->sensors;
this->sensors = newSensors;
this->sensorCount = newSensorCount;
delete[] oldSensors;
return sensorId;
}
Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
Sensor* Perception::FindSensorOfType(unsigned int sensorType) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Sensor *sensor = this->sensors[sensorIx];
Sensor* sensor = this->sensors[sensorIx];
if (sensor->type == sensorType)
return sensor;
}
@ -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,9 +91,9 @@ 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?
distance = 9999; // infinity?
} else if (deltaAngle > range) {
float angle = deltaAngle - range;
@ -96,9 +101,9 @@ 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?
distance = 9999; // infinity?
}
// printf(" distance = %f\n", distance);
return distance;
@ -110,16 +115,17 @@ float Perception::GetDistance(float horizontalDirection, float range) {
range = -range;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *obj = trackedObjects[objIx];
InterestingThing* obj = trackedObjects[objIx];
if (obj == nullptr)
continue;
if (obj->type == 0x080) { // plane
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,24 +133,26 @@ 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;
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing *thing = trackedObjects[thingIx];
InterestingThing* thing = trackedObjects[thingIx];
if (thing == nullptr)
continue;
if (thing->type != thingType)
continue;
if (thing->type == 0x080) { // plane
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,18 +160,18 @@ 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;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *obj = trackedObjects[objIx];
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);
}
}
@ -175,12 +183,12 @@ bool Perception::ObjectNearby(float direction, float range) {
range = -range;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *obj = trackedObjects[objIx];
InterestingThing* obj = trackedObjects[objIx];
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,11 +281,13 @@ 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 *thing = new InterestingThing(sensor, position, orientation);
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;
thing->type = thingType;
@ -316,7 +327,7 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
this->trackedObjects[availableSlotIx] = thing;
thing->networkId = networkId;
if (thingId == 0x00)
thing->id = lastObjectId++; // availableSlotIx + 1;
thing->id = lastObjectId++; // availableSlotIx + 1;
return thing;
}
// If this object is closer than the farthest object, then replace it
@ -326,7 +337,7 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
this->trackedObjects[farthestObjIx] = thing;
thing->networkId = networkId;
if (thingId == 0x00)
thing->id = lastObjectId++; // availableSlotIx + 1;
thing->id = lastObjectId++; // availableSlotIx + 1;
return thing;
} else {
// No available slot, delete trackedobject
@ -335,25 +346,25 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
}
}
InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
unsigned char networkId,
unsigned char objectId,
Spherical position,
Quaternion orientation) {
InterestingThing *thing = FindTrackedObject(networkId, objectId);
InterestingThing* thing = FindTrackedObject(networkId, objectId);
if (thing == nullptr) {
thing =
AddTrackedObject(sensor, position, orientation, objectId, networkId);
thing->networkId = networkId;
thing->id = objectId;
thing->type = 0xFF; // unknown
thing->type = 0xFF; // unknown
}
return thing;
}
bool Perception::IsInteresting(float distance) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx];
InterestingThing* thing = this->trackedObjects[objIx];
if (thing == nullptr)
return true;
if (thing->position.distance > distance)
@ -362,9 +373,9 @@ bool Perception::IsInteresting(float distance) {
return false;
}
InterestingThing *Perception::FindTrackedObject(char objectId) {
InterestingThing* Perception::FindTrackedObject(char objectId) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx];
InterestingThing* thing = this->trackedObjects[objIx];
if (thing == nullptr)
continue;
if (thing->id == objectId) {
@ -374,10 +385,10 @@ InterestingThing *Perception::FindTrackedObject(char objectId) {
return nullptr;
}
InterestingThing *Perception::FindTrackedObject(unsigned char networkId,
InterestingThing* Perception::FindTrackedObject(unsigned char networkId,
unsigned char objectId) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx];
InterestingThing* thing = this->trackedObjects[objIx];
if (thing == nullptr)
continue;
@ -397,16 +408,16 @@ unsigned char Perception::TrackedObjectCount() {
return objectCount;
}
InterestingThing **Perception::GetTrackedObjects() {
InterestingThing** Perception::GetTrackedObjects() {
return this->trackedObjects;
}
unsigned char Perception::ThingsOfType(unsigned char thingType,
InterestingThing *buffer[],
InterestingThing* buffer[],
unsigned char bufferSize) {
unsigned char thingCount = 0;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx];
InterestingThing* thing = this->trackedObjects[objIx];
if (thing == nullptr)
continue;
@ -424,9 +435,9 @@ unsigned char Perception::ThingsOfType(unsigned char thingType,
return thingCount;
}
InterestingThing *Perception::ThingOfType(unsigned char thingType) {
InterestingThing* Perception::ThingOfType(unsigned char thingType) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx];
InterestingThing* thing = this->trackedObjects[objIx];
if (thing == nullptr)
continue;
@ -436,14 +447,14 @@ InterestingThing *Perception::ThingOfType(unsigned char thingType) {
return nullptr;
}
InterestingThing *Perception::GetMostInterestingThing() {
InterestingThing* Perception::GetMostInterestingThing() {
if (this->trackedObjects == nullptr)
return nullptr;
InterestingThing *closestObject = nullptr;
InterestingThing* closestObject = nullptr;
float closestDistance = INFINITY;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *obj = this->trackedObjects[objIx];
InterestingThing* obj = this->trackedObjects[objIx];
if (obj != nullptr) {
if (obj->position.distance < closestDistance) {
closestObject = obj;
@ -463,23 +474,23 @@ void Perception::Update(unsigned long currentTimeMs) {
// Update sensing
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Sensor *sensor = sensors[sensorIx];
Sensor* sensor = sensors[sensorIx];
if (sensor == nullptr)
continue;
if (sensor->type == Thing::DistanceSensorType) {
DistanceSensor *distanceSensor = (DistanceSensor *)sensor;
DistanceSensor* distanceSensor = (DistanceSensor*)sensor;
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);
}
} else if (sensor->type == Thing::SwitchType) {
Switch *switchSensor = (Switch *)sensor;
Switch* switchSensor = (Switch*)sensor;
if (switchSensor->IsOn()) {
// Polar position = Polar(sensor->position.angle, nearbyDistance);
Polar position =
@ -492,7 +503,7 @@ void Perception::Update(unsigned long currentTimeMs) {
}
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = trackedObjects[objIx];
InterestingThing* thing = trackedObjects[objIx];
if (thing == nullptr)
continue;
@ -509,21 +520,23 @@ void Perception::Update(unsigned long currentTimeMs) {
void Perception::UpdatePose(Polar translation) {
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing *thing = trackedObjects[thingIx];
InterestingThing* thing = trackedObjects[thingIx];
if (thing == nullptr)
continue;
// We only support translations in the horizontal plane at this moment...
// This needs Spherical operator- to be implemented to work in 3d space
if (thing->type == 0x80) { // plane
if (thing->type == 0x80) { // plane
// printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
// (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;
@ -532,7 +545,7 @@ void Perception::UpdatePose(Polar translation) {
// (float)thing->position.verticalAngle);
} else {
Polar horizontalPosition =
Polar(thing->position); // obj->position.ProjectOnHorizontalPlane();
Polar(thing->position); // obj->position.ProjectOnHorizontalPlane();
Spherical newPosition = Spherical(horizontalPosition - translation);
thing->position = newPosition;
}
@ -549,7 +562,7 @@ void Perception::UpdatePose(Quaternion rotation) {
rotationAngle = -rotationAngle;
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing *thing = trackedObjects[thingIx];
InterestingThing* thing = trackedObjects[thingIx];
if (thing == nullptr)
continue;
@ -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,

23
Roboid.cpp
View File

@ -26,7 +26,7 @@ Roboid::Roboid() : Thing(0) {
this->worldAngleAxis = AngleAxis();
}
Roboid::Roboid(Propulsion *propulsion) : Roboid() {
Roboid::Roboid(Propulsion* propulsion) : Roboid() {
this->propulsion = propulsion;
if (propulsion != nullptr)
propulsion->roboid = this;
@ -61,25 +61,28 @@ 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;
}
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
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);
@ -119,10 +122,10 @@ void Roboid::SetOrientation2D(float angle) {
this->worldAngleAxis = AngleAxis(angle, Axis::up);
}
void Roboid::AddChild(Thing *child) {
void Roboid::AddChild(Thing* child) {
Thing::AddChild(child);
if (child->IsSensor()) {
Sensor *childSensor = (Sensor *)child;
Sensor* childSensor = (Sensor*)child;
this->perception->AddSensor(childSensor);
}
}

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

15
TrackedObject.cpp
View File

@ -2,7 +2,7 @@
#include <math.h>
InterestingThing::InterestingThing(Sensor *sensor, Polar position) {
InterestingThing::InterestingThing(Sensor* sensor, Polar position) {
this->id = 0;
this->confidence = maxConfidence;
this->sensor = sensor;
@ -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;
@ -21,7 +22,7 @@ InterestingThing::InterestingThing(Sensor *sensor, Spherical position,
}
// #include <Arduino.h>
bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) {
bool InterestingThing::IsTheSameAs(InterestingThing* otherObj) {
if (id != 0 && id == otherObj->id)
return true;
if (type != otherObj->type)
@ -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;