diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp
index 79ef43f..4998514 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.angle = -90;
+ leftMotor->position.horizontalAngle = -90;
leftMotor->position.distance = distance;
rightMotor->direction = Motor::Direction::Clockwise;
- rightMotor->position.angle = 90;
+ rightMotor->position.horizontalAngle = 90;
rightMotor->position.distance = distance;
}
@@ -39,7 +39,7 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
if (motor == nullptr)
continue;
- float xPosition = motors[motorIx]->position.angle;
+ float xPosition = motors[motorIx]->position.horizontalAngle;
if (xPosition < 0)
motor->SetTargetSpeed(leftSpeed);
else if (xPosition > 0)
diff --git a/Motor.cpp b/Motor.cpp
index 3b5eb4f..c005d72 100644
--- a/Motor.cpp
+++ b/Motor.cpp
@@ -2,7 +2,9 @@
#include <time.h>
-Motor::Motor() { type = (int)Thing::UncontrolledMotorType; }
+Motor::Motor() : Thing(0) { // for now, id should be set properly later
+ this->type = (int)Thing::UncontrolledMotorType;
+}
float Motor::GetActualSpeed() { return this->currentTargetSpeed; }
diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index abd2014..6e60913 100644
--- a/NetworkSync.cpp
+++ b/NetworkSync.cpp
@@ -25,12 +25,18 @@ void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
}
}
+#include <Arduino.h>
+
void NetworkSync::ReceiveNetworkId() {
this->networkId = buffer[1];
#ifdef RC_DEBUG
printf("_Received network Id %d\n", this->networkId);
#endif
+ delay(100);
PublishModel(roboid);
+ delay(100);
+ if (roboid->actuationRoot != nullptr)
+ PublishRelativeThing(roboid->actuationRoot, true);
}
void NetworkSync::NewObject(InterestingThing *thing) {
@@ -50,29 +56,71 @@ void NetworkSync::NewObject(InterestingThing *thing) {
// PublishTrackedObject(roboid, obj);
}
+void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) {
+ Thing *parentThing = thing->GetParent();
+
+ unsigned char ix = 0;
+ buffer[ix++] = RelativePoseMsg;
+ buffer[ix++] = thing->id;
+ if (parentThing != nullptr)
+ buffer[ix++] = parentThing->id;
+ else
+ buffer[ix++] = 0x00;
+ SendSpherical(buffer, &ix, thing->position);
+ SendBuffer(ix);
+
+ delay(100);
+ PublishModel(thing);
+
+ delay(1000);
+ if (recurse) {
+ Thing *child = thing->GetChild(0);
+ if (child != nullptr)
+ PublishRelativeThing(child, true);
+ }
+}
+
void NetworkSync::PublishModel(Roboid *roboid) {
if (roboid->modelUrl == nullptr)
return;
- int len = strlen(roboid->modelUrl);
+ unsigned char len = strlen(roboid->modelUrl);
if (len > 255)
return;
unsigned char ix = 0;
buffer[ix++] = 0x90; // modelMsg
buffer[ix++] = 0x00; // objectId
- // SendVector3(buffer, &ix, position);
- // Polar polar = Polar(Vector2(position));
- // SendPolar(buffer, &ix, polar);
Spherical s = Spherical(roboid->modelPosition);
SendSpherical(buffer, &ix, s);
SendFloat16(buffer, &ix, roboid->modelScale);
buffer[ix++] = len;
- // printf("send url %d, scale = %f \n", ix, sscale);
- for (int urlIx = 0; urlIx < len; urlIx++) {
+ for (int urlIx = 0; urlIx < len; urlIx++)
buffer[ix++] = roboid->modelUrl[urlIx];
- }
+
+ SendBuffer(ix);
+}
+
+void NetworkSync::PublishModel(Thing *thing) {
+ if (thing->modelUrl == nullptr)
+ return;
+
+ unsigned char len = strlen(thing->modelUrl);
+ if (len > 255)
+ return;
+
+ unsigned char ix = 0;
+ buffer[ix++] = 0x90; // modelMsg
+ buffer[ix++] = thing->id; // objectId
+ Spherical s = Spherical(thing->modelPosition);
+ SendSpherical(buffer, &ix, s);
+ SendFloat16(buffer, &ix, thing->modelScale);
+
+ buffer[ix++] = len;
+ for (int urlIx = 0; urlIx < len; urlIx++)
+ buffer[ix++] = thing->modelUrl[urlIx];
+
SendBuffer(ix);
}
@@ -87,151 +135,61 @@ void NetworkSync::DestroyObject(InterestingThing *thing) {
#endif
}
-void NetworkSync::SendPose(Roboid *roboid) {
- if (roboid->propulsion == nullptr)
- return;
+void NetworkSync::SendPose(Roboid *roboid, bool recurse) {
+ // if (roboid->propulsion == nullptr)
+ // return;
- Polar velocity = roboid->propulsion->GetVelocity();
- Vector2 worldVelocity2 =
- Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
- Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y);
+ // Polar velocity = roboid->propulsion->GetVelocity();
+ // Vector2 worldVelocity2 =
+ // Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
+ // Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y);
- float angularVelocity = roboid->propulsion->GetAngularVelocity();
- Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0);
+ // float angularVelocity = roboid->propulsion->GetAngularVelocity();
+ // Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0);
- unsigned char buffer[3 + 12 + 12] = {
- PoseMsg,
- 0, // objectId;
- Pose_LinearVelocity | Pose_AngularVelocity,
- };
- unsigned char ix = 3;
- SendVector3(buffer, &ix, worldVelocity3);
- SendVector3(buffer, &ix, worldAngularVelocity);
+ // unsigned char buffer[3 + 12 + 12] = {
+ // PoseMsg,
+ // 0, // objectId;
+ // Pose_LinearVelocity | Pose_AngularVelocity,
+ // };
+ // unsigned char ix = 3;
+ // SendVector3(buffer, &ix, worldVelocity3);
+ // SendVector3(buffer, &ix, worldAngularVelocity);
+ // SendBuffer(ix);
+
+ unsigned char ix = 0;
+ buffer[ix++] = PoseMsg;
+ buffer[ix++] = 0x00;
+ buffer[ix++] = Pose_Position | Pose_Orientation;
+ SendSpherical(buffer, &ix, Spherical(roboid->GetPosition()));
+ SendQuat32(buffer, &ix, roboid->GetOrientation());
SendBuffer(ix);
-}
-void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
- const Vector3 v) {
- SendSingle100(data, *startIndex, v.Right());
- (*startIndex) += 4;
- SendSingle100(data, *startIndex, v.Up());
- (*startIndex) += 4;
- SendSingle100(data, *startIndex, v.Forward());
- (*startIndex) += 4;
-}
-
-void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
- const Quaternion q) {
- Vector3 angles = Quaternion::ToAngles(q);
- int ix = startIndex;
- SendAngle8(data, ix++, angles.Right());
- SendAngle8(data, ix++, angles.Up());
- SendAngle8(data, ix++, angles.Forward());
-}
-
-void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex,
- Polar p) {
- SendAngle8(data, *startIndex, (const float)p.angle);
- 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.distance);
- SendFloat16(data, startIndex, s.distance);
-}
-
-// void NetworkSync::SendSpherical16(unsigned char *data, int startIndex,
-// Spherical s) {
-// SendAngle16(data, startIndex, s.horizontalAngle);
-// SendAngle16(data, startIndex += 2, s.verticalAngle);
-// SendSingle100(data, startIndex += 2, s.distance);
-// }
-
-// void NetworkSync::SendSpherical32(unsigned char *data, int startIndex,
-// Spherical s) {
-// SendAngle32(data, startIndex, s.horizontalAngle);
-// SendAngle32(data, startIndex += 4, s.verticalAngle);
-// SendSingle100(data, startIndex += 4, s.distance);
-// }
-
-void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex,
- const Quaternion q) {
- unsigned char qx = (char)(q.x * 127 + 128);
- unsigned char qy = (char)(q.y * 127 + 128);
- unsigned char qz = (char)(q.z * 127 + 128);
- unsigned char qw = (char)(q.w * 255);
- if (q.w < 0) {
- qx = -qx;
- qy = -qy;
- qz = -qz;
- qw = -qw;
- }
- // Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
- data[(*startIndex)++] = qx;
- data[(*startIndex)++] = qy;
- data[(*startIndex)++] = qz;
- data[(*startIndex)++] = qw;
-}
-
-void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
- const float angle) {
- AngleUsing<signed char> packedAngle = AngleUsing<signed char>(angle);
- data[startIndex] = packedAngle.GetValue();
-}
-
-// void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex,
-// const float angle) {
-// AngleUsing<signed short> packedAngle = AngleUsing<signed short>(angle);
-// signed short value = packedAngle.GetValue();
-// data[startIndex] = value >> 8;
-// data[startIndex + 1] = value & 0xFF;
-// }
-
-// void NetworkSync::SendAngle32(unsigned char *data, unsigned int startIndex,
-// const float angle) {
-// AngleUsing<signed long> packedAngle = AngleUsing<signed long>(angle);
-// unsigned long value = packedAngle.GetValue();
-// data[startIndex] = value >> 24 & 0xFF;
-// data[startIndex + 1] = value >> 16 & 0xFF;
-// data[startIndex + 2] = value >> 8 & 0xFF;
-// data[startIndex + 3] = value & 0xFF;
-// // Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
-// // data[startIndex + 1], data[startIndex + 2],
-// // data[startIndex + 3]);
-// }
-
-void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex,
- float value) {
- // Sends a float with truncated 2 decimal precision
- Int32 intValue = value * 100;
- SendInt32(data, startIndex, intValue);
- // for (unsigned char ix = 0; ix < 4; ix++) {
- // data[startIndex + ix] = ((unsigned char *)&intValue)[ix];
- // }
-}
-void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex,
- float value) {
- float16 value16 = float16(value);
- short binary = value16.getBinary();
-
- data[(*startIndex)++] = (binary >> 8) & 0xFF;
- data[(*startIndex)++] = binary & 0xFF;
-}
-
-void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex,
- Int32 value) {
- for (unsigned char ix = 0; ix < 4; ix++) {
- data[startIndex++] = ((unsigned char *)&value)[ix];
+ if (recurse) {
+ delay(10);
+ Thing *child = roboid->actuationRoot;
+ if (child != nullptr)
+ SendPose(child, true);
}
}
-// static unsigned char buffer[100];
+void NetworkSync::SendPose(Thing *thing, bool recurse) {
+ unsigned char ix = 0;
+ buffer[ix++] = PoseMsg;
+ buffer[ix++] = thing->id;
+ buffer[ix++] = Pose_Position | Pose_Orientation;
+ SendSpherical(buffer, &ix, thing->position);
+ SendQuat32(buffer, &ix, thing->orientation);
+ SendBuffer(ix);
-void NetworkSync::SendBuffer(unsigned char bufferSize) {}
+ if (recurse) {
+ delay(10);
+ Thing *child = thing->GetChild(0);
+ if (child != nullptr)
+ SendPose(child, true);
+ }
+}
void NetworkSync::PublishClient() {
unsigned char ix = 0;
@@ -242,6 +200,10 @@ void NetworkSync::PublishClient() {
#ifdef RC_DEBUG
printf("Sent new Client\n");
#endif
+
+ // PublishModel(roboid);
+ // if (roboid->actuationRoot != nullptr)
+ // PublishRelativeThing(roboid->actuationRoot, false);
}
void NetworkSync::PublishTrackedObjects(Roboid *roboid,
@@ -296,68 +258,6 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
#endif
}
-// void NetworkSync::PublishTrackedObjects(Buffer sendBuffer,
-// InterestingThing **objects) {
-// for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++)
-// {
-// InterestingThing *obj = objects[objIx];
-// if (obj == nullptr)
-// continue;
-// if (obj->networkId != 0x00)
-// continue; // object is external
-// // if (obj->sensor->type == Thing::ExternalType)
-// // continue;
-
-// // tmp
-// obj->id = objIx;
-// PublishTrackedObject(sendBuffer, obj);
-// }
-// }
-
-// void NetworkSync::PublishTrackedObject(Buffer sendBuffer,
-// InterestingThing *object) {
-// Vector2 worldPosition2 =
-// Vector2::Rotate(Vector2::forward * object->position.distance,
-// -object->position.horizontalAngle);
-// Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y);
-
-// #ifdef RC_DEBUG
-// Serial.print("Send Pose ");
-// Serial.print((int)object->id);
-// Serial.print(" Position ");
-// Serial.print(worldPosition3.Right());
-// Serial.print(", ");
-// Serial.print(worldPosition3.Up());
-// Serial.print(", ");
-// Serial.println(worldPosition3.Forward());
-// #else
-// const UInt16 bufferSize = 3 + 12;
-// UInt8 buffer[bufferSize] = {
-// PoseMsg,
-// (UInt8)object->id,
-// Pose_Position,
-// };
-// unsigned char ix = 3;
-// SendVector3(buffer, &ix, worldPosition3);
-// sendBuffer(buffer, bufferSize);
-// #endif
-// }
-
-// void NetworkSync::PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
-// InterestingThing *object) {
-// const UInt16 bufferSize = 4 + 12;
-// UInt8 buffer[bufferSize] = {
-// RelativePoseMsg,
-// (UInt8)parentId,
-// (UInt8)object->id,
-// Pose_Position,
-// };
-// unsigned char ix = 4;
-// SendSpherical(buffer, &ix, object->position);
-// // SendVector3(buffer, ix, worldPosition3);
-// sendBuffer(buffer, bufferSize);
-// }
-
void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
Polar velocity = roboid->propulsion->GetVelocity();
Vector2 worldVelocity2 =
@@ -432,4 +332,112 @@ void NetworkSync::SendText(const char *s) {
buffer[ix++] = s[urlIx];
SendBuffer(ix);
-}
\ No newline at end of file
+}
+
+// Low-level functions
+
+void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
+ const Vector3 v) {
+ SendSingle100(data, *startIndex, v.Right());
+ (*startIndex) += 4;
+ SendSingle100(data, *startIndex, v.Up());
+ (*startIndex) += 4;
+ SendSingle100(data, *startIndex, v.Forward());
+ (*startIndex) += 4;
+}
+
+void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
+ const Quaternion q) {
+ Vector3 angles = Quaternion::ToAngles(q);
+ int ix = startIndex;
+ SendAngle8(data, ix++, angles.Right());
+ SendAngle8(data, ix++, angles.Up());
+ SendAngle8(data, ix++, angles.Forward());
+}
+
+void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex,
+ Polar p) {
+ SendAngle8(data, *startIndex, (const float)p.angle);
+ 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.distance);
+ SendFloat16(data, startIndex, s.distance);
+}
+
+void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex,
+ const Quaternion q) {
+ unsigned char qx = (char)(q.x * 127 + 128);
+ unsigned char qy = (char)(q.y * 127 + 128);
+ unsigned char qz = (char)(q.z * 127 + 128);
+ unsigned char qw = (char)(q.w * 255);
+ if (q.w < 0) {
+ qx = -qx;
+ qy = -qy;
+ qz = -qz;
+ qw = -qw;
+ }
+ // Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
+ data[(*startIndex)++] = qx;
+ data[(*startIndex)++] = qy;
+ data[(*startIndex)++] = qz;
+ data[(*startIndex)++] = qw;
+}
+
+void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
+ const float angle) {
+ AngleUsing<signed char> packedAngle = AngleUsing<signed char>(angle);
+ data[startIndex] = packedAngle.GetValue();
+}
+
+// void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex,
+// const float angle) {
+// AngleUsing<signed short> packedAngle = AngleUsing<signed short>(angle);
+// signed short value = packedAngle.GetValue();
+// data[startIndex] = value >> 8;
+// data[startIndex + 1] = value & 0xFF;
+// }
+
+// void NetworkSync::SendAngle32(unsigned char *data, unsigned int startIndex,
+// const float angle) {
+// AngleUsing<signed long> packedAngle = AngleUsing<signed long>(angle);
+// unsigned long value = packedAngle.GetValue();
+// data[startIndex] = value >> 24 & 0xFF;
+// data[startIndex + 1] = value >> 16 & 0xFF;
+// data[startIndex + 2] = value >> 8 & 0xFF;
+// data[startIndex + 3] = value & 0xFF;
+// // Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
+// // data[startIndex + 1], data[startIndex + 2],
+// // data[startIndex + 3]);
+// }
+
+void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex,
+ float value) {
+ // Sends a float with truncated 2 decimal precision
+ Int32 intValue = value * 100;
+ SendInt32(data, startIndex, intValue);
+ // for (unsigned char ix = 0; ix < 4; ix++) {
+ // data[startIndex + ix] = ((unsigned char *)&intValue)[ix];
+ // }
+}
+void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex,
+ float value) {
+ float16 value16 = float16(value);
+ short binary = value16.getBinary();
+
+ data[(*startIndex)++] = (binary >> 8) & 0xFF;
+ data[(*startIndex)++] = binary & 0xFF;
+}
+
+void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex,
+ Int32 value) {
+ for (unsigned char ix = 0; ix < 4; ix++) {
+ data[startIndex++] = ((unsigned char *)&value)[ix];
+ }
+}
+
+void NetworkSync::SendBuffer(unsigned char bufferSize) {}
diff --git a/NetworkSync.h b/NetworkSync.h
index a0253ca..bba05f2 100644
--- a/NetworkSync.h
+++ b/NetworkSync.h
@@ -23,6 +23,7 @@ public:
virtual void DestroyObject(InterestingThing *obj);
virtual void NewObject(InterestingThing *obj);
virtual void PublishModel(Roboid *obj);
+ void PublishModel(Thing *thing);
/// @brief The id of a Pose message
static const unsigned char PoseMsg = 0x10;
@@ -62,12 +63,14 @@ public:
void SendPoseMsg(Buffer sendBuffer, Roboid *roboid);
void SendDestroyObject(Buffer sendBuffer, InterestingThing *obj);
void PublishNewObject();
+ void PublishRelativeThing(Thing *thing, bool recurse = false);
void PublishTrackedObjects(Roboid *roboid, InterestingThing **objects);
virtual void SendPosition(Vector3 worldPosition) {};
virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {};
- void SendPose(Roboid *roboid);
+ void SendPose(Roboid *roboid, bool recurse = true);
+ void SendPose(Thing *thing, bool recurse = true);
void SendText(const char *s);
diff --git a/Perception.cpp b/Perception.cpp
index d813b10..ef2e96b 100644
--- a/Perception.cpp
+++ b/Perception.cpp
@@ -478,7 +478,7 @@ void Perception::Update(float currentTimeMs) {
float distance = distanceSensor->GetDistance();
if (distance >= 0) {
- float angle = sensor->position.angle;
+ float angle = sensor->position.horizontalAngle;
// Polar position = Polar(angle, distance);
Polar position = Polar(distance, angle);
AddTrackedObject(distanceSensor, position);
@@ -488,7 +488,8 @@ void Perception::Update(float currentTimeMs) {
Switch *switchSensor = (Switch *)sensor;
if (switchSensor->IsOn()) {
// Polar position = Polar(sensor->position.angle, nearbyDistance);
- Polar position = Polar(nearbyDistance, sensor->position.angle);
+ Polar position =
+ Polar(nearbyDistance, sensor->position.horizontalAngle);
// AddTrackedObject(switchSensor, position);
}
} else {
diff --git a/Roboid.cpp b/Roboid.cpp
index 23a8016..f2a0417 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -64,12 +64,12 @@ void Roboid::Update(float currentTimeMs) {
Vector3::up));
}
- if (networkSync != nullptr)
- networkSync->NetworkUpdate(this);
-
if (actuationRoot != nullptr)
actuationRoot->Update(currentTimeMs);
+ if (networkSync != nullptr)
+ networkSync->NetworkUpdate(this);
+
lastUpdateTimeMs = currentTimeMs;
}
diff --git a/Sensor.cpp b/Sensor.cpp
index 82b20f0..3808d15 100644
--- a/Sensor.cpp
+++ b/Sensor.cpp
@@ -1,6 +1,5 @@
#include "Sensor.h"
-Sensor::Sensor() {
- // this->isSensor = true;
- type = Thing::SensorType;
+Sensor::Sensor() : Thing(0) { // for now, id should be set properly later
+ this->type = Thing::SensorType;
}
\ No newline at end of file
diff --git a/ServoMotor.cpp b/ServoMotor.cpp
index 451f2ac..641c57c 100644
--- a/ServoMotor.cpp
+++ b/ServoMotor.cpp
@@ -2,7 +2,8 @@
#include "LinearAlgebra/FloatSingle.h"
-ServoMotor::ServoMotor() {
+ServoMotor::ServoMotor()
+ : Thing(0) { // for now, id should be set properly later
this->type = Thing::ServoType;
this->controlMode = ControlMode::Position;
this->targetAngle = 0;
diff --git a/ServoMotor.h b/ServoMotor.h
index a32583e..0c8b795 100644
--- a/ServoMotor.h
+++ b/ServoMotor.h
@@ -9,6 +9,7 @@ class ServoMotor : public Thing {
public:
ServoMotor();
+ Vector3 rotationAxis = Vector3::up;
float minAngle = -90.0F;
float maxAngle = 90.0F;
diff --git a/Thing.cpp b/Thing.cpp
index 537a691..5fddcf6 100644
--- a/Thing.cpp
+++ b/Thing.cpp
@@ -2,7 +2,7 @@
using namespace Passer::RoboidControl;
-Thing::Thing() : position(Polar::zero) {
+Thing::Thing(unsigned char id) : position(Polar::zero), id(id) {
this->type = (unsigned int)Type::Undetermined;
this->childCount = 0;
this->parent = nullptr;
@@ -23,6 +23,14 @@ bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; }
bool Thing::IsSensor() { return (type & Thing::SensorType) != 0; }
+void Thing::SetModel(const char *url, Vector3 position, Quaternion orientation,
+ float scale) {
+ this->modelUrl = url;
+ this->modelPosition = position;
+ this->modelOrientation = orientation;
+ this->modelScale = scale;
+}
+
void Thing::SetParent(Thing *parent) { this->parent = parent; }
Thing *Thing::GetParent() { return this->parent; }
diff --git a/Thing.h b/Thing.h
index 7bcb8da..bd1ec14 100644
--- a/Thing.h
+++ b/Thing.h
@@ -1,6 +1,7 @@
#pragma once
#include "LinearAlgebra/Polar.h"
+#include "LinearAlgebra/Quaternion.h"
namespace Passer {
namespace RoboidControl {
@@ -9,8 +10,10 @@ namespace RoboidControl {
class Thing {
public:
/// @brief Default constructor for a Thing
- Thing();
+ Thing(unsigned char id);
+ /// @char The id of the thing
+ unsigned char id;
/// @brief The type of Thing
unsigned int type;
@@ -33,7 +36,15 @@ public:
/// @returns True when the Thing is a Sensor and False otherwise
bool IsSensor();
- Polar position;
+ Spherical position;
+ Quaternion orientation;
+
+ void SetModel(const char *url, Vector3 position = Vector3(0, 0, 0),
+ Quaternion orientation = Quaternion::identity, float scale = 1);
+ const char *modelUrl = nullptr;
+ Vector3 modelPosition = Vector3::zero;
+ Quaternion modelOrientation = Quaternion::identity;
+ float modelScale = 1;
void SetParent(Thing *parent);
Thing *GetParent();