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

Updated LinearAlgebra

Browse Source
This commit is contained in:
Pascal Serrarens 2024-05-14 09:21:50 +02:00
parent afa1809cb7
commit eb15b11652
5 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
DifferentialDrive.cpp
View File

@ -65,7 +65,7 @@ void DifferentialDrive::SetTwistSpeed(Vector2 linear, float 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.z, yaw); SetTwistSpeed(linear.Forward(), yaw);
} }
void DifferentialDrive::SetVelocity(Polar velocity) { void DifferentialDrive::SetVelocity(Polar velocity) {

2
LinearAlgebra

@ -1 +1 @@
Subproject commit 5489b3c45cb2eded59262983c2a5f23340465f26 Subproject commit ee62cbae3ec63b7847df242e02b453a43defa5de

30
NetworkSync.cpp
View File

@ -29,11 +29,11 @@ void NetworkSync::ReceiveNetworkId() {
void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex, void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
const Vector3 v) { const Vector3 v) {
SendSingle100(data, *startIndex, v.x); SendSingle100(data, *startIndex, v.Right());
(*startIndex) += 4; (*startIndex) += 4;
SendSingle100(data, *startIndex, v.y); SendSingle100(data, *startIndex, v.Up());
(*startIndex) += 4; (*startIndex) += 4;
SendSingle100(data, *startIndex, v.z); SendSingle100(data, *startIndex, v.Forward());
(*startIndex) += 4; (*startIndex) += 4;
} }
@ -41,9 +41,9 @@ void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
const Quaternion q) { const Quaternion q) {
Vector3 angles = Quaternion::ToAngles(q); Vector3 angles = Quaternion::ToAngles(q);
int ix = startIndex; int ix = startIndex;
SendAngle8(data, ix++, angles.x); SendAngle8(data, ix++, angles.Right());
SendAngle8(data, ix++, angles.y); SendAngle8(data, ix++, angles.Up());
SendAngle8(data, ix++, angles.z); SendAngle8(data, ix++, angles.Forward());
} }
void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex, void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex,
@ -175,11 +175,11 @@ void NetworkSync::PublishTrackedObject(Buffer sendBuffer,
Serial.print("Send Pose "); Serial.print("Send Pose ");
Serial.print((int)object->id); Serial.print((int)object->id);
Serial.print(" Position "); Serial.print(" Position ");
Serial.print(worldPosition3.x); Serial.print(worldPosition3.Right());
Serial.print(", "); Serial.print(", ");
Serial.print(worldPosition3.y); Serial.print(worldPosition3.Up());
Serial.print(", "); Serial.print(", ");
Serial.println(worldPosition3.z); Serial.println(worldPosition3.Forward());
#else #else
const UInt16 bufferSize = 3 + 12; const UInt16 bufferSize = 3 + 12;
UInt8 buffer[bufferSize] = { UInt8 buffer[bufferSize] = {
@ -219,17 +219,17 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
#ifdef RC_DEBUG #ifdef RC_DEBUG
Serial.print("Send Pose 0 LinearVelocity "); Serial.print("Send Pose 0 LinearVelocity ");
Serial.print(worldVelocity3.x); Serial.print(worldVelocity3.Right());
Serial.print(", "); Serial.print(", ");
Serial.print(worldVelocity3.y); Serial.print(worldVelocity3.Up());
Serial.print(", "); Serial.print(", ");
Serial.print(worldVelocity3.z); Serial.print(worldVelocity3.Forward());
Serial.print(" AngularVelocity "); Serial.print(" AngularVelocity ");
Serial.print(worldAngularVelocity.x); Serial.print(worldAngularVelocity.Right());
Serial.print(", "); Serial.print(", ");
Serial.print(worldAngularVelocity.y); Serial.print(worldAngularVelocity.Up());
Serial.print(", "); Serial.print(", ");
Serial.println(worldAngularVelocity.z); Serial.println(worldAngularVelocity.Forward());
#else #else
const unsigned int bufferSize = 3 + 12 + 12; const unsigned int bufferSize = 3 + 12 + 12;
unsigned char buffer[bufferSize] = { unsigned char buffer[bufferSize] = {

2
Perception.cpp
View File

@ -549,7 +549,7 @@ void Perception::UpdatePose(Quaternion rotation) {
Vector3 rotationAxis; Vector3 rotationAxis;
rotation.ToAngleAxis(&rotationAngle, &rotationAxis); rotation.ToAngleAxis(&rotationAngle, &rotationAxis);
// Make sure rotation axis is positive // Make sure rotation axis is positive
if (rotationAxis.y < 0) if (rotationAxis.Up() < 0)
rotationAngle = -rotationAngle; rotationAngle = -rotationAngle;
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {

2
Roboid.cpp
View File

@ -73,7 +73,7 @@ void Roboid::Update(float currentTimeMs) {
Vector3 Roboid::GetPosition() { return this->worldPosition; } Vector3 Roboid::GetPosition() { return this->worldPosition; }
Vector2 Roboid::GetPosition2D() { Vector2 Roboid::GetPosition2D() {
return Vector2(this->worldPosition.x, this->worldPosition.z); return Vector2(this->worldPosition.Right(), this->worldPosition.Forward());
} }
#include <Arduino.h> #include <Arduino.h>