RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix incompatibilities

Browse Source
This commit is contained in:
Pascal Serrarens 2024-09-24 11:33:07 +02:00
parent 60516dec0d
commit 84654af2c3
8 changed files with 55 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
DifferentialDrive.cpp
View File

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

2
LinearAlgebra

@ -1 +1 @@
Subproject commit 1ea65d56b1ee69407ff0ac2f9e1f232fed7e57e5
Subproject commit 136e44e0001f382b6378cefa4cf17573d0b0dd98

4
NetworkSync.cpp
View File

@ -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<signed char> packedAngle2 = AngleOf<signed char>(angle);
Angle8 packedAngle2 = Angle8::Degrees(angle);
data[startIndex] = packedAngle2.GetBinary();
}

35
Perception.cpp
View File

@ -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;
}

2
Roboid.cpp
View File

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

21
ServoMotor.cpp
View File

@ -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;
}

10
ServoMotor.h
View File

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

13
TrackedObject.cpp
View File

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