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

30
DifferentialDrive.cpp
View File

@ -4,11 +4,11 @@
#include <math.h> #include <math.h>
DifferentialDrive::DifferentialDrive(){}; DifferentialDrive::DifferentialDrive() {};
DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor) { DifferentialDrive::DifferentialDrive(Motor* leftMotor, Motor* rightMotor) {
this->motorCount = 2; this->motorCount = 2;
this->motors = new Motor *[2]; this->motors = new Motor*[2];
this->motors[0] = leftMotor; this->motors[0] = leftMotor;
this->motors[1] = rightMotor; this->motors[1] = rightMotor;
@ -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;
@ -35,11 +35,11 @@ void DifferentialDrive::SetDimensions(float wheelDiameter,
void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
float rightSpeed) { float rightSpeed) {
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Motor *motor = motors[motorIx]; Motor* motor = motors[motorIx];
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,18 +63,20 @@ 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() {
Motor *leftMotor = motors[0]; Motor* leftMotor = motors[0];
Motor *rightMotor = motors[1]; Motor* rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
@ -89,8 +91,8 @@ Polar DifferentialDrive::GetVelocity() {
} }
float DifferentialDrive::GetAngularVelocity() { float DifferentialDrive::GetAngularVelocity() {
Motor *leftMotor = motors[0]; Motor* leftMotor = motors[0];
Motor *rightMotor = motors[1]; Motor* rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
@ -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);
} }

147
Perception.cpp
View File

@ -16,52 +16,54 @@ unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
// be tracked by a human // be tracked by a human
Perception::Perception() { Perception::Perception() {
this->trackedObjects = new InterestingThing *[maxObjectCount]; this->trackedObjects = new InterestingThing*[maxObjectCount];
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
this->trackedObjects[objIx] = nullptr; this->trackedObjects[objIx] = nullptr;
} }
Perception::Perception(Sensor **sensors, unsigned int sensorCount) Perception::Perception(Sensor** sensors, unsigned int sensorCount)
: Perception() { : Perception() {
this->sensorCount = sensorCount; this->sensorCount = sensorCount;
this->sensors = new Sensor *[this->sensorCount]; this->sensors = new Sensor*[this->sensorCount];
for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
this->sensors[sensorIx] = sensors[sensorIx]; this->sensors[sensorIx] = sensors[sensorIx];
this->trackedObjects = new InterestingThing *[maxObjectCount]; this->trackedObjects = new InterestingThing*[maxObjectCount];
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
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)
return nullptr; return nullptr;
Sensor *sensor = this->sensors[sensorId]; Sensor* sensor = this->sensors[sensorId];
return sensor; return sensor;
} }
unsigned int Perception::AddSensor(Sensor *newSensor) { unsigned int Perception::AddSensor(Sensor* newSensor) {
unsigned int newSensorCount = this->sensorCount + 1; 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++) for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
newSensors[sensorIx] = sensors[sensorIx]; newSensors[sensorIx] = sensors[sensorIx];
unsigned int sensorId = this->sensorCount; unsigned int sensorId = this->sensorCount;
newSensors[sensorId] = newSensor; newSensors[sensorId] = newSensor;
Sensor **oldSensors = this->sensors; Sensor** oldSensors = this->sensors;
this->sensors = newSensors; this->sensors = newSensors;
this->sensorCount = newSensorCount; this->sensorCount = newSensorCount;
delete[] oldSensors; delete[] oldSensors;
return sensorId; return sensorId;
} }
Sensor *Perception::FindSensorOfType(unsigned int sensorType) { Sensor* Perception::FindSensorOfType(unsigned int sensorType) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Sensor *sensor = this->sensors[sensorIx]; Sensor* sensor = this->sensors[sensorIx];
if (sensor->type == sensorType) if (sensor->type == sensorType)
return sensor; return sensor;
} }
@ -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?
} }
@ -110,16 +115,17 @@ float Perception::GetDistance(float horizontalDirection, float range) {
range = -range; range = -range;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *obj = trackedObjects[objIx]; InterestingThing* obj = trackedObjects[objIx];
if (obj == nullptr) if (obj == nullptr)
continue; continue;
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,13 +133,14 @@ 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;
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing *thing = trackedObjects[thingIx]; InterestingThing* thing = trackedObjects[thingIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
if (thing->type != thingType) if (thing->type != thingType)
@ -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,18 +160,18 @@ 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;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
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);
} }
} }
@ -175,12 +183,12 @@ bool Perception::ObjectNearby(float direction, float range) {
range = -range; range = -range;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *obj = trackedObjects[objIx]; InterestingThing* obj = trackedObjects[objIx];
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,11 +281,13 @@ 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,
InterestingThing *thing = new InterestingThing(sensor, position, orientation); unsigned char thingId,
unsigned networkId) {
InterestingThing* thing = new InterestingThing(sensor, position, orientation);
if (thingId != 0x00) if (thingId != 0x00)
thing->id = thingId; thing->id = thingId;
thing->type = thingType; thing->type = thingType;
@ -335,12 +346,12 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
} }
} }
InterestingThing *Perception::AddTrackedObject(Sensor *sensor, InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
unsigned char networkId, unsigned char networkId,
unsigned char objectId, unsigned char objectId,
Spherical position, Spherical position,
Quaternion orientation) { Quaternion orientation) {
InterestingThing *thing = FindTrackedObject(networkId, objectId); InterestingThing* thing = FindTrackedObject(networkId, objectId);
if (thing == nullptr) { if (thing == nullptr) {
thing = thing =
AddTrackedObject(sensor, position, orientation, objectId, networkId); AddTrackedObject(sensor, position, orientation, objectId, networkId);
@ -353,7 +364,7 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
bool Perception::IsInteresting(float distance) { bool Perception::IsInteresting(float distance) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx]; InterestingThing* thing = this->trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
return true; return true;
if (thing->position.distance > distance) if (thing->position.distance > distance)
@ -362,9 +373,9 @@ bool Perception::IsInteresting(float distance) {
return false; return false;
} }
InterestingThing *Perception::FindTrackedObject(char objectId) { InterestingThing* Perception::FindTrackedObject(char objectId) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx]; InterestingThing* thing = this->trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
if (thing->id == objectId) { if (thing->id == objectId) {
@ -374,10 +385,10 @@ InterestingThing *Perception::FindTrackedObject(char objectId) {
return nullptr; return nullptr;
} }
InterestingThing *Perception::FindTrackedObject(unsigned char networkId, InterestingThing* Perception::FindTrackedObject(unsigned char networkId,
unsigned char objectId) { unsigned char objectId) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx]; InterestingThing* thing = this->trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
@ -397,16 +408,16 @@ unsigned char Perception::TrackedObjectCount() {
return objectCount; return objectCount;
} }
InterestingThing **Perception::GetTrackedObjects() { InterestingThing** Perception::GetTrackedObjects() {
return this->trackedObjects; return this->trackedObjects;
} }
unsigned char Perception::ThingsOfType(unsigned char thingType, unsigned char Perception::ThingsOfType(unsigned char thingType,
InterestingThing *buffer[], InterestingThing* buffer[],
unsigned char bufferSize) { unsigned char bufferSize) {
unsigned char thingCount = 0; unsigned char thingCount = 0;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx]; InterestingThing* thing = this->trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
@ -424,9 +435,9 @@ unsigned char Perception::ThingsOfType(unsigned char thingType,
return thingCount; return thingCount;
} }
InterestingThing *Perception::ThingOfType(unsigned char thingType) { InterestingThing* Perception::ThingOfType(unsigned char thingType) {
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = this->trackedObjects[objIx]; InterestingThing* thing = this->trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
@ -436,14 +447,14 @@ InterestingThing *Perception::ThingOfType(unsigned char thingType) {
return nullptr; return nullptr;
} }
InterestingThing *Perception::GetMostInterestingThing() { InterestingThing* Perception::GetMostInterestingThing() {
if (this->trackedObjects == nullptr) if (this->trackedObjects == nullptr)
return nullptr; return nullptr;
InterestingThing *closestObject = nullptr; InterestingThing* closestObject = nullptr;
float closestDistance = INFINITY; float closestDistance = INFINITY;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *obj = this->trackedObjects[objIx]; InterestingThing* obj = this->trackedObjects[objIx];
if (obj != nullptr) { if (obj != nullptr) {
if (obj->position.distance < closestDistance) { if (obj->position.distance < closestDistance) {
closestObject = obj; closestObject = obj;
@ -463,23 +474,23 @@ void Perception::Update(unsigned long currentTimeMs) {
// Update sensing // Update sensing
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Sensor *sensor = sensors[sensorIx]; Sensor* sensor = sensors[sensorIx];
if (sensor == nullptr) if (sensor == nullptr)
continue; continue;
if (sensor->type == Thing::DistanceSensorType) { if (sensor->type == Thing::DistanceSensorType) {
DistanceSensor *distanceSensor = (DistanceSensor *)sensor; DistanceSensor* distanceSensor = (DistanceSensor*)sensor;
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);
} }
} else if (sensor->type == Thing::SwitchType) { } else if (sensor->type == Thing::SwitchType) {
Switch *switchSensor = (Switch *)sensor; Switch* switchSensor = (Switch*)sensor;
if (switchSensor->IsOn()) { if (switchSensor->IsOn()) {
// Polar position = Polar(sensor->position.angle, nearbyDistance); // Polar position = Polar(sensor->position.angle, nearbyDistance);
Polar position = Polar position =
@ -492,7 +503,7 @@ void Perception::Update(unsigned long currentTimeMs) {
} }
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
InterestingThing *thing = trackedObjects[objIx]; InterestingThing* thing = trackedObjects[objIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
@ -509,7 +520,7 @@ void Perception::Update(unsigned long currentTimeMs) {
void Perception::UpdatePose(Polar translation) { void Perception::UpdatePose(Polar translation) {
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing *thing = trackedObjects[thingIx]; InterestingThing* thing = trackedObjects[thingIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
@ -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;
@ -549,7 +562,7 @@ void Perception::UpdatePose(Quaternion rotation) {
rotationAngle = -rotationAngle; rotationAngle = -rotationAngle;
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing *thing = trackedObjects[thingIx]; InterestingThing* thing = trackedObjects[thingIx];
if (thing == nullptr) if (thing == nullptr)
continue; continue;
@ -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,

21
Roboid.cpp
View File

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

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

15
TrackedObject.cpp
View File

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