diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index 600aebc..db67a68 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -14,10 +14,10 @@ DifferentialDrive::DifferentialDrive(Motor* leftMotor, Motor* rightMotor) { float distance = this->wheelSeparation / 2; leftMotor->direction = Motor::Direction::CounterClockwise; - leftMotor->position.horizontal = -90; + leftMotor->position.horizontal = Angle16::Degrees(-90); leftMotor->position.distance = distance; rightMotor->direction = Motor::Direction::Clockwise; - rightMotor->position.horizontal = 90; + rightMotor->position.horizontal = Angle16::Degrees(90); rightMotor->position.distance = distance; } @@ -39,7 +39,7 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, if (motor == nullptr) continue; - float xPosition = motors[motorIx]->position.horizontal.ToFloat(); + float xPosition = motors[motorIx]->position.horizontal.InDegrees(); if (xPosition < 0) motor->SetTargetSpeed(leftSpeed); else if (xPosition > 0) @@ -71,7 +71,7 @@ void DifferentialDrive::SetTwistSpeed(Vector3 linear, } void DifferentialDrive::SetVelocity(Polar velocity) { - SetTwistSpeed(velocity.distance, velocity.angle.ToFloat()); + SetTwistSpeed(velocity.distance, velocity.angle.InDegrees()); } Polar DifferentialDrive::GetVelocity() { @@ -86,7 +86,8 @@ Polar DifferentialDrive::GetVelocity() { float direction = speed >= 0 ? 0.0F : 180.0F; float magnitude = fabsf(speed); - Polar velocity = Polar(magnitude, direction); // Polar(direction, magnitude); + Polar velocity = Polar( + magnitude, Angle::Degrees(direction)); // Polar(direction, magnitude); return velocity; } @@ -102,7 +103,8 @@ float DifferentialDrive::GetAngularVelocity() { float angularSpeed = (leftSpeed - rightSpeed) / 2; float angularDistance = wheelSeparation / 2 * Passer::LinearAlgebra::pi; float rotationsPerSecond = angularSpeed / angularDistance; - float degreesPerSecond = Angle::Normalize(360 * rotationsPerSecond).ToFloat(); + float degreesPerSecond = + Angle::Normalize(Angle::Degrees(360 * rotationsPerSecond)).InDegrees(); float angularVelocity = degreesPerSecond; return angularVelocity; diff --git a/LinearAlgebra b/LinearAlgebra index 1ea65d5..136e44e 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit 1ea65d56b1ee69407ff0ac2f9e1f232fed7e57e5 +Subproject commit 136e44e0001f382b6378cefa4cf17573d0b0dd98 diff --git a/NetworkSync.cpp b/NetworkSync.cpp index c9bb0bb..cd3227a 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -412,7 +412,7 @@ void NetworkSync::SendQuaternion(unsigned char* data, void NetworkSync::SendPolar(unsigned char* data, unsigned char* startIndex, Polar p) { - SendAngle8(data, *startIndex, (const float)p.angle.ToFloat()); + SendAngle8(data, *startIndex, (const float)p.angle.InDegrees()); SendSingle100(data, (*startIndex) + 1, p.distance); } @@ -453,7 +453,7 @@ void NetworkSync::SendQuat32(unsigned char* data, void NetworkSync::SendAngle8(unsigned char* data, unsigned int startIndex, const float angle) { - AngleOf packedAngle2 = AngleOf(angle); + Angle8 packedAngle2 = Angle8::Degrees(angle); data[startIndex] = packedAngle2.GetBinary(); } diff --git a/Perception.cpp b/Perception.cpp index df0ad50..b75639d 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -79,9 +79,9 @@ float GetPlaneDistance(InterestingThing* plane, float range) { float distance = plane->position.distance; float deltaAngle = - Angle::Normalize((float)plane->position.horizontal.ToFloat() - - horizontalAngle) - .ToFloat(); + Angle::Normalize(Angle::Degrees(plane->position.horizontal.InDegrees() - + horizontalAngle)) + .InDegrees(); if (fabsf(deltaAngle) < fabsf(range)) { // distance = distance // printf(" plane distance = %f (%f-%f)+%f=%f", distance, @@ -125,9 +125,9 @@ 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.horizontal.ToFloat() > + } else if (obj->position.horizontal.InDegrees() > horizontalDirection - range && - obj->position.horizontal.ToFloat() < + obj->position.horizontal.InDegrees() < horizontalDirection + range) { minDistance = fminf(minDistance, obj->position.distance); } @@ -152,8 +152,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.horizontal.ToFloat() > horizontalAngle - range && - thing->position.horizontal.ToFloat() < horizontalAngle + range) { + } else if (thing->position.horizontal.InDegrees() > + horizontalAngle - range && + thing->position.horizontal.InDegrees() < + horizontalAngle + range) { minDistance = fminf(minDistance, thing->position.distance); } } @@ -171,8 +173,8 @@ float Perception::GetDistance(float horizontalDirection, InterestingThing* obj = trackedObjects[objIx]; if (obj == nullptr) continue; - if (obj->position.horizontal.ToFloat() > horizontalDirection - range && - obj->position.horizontal.ToFloat() < horizontalDirection + range) { + if (obj->position.horizontal.InDegrees() > horizontalDirection - range && + obj->position.horizontal.InDegrees() < horizontalDirection + range) { minDistance = fminf(minDistance, obj->position.distance); } } @@ -188,8 +190,8 @@ bool Perception::ObjectNearby(float direction, float range) { if (obj == nullptr) continue; - if (obj->position.horizontal.ToFloat() > direction - range && - obj->position.horizontal.ToFloat() < direction + range) { + if (obj->position.horizontal.InDegrees() > direction - range && + obj->position.horizontal.InDegrees() < direction + range) { if (obj->position.distance <= nearbyDistance) return true; } @@ -409,8 +411,8 @@ void Perception::Update(unsigned long currentTimeMs) { Switch* switchSensor = (Switch*)sensor; if (switchSensor->IsOn()) { // Polar position = Polar(sensor->position.angle, nearbyDistance); - Polar position = - Polar(nearbyDistance, sensor->position.horizontal.ToFloat()); + Angle horizontal = Angle::Degrees(horizontal.InDegrees()); + Polar position = Polar(nearbyDistance, horizontal); // AddTrackedObject(switchSensor, position); } } else { @@ -447,8 +449,8 @@ 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.horizontal.ToFloat() + - translation.angle.ToFloat(); + float angle = (float)thing->position.horizontal.InDegrees() + + translation.angle.InDegrees(); angle = fabsf(angle); float deltaDistance = @@ -465,7 +467,8 @@ void Perception::UpdatePose(Polar translation) { // Spherical16 newPosition = Spherical16(horizontalPosition - // translation); Spherical16 translationS = Spherical16( - translation.distance, Angle16(translation.angle.ToFloat()), 0); + translation.distance, Angle16::Degrees(translation.angle.InDegrees()), + Angle16::Degrees(0)); Spherical16 newPosition = thing->position + translationS; thing->position = newPosition; } diff --git a/Roboid.cpp b/Roboid.cpp index 591e405..031c7af 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -99,7 +99,7 @@ void Roboid::SetPosition(Spherical16 newWorldPosition) { float distance = translation.distance; Angle16 angle = Spherical16::SignedAngleBetween( roboidOrientation * Spherical16::forward, translation, Spherical16::up); - Polar polarTranslation = Polar(angle.ToFloat(), distance); + Polar polarTranslation = Polar(angle.InDegrees(), Angle::Degrees(distance)); if (perception != nullptr) perception->UpdatePose(polarTranslation); this->worldPosition = newWorldPosition; diff --git a/ServoMotor.cpp b/ServoMotor.cpp index b9f0495..4867d4c 100644 --- a/ServoMotor.cpp +++ b/ServoMotor.cpp @@ -6,12 +6,13 @@ ServoMotor::ServoMotor() : Thing(0) { // for now, id should be set properly later this->type = Thing::ServoType; this->controlMode = ControlMode::Position; - this->targetAngle = 0; + this->targetAngle = Angle16(); this->hasTargetAngle = false; } void ServoMotor::SetTargetAngle(Angle16 angle) { - angle = Float::Clamp(angle.ToFloat(), minAngle.ToFloat(), maxAngle.ToFloat()); + angle = Angle16::Degrees(Float::Clamp(angle.InDegrees(), minAngle.InDegrees(), + maxAngle.InDegrees())); if (maxSpeed == 0.0F) { SetAngle(angle); @@ -68,7 +69,7 @@ void ServoMotor::Update(unsigned long currentTimeMs) { } else { float angleStep = maxSpeed * deltaTime; float deltaAngle = - this->targetAngle.ToFloat() - this->limitedTargetAngle.ToFloat(); + this->targetAngle.InDegrees() - this->limitedTargetAngle.InDegrees(); 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.ToFloat() - angleStep; + limitedTargetAngle = limitedTargetAngle - Angle16::Degrees(angleStep); else - limitedTargetAngle = limitedTargetAngle.ToFloat() + angleStep; + limitedTargetAngle = limitedTargetAngle + Angle16::Degrees(angleStep); } SetAngle(limitedTargetAngle); @@ -88,11 +89,13 @@ void ServoMotor::Update(unsigned long currentTimeMs) { // Velocity control } else { - float newAngle = this->targetAngle.ToFloat() + targetVelocity * deltaTime; - newAngle = Float::Clamp(newAngle, minAngle.ToFloat(), maxAngle.ToFloat()); + float newAngle = this->targetAngle.InDegrees() + targetVelocity * deltaTime; + newAngle = + Float::Clamp(newAngle, minAngle.InDegrees(), maxAngle.InDegrees()); - ServoMotor::SetTargetAngle(newAngle); - SetAngle(newAngle); + Angle16 targetAngle = Angle16::Degrees(newAngle); + ServoMotor::SetTargetAngle(targetAngle); + SetAngle(targetAngle); this->lastUpdateTimeMs = currentTimeMs; } diff --git a/ServoMotor.h b/ServoMotor.h index 1045c16..4282818 100644 --- a/ServoMotor.h +++ b/ServoMotor.h @@ -11,8 +11,8 @@ class ServoMotor : public Thing { ServoMotor(); Vector3 rotationAxis = Vector3::up; - Angle minAngle = -90.0F; - Angle maxAngle = 90.0F; + Angle minAngle = Angle::Degrees(-90); + Angle maxAngle = Angle::Degrees(90); enum ControlMode { Position, Velocity }; ControlMode controlMode = ControlMode::Position; @@ -29,12 +29,12 @@ class ServoMotor : public Thing { virtual void Update(unsigned long currentTimeMs) override; - Angle16 limitedTargetAngle = 0.0F; + Angle16 limitedTargetAngle = Angle16(); protected: bool hasTargetAngle = false; - Angle16 targetAngle = 0.0F; - Angle16 actualAngle = 0.0F; + Angle16 targetAngle = Angle16(); + Angle16 actualAngle = Angle16(); float maxSpeed = 0.0F; diff --git a/TrackedObject.cpp b/TrackedObject.cpp index 49892e0..03652fe 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -6,8 +6,9 @@ InterestingThing::InterestingThing(Sensor* sensor, Polar position) { this->id = 0; this->confidence = maxConfidence; this->sensor = sensor; - this->position = - Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0); + this->position = Spherical16(position.distance, + Angle16::Degrees(position.angle.InDegrees()), + Angle16::Degrees(0)); this->updated = true; } @@ -45,11 +46,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.horizontal.ToFloat() - - otherObj->position.horizontal.ToFloat()) > equalAngle) + if (fabsf(position.horizontal.InDegrees() - + otherObj->position.horizontal.InDegrees()) > equalAngle) return false; - if (fabsf(position.vertical.ToFloat() - - otherObj->position.vertical.ToFloat()) > equalAngle) + if (fabsf(position.vertical.InDegrees() - + otherObj->position.vertical.InDegrees()) > equalAngle) return false; // printf(" -> yes "); return true;