diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index 4998514..b4f3933 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -4,11 +4,11 @@ #include -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; diff --git a/LinearAlgebra b/LinearAlgebra index 608c45c..49c6740 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit 608c45c1a7b7f5469cfdcd1a8ff30036111aefb2 +Subproject commit 49c67405fcbf15b3470080e67ab203c835f1f9b8 diff --git a/NetworkSync.cpp b/NetworkSync.cpp index dbe55ca..d22b6d4 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -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); } diff --git a/Perception.cpp b/Perception.cpp index 020a0eb..0866944 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -11,57 +11,59 @@ #include #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 // 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, diff --git a/Roboid.cpp b/Roboid.cpp index 6bef157..5348e59 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -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); } } diff --git a/ServoMotor.cpp b/ServoMotor.cpp index c21294b..d388ed1 100644 --- a/ServoMotor.cpp +++ b/ServoMotor.cpp @@ -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); diff --git a/TrackedObject.cpp b/TrackedObject.cpp index b27e378..3a60bba 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -2,7 +2,7 @@ #include -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 -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;