diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index bf2c3d0..79ef43f 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -65,7 +65,7 @@ void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) { void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch, float roll) { - SetTwistSpeed(linear.z, yaw); + SetTwistSpeed(linear.Forward(), yaw); } void DifferentialDrive::SetVelocity(Polar velocity) { diff --git a/LinearAlgebra b/LinearAlgebra index 5489b3c..ee62cba 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit 5489b3c45cb2eded59262983c2a5f23340465f26 +Subproject commit ee62cbae3ec63b7847df242e02b453a43defa5de diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 3f238d2..bcdaf67 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -29,11 +29,11 @@ void NetworkSync::ReceiveNetworkId() { void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex, const Vector3 v) { - SendSingle100(data, *startIndex, v.x); + SendSingle100(data, *startIndex, v.Right()); (*startIndex) += 4; - SendSingle100(data, *startIndex, v.y); + SendSingle100(data, *startIndex, v.Up()); (*startIndex) += 4; - SendSingle100(data, *startIndex, v.z); + SendSingle100(data, *startIndex, v.Forward()); (*startIndex) += 4; } @@ -41,9 +41,9 @@ void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex, const Quaternion q) { Vector3 angles = Quaternion::ToAngles(q); int ix = startIndex; - SendAngle8(data, ix++, angles.x); - SendAngle8(data, ix++, angles.y); - SendAngle8(data, ix++, angles.z); + SendAngle8(data, ix++, angles.Right()); + SendAngle8(data, ix++, angles.Up()); + SendAngle8(data, ix++, angles.Forward()); } void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex, @@ -175,11 +175,11 @@ void NetworkSync::PublishTrackedObject(Buffer sendBuffer, Serial.print("Send Pose "); Serial.print((int)object->id); Serial.print(" Position "); - Serial.print(worldPosition3.x); + Serial.print(worldPosition3.Right()); Serial.print(", "); - Serial.print(worldPosition3.y); + Serial.print(worldPosition3.Up()); Serial.print(", "); - Serial.println(worldPosition3.z); + Serial.println(worldPosition3.Forward()); #else const UInt16 bufferSize = 3 + 12; UInt8 buffer[bufferSize] = { @@ -219,17 +219,17 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) { #ifdef RC_DEBUG Serial.print("Send Pose 0 LinearVelocity "); - Serial.print(worldVelocity3.x); + Serial.print(worldVelocity3.Right()); Serial.print(", "); - Serial.print(worldVelocity3.y); + Serial.print(worldVelocity3.Up()); Serial.print(", "); - Serial.print(worldVelocity3.z); + Serial.print(worldVelocity3.Forward()); Serial.print(" AngularVelocity "); - Serial.print(worldAngularVelocity.x); + Serial.print(worldAngularVelocity.Right()); Serial.print(", "); - Serial.print(worldAngularVelocity.y); + Serial.print(worldAngularVelocity.Up()); Serial.print(", "); - Serial.println(worldAngularVelocity.z); + Serial.println(worldAngularVelocity.Forward()); #else const unsigned int bufferSize = 3 + 12 + 12; unsigned char buffer[bufferSize] = { diff --git a/Perception.cpp b/Perception.cpp index 48f5173..b38c9a5 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -549,7 +549,7 @@ void Perception::UpdatePose(Quaternion rotation) { Vector3 rotationAxis; rotation.ToAngleAxis(&rotationAngle, &rotationAxis); // Make sure rotation axis is positive - if (rotationAxis.y < 0) + if (rotationAxis.Up() < 0) rotationAngle = -rotationAngle; for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { diff --git a/Roboid.cpp b/Roboid.cpp index 7909e59..82b0b69 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -73,7 +73,7 @@ void Roboid::Update(float currentTimeMs) { Vector3 Roboid::GetPosition() { return this->worldPosition; } Vector2 Roboid::GetPosition2D() { - return Vector2(this->worldPosition.x, this->worldPosition.z); + return Vector2(this->worldPosition.Right(), this->worldPosition.Forward()); } #include