Improve use of position/orientation
This commit is contained in:
Pascal Serrarens
parent
6e996c64eb
commit
0e2f628e3e
@ -6,9 +6,9 @@
|
||||
|
||||
DifferentialDrive::DifferentialDrive() {};
|
||||
|
||||
DifferentialDrive::DifferentialDrive(Motor* leftMotor, Motor* rightMotor) {
|
||||
DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor) {
|
||||
this->motorCount = 2;
|
||||
this->motors = new Motor*[2];
|
||||
this->motors = new Motor *[2];
|
||||
this->motors[0] = leftMotor;
|
||||
this->motors[1] = rightMotor;
|
||||
|
||||
@ -35,7 +35,7 @@ void DifferentialDrive::SetDimensions(float wheelDiameter,
|
||||
void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
|
||||
float rightSpeed) {
|
||||
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
||||
Motor* motor = motors[motorIx];
|
||||
Motor *motor = motors[motorIx];
|
||||
if (motor == nullptr)
|
||||
continue;
|
||||
|
||||
@ -50,12 +50,12 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
|
||||
|
||||
void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
|
||||
float leftSpeed =
|
||||
Float::Clamp(forward + yaw, -1, 1); // revolutions per second
|
||||
Float::Clamp(forward + yaw, -1, 1); // revolutions per second
|
||||
float rightSpeed =
|
||||
Float::Clamp(forward - yaw, -1, 1); // revolutions per second
|
||||
Float::Clamp(forward - yaw, -1, 1); // revolutions per second
|
||||
|
||||
float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
|
||||
float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
|
||||
float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second
|
||||
float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
|
||||
|
||||
SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
|
||||
}
|
||||
@ -64,9 +64,7 @@ void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
|
||||
SetTwistSpeed(linear.y, yaw);
|
||||
}
|
||||
|
||||
void DifferentialDrive::SetTwistSpeed(Vector3 linear,
|
||||
float yaw,
|
||||
float pitch,
|
||||
void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
|
||||
float roll) {
|
||||
SetTwistSpeed(linear.Forward(), yaw);
|
||||
}
|
||||
@ -76,30 +74,30 @@ void DifferentialDrive::SetVelocity(Polar velocity) {
|
||||
}
|
||||
|
||||
Polar DifferentialDrive::GetVelocity() {
|
||||
Motor* leftMotor = motors[0];
|
||||
Motor* rightMotor = motors[1];
|
||||
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
|
||||
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
|
||||
Motor *leftMotor = motors[0];
|
||||
Motor *rightMotor = motors[1];
|
||||
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
|
||||
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
|
||||
|
||||
leftSpeed = leftSpeed * rpsToMs; // in meters per second
|
||||
rightSpeed = rightSpeed * rpsToMs; // in meters per second
|
||||
leftSpeed = leftSpeed * rpsToMs; // in meters per second
|
||||
rightSpeed = rightSpeed * rpsToMs; // in meters per second
|
||||
float speed = (leftSpeed + rightSpeed) / 2;
|
||||
|
||||
float direction = speed >= 0 ? 0.0F : 180.0F;
|
||||
float magnitude = fabsf(speed);
|
||||
Polar velocity = Polar(
|
||||
magnitude, Angle::Degrees(direction)); // Polar(direction, magnitude);
|
||||
magnitude, Angle::Degrees(direction)); // Polar(direction, magnitude);
|
||||
return velocity;
|
||||
}
|
||||
|
||||
float DifferentialDrive::GetAngularVelocity() {
|
||||
Motor* leftMotor = motors[0];
|
||||
Motor* rightMotor = motors[1];
|
||||
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
|
||||
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
|
||||
Motor *leftMotor = motors[0];
|
||||
Motor *rightMotor = motors[1];
|
||||
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
|
||||
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
|
||||
|
||||
leftSpeed = leftSpeed * rpsToMs; // in meters per second
|
||||
rightSpeed = rightSpeed * rpsToMs; // in meters per second
|
||||
leftSpeed = leftSpeed * rpsToMs; // in meters per second
|
||||
rightSpeed = rightSpeed * rpsToMs; // in meters per second
|
||||
|
||||
float angularSpeed = (leftSpeed - rightSpeed) / 2;
|
||||
float angularDistance = wheelSeparation / 2 * Passer::LinearAlgebra::pi;
|
||||
|
||||
128
NetworkSync.cpp
128
NetworkSync.cpp
@ -5,7 +5,7 @@
|
||||
#ifdef RC_DEBUG
|
||||
#include <Arduino.h>
|
||||
#if ESP32
|
||||
#define SERIALPORT Serial0
|
||||
#define SERIALPORT Serial
|
||||
#else
|
||||
#define SERIALPORT Serial
|
||||
#endif
|
||||
@ -18,18 +18,18 @@
|
||||
|
||||
#include <string.h>
|
||||
|
||||
NetworkSync::NetworkSync(Roboid* roboid) {
|
||||
NetworkSync::NetworkSync(Roboid *roboid) {
|
||||
this->roboid = roboid;
|
||||
this->networkId = 0;
|
||||
}
|
||||
|
||||
void NetworkSync::ReceiveMessage(Roboid* roboid, unsigned char bytecount) {
|
||||
void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
|
||||
networkPerception->ProcessPacket(roboid, buffer, bytecount);
|
||||
|
||||
switch (buffer[0]) {
|
||||
case NetworkIdMsg:
|
||||
ReceiveNetworkId();
|
||||
break;
|
||||
case NetworkIdMsg:
|
||||
ReceiveNetworkId();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@ -42,7 +42,7 @@ void NetworkSync::ReceiveNetworkId() {
|
||||
SendName(roboid);
|
||||
SendModel(roboid);
|
||||
for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) {
|
||||
Thing* child = roboid->GetChild(childIx);
|
||||
Thing *child = roboid->GetChild(childIx);
|
||||
if (child != nullptr)
|
||||
PublishRelativeThing(child, true);
|
||||
}
|
||||
@ -51,7 +51,7 @@ void NetworkSync::ReceiveNetworkId() {
|
||||
void NetworkSync::PublishDevice() {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = DeviceMsg;
|
||||
buffer[ix++] = 0; // No network ID
|
||||
buffer[ix++] = 0; // No network ID
|
||||
PublishBuffer(ix);
|
||||
|
||||
#ifdef RC_DEBUG
|
||||
@ -59,12 +59,12 @@ void NetworkSync::PublishDevice() {
|
||||
#endif
|
||||
}
|
||||
|
||||
void NetworkSync::PublishState(Roboid* roboid) {
|
||||
void NetworkSync::PublishState(Roboid *roboid) {
|
||||
SendPose(roboid);
|
||||
PublishPerception(roboid);
|
||||
}
|
||||
|
||||
void NetworkSync::NewObject(InterestingThing* thing) {
|
||||
void NetworkSync::NewObject(InterestingThing *thing) {
|
||||
if (thing == nullptr || thing->networkId != 0x00)
|
||||
return;
|
||||
|
||||
@ -82,8 +82,8 @@ void NetworkSync::NewObject(InterestingThing* thing) {
|
||||
PublishTrackedObject(roboid, thing);
|
||||
}
|
||||
|
||||
void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) {
|
||||
Thing* parentThing = thing->GetParent();
|
||||
void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) {
|
||||
Thing *parentThing = thing->GetParent();
|
||||
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = RelativePoseMsg;
|
||||
@ -99,14 +99,14 @@ void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) {
|
||||
|
||||
if (recurse) {
|
||||
for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
|
||||
Thing* child = thing->GetChild(childIx);
|
||||
Thing *child = thing->GetChild(childIx);
|
||||
if (child != nullptr)
|
||||
PublishRelativeThing(child, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NetworkSync::SendName(Roboid* roboid) {
|
||||
void NetworkSync::SendName(Roboid *roboid) {
|
||||
if (roboid->name == nullptr)
|
||||
return;
|
||||
|
||||
@ -116,7 +116,7 @@ void NetworkSync::SendName(Roboid* roboid) {
|
||||
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = NameMsg;
|
||||
buffer[ix++] = 0x00; // objectId
|
||||
buffer[ix++] = 0x00; // objectId
|
||||
|
||||
buffer[ix++] = len;
|
||||
for (unsigned char nameIx = 0; nameIx < len; nameIx++)
|
||||
@ -130,7 +130,7 @@ void NetworkSync::SendName(Roboid* roboid) {
|
||||
#endif
|
||||
}
|
||||
|
||||
void NetworkSync::SendModel(Roboid* roboid) {
|
||||
void NetworkSync::SendModel(Roboid *roboid) {
|
||||
if (roboid->modelUrl == nullptr)
|
||||
return;
|
||||
|
||||
@ -140,10 +140,10 @@ void NetworkSync::SendModel(Roboid* roboid) {
|
||||
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = ModelMsg;
|
||||
buffer[ix++] = 0x00; // objectId
|
||||
Spherical16 s = Spherical16::zero; //(roboid->modelPosition);
|
||||
buffer[ix++] = 0x00; // objectId
|
||||
Spherical16 s = Spherical16::zero; //(roboid->modelPosition);
|
||||
SendSpherical16(buffer, &ix, s);
|
||||
SendFloat16(buffer, &ix, 1); // roboid->modelScale);
|
||||
SendFloat16(buffer, &ix, 1); // roboid->modelScale);
|
||||
|
||||
buffer[ix++] = len;
|
||||
for (int urlIx = 0; urlIx < len; urlIx++)
|
||||
@ -152,7 +152,7 @@ void NetworkSync::SendModel(Roboid* roboid) {
|
||||
SendBuffer(ix);
|
||||
}
|
||||
|
||||
void NetworkSync::SendModel(Thing* thing) {
|
||||
void NetworkSync::SendModel(Thing *thing) {
|
||||
if (thing->modelUrl == nullptr)
|
||||
return;
|
||||
|
||||
@ -162,10 +162,10 @@ void NetworkSync::SendModel(Thing* thing) {
|
||||
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = ModelMsg;
|
||||
buffer[ix++] = thing->id; // objectId
|
||||
Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
|
||||
buffer[ix++] = thing->id; // objectId
|
||||
Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
|
||||
SendSpherical16(buffer, &ix, s);
|
||||
SendFloat16(buffer, &ix, 1); // thing->modelScale);
|
||||
SendFloat16(buffer, &ix, 1); // thing->modelScale);
|
||||
|
||||
buffer[ix++] = len;
|
||||
for (int urlIx = 0; urlIx < len; urlIx++)
|
||||
@ -174,8 +174,8 @@ void NetworkSync::SendModel(Thing* thing) {
|
||||
SendBuffer(ix);
|
||||
}
|
||||
|
||||
void NetworkSync::SendDestroyThing(InterestingThing* thing) {
|
||||
if (networkId == 0) // We're not connected to a site yet
|
||||
void NetworkSync::SendDestroyThing(InterestingThing *thing) {
|
||||
if (networkId == 0) // We're not connected to a site yet
|
||||
return;
|
||||
|
||||
unsigned char ix = 0;
|
||||
@ -188,8 +188,8 @@ void NetworkSync::SendDestroyThing(InterestingThing* thing) {
|
||||
#endif
|
||||
}
|
||||
|
||||
void NetworkSync::SendPose(Thing* thing, bool recurse) {
|
||||
if (networkId == 0) // We're not connected to a site yet
|
||||
void NetworkSync::SendPose(Thing *thing, bool recurse) {
|
||||
if (this->networkId == 0) // We're not connected to a site yet
|
||||
return;
|
||||
|
||||
if (thing->GetLinearVelocity().distance > 0 ||
|
||||
@ -204,21 +204,22 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) {
|
||||
|
||||
#if RC_DEBUG
|
||||
if (thing->id == 0)
|
||||
SERIALPORT.printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]);
|
||||
SERIALPORT.printf("Sent PoseMsg Thing [%d/%d]\n", this->networkId,
|
||||
buffer[1]);
|
||||
#endif
|
||||
}
|
||||
|
||||
if (recurse) {
|
||||
for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
|
||||
Thing* child = thing->GetChild(childIx);
|
||||
Thing *child = thing->GetChild(childIx);
|
||||
if (child != nullptr)
|
||||
SendPose(thing->GetChild(childIx), true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NetworkSync::PublishState(Sensor* sensor) {
|
||||
float* value = (float*)sensor->GetValue();
|
||||
void NetworkSync::PublishState(Sensor *sensor) {
|
||||
float *value = (float *)sensor->GetValue();
|
||||
if (value == nullptr)
|
||||
return;
|
||||
|
||||
@ -229,14 +230,14 @@ void NetworkSync::PublishState(Sensor* sensor) {
|
||||
PublishBuffer(ix);
|
||||
}
|
||||
|
||||
void NetworkSync::PublishPerception(Roboid* roboid) {
|
||||
Perception* perception = roboid->perception;
|
||||
void NetworkSync::PublishPerception(Roboid *roboid) {
|
||||
Perception *perception = roboid->perception;
|
||||
if (perception == nullptr)
|
||||
return;
|
||||
|
||||
for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount;
|
||||
sensorIx++) {
|
||||
Sensor* sensor = perception->sensors[sensorIx];
|
||||
Sensor *sensor = perception->sensors[sensorIx];
|
||||
if (sensor == nullptr)
|
||||
continue;
|
||||
PublishState(sensor);
|
||||
@ -244,14 +245,14 @@ void NetworkSync::PublishPerception(Roboid* roboid) {
|
||||
PublishTrackedObjects(roboid, roboid->perception->GetTrackedObjects());
|
||||
}
|
||||
|
||||
void NetworkSync::PublishTrackedObjects(Roboid* roboid,
|
||||
InterestingThing** objects) {
|
||||
if (networkId == 0) // We're not connected to a site yet
|
||||
void NetworkSync::PublishTrackedObjects(Roboid *roboid,
|
||||
InterestingThing **objects) {
|
||||
if (networkId == 0) // We're not connected to a site yet
|
||||
return;
|
||||
|
||||
int n = 0;
|
||||
for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
|
||||
InterestingThing* obj = objects[objIx];
|
||||
InterestingThing *obj = objects[objIx];
|
||||
if (obj == nullptr)
|
||||
continue;
|
||||
|
||||
@ -262,8 +263,8 @@ void NetworkSync::PublishTrackedObjects(Roboid* roboid,
|
||||
}
|
||||
}
|
||||
|
||||
void NetworkSync::PublishTrackedObject(Roboid* roboid,
|
||||
InterestingThing* object) {
|
||||
void NetworkSync::PublishTrackedObject(Roboid *roboid,
|
||||
InterestingThing *object) {
|
||||
if (object == nullptr || object->updated == false ||
|
||||
object->networkId != 0x00) {
|
||||
return;
|
||||
@ -277,8 +278,8 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid,
|
||||
SwingTwist16 worldOrientation = roboidOrientation * object->orientation;
|
||||
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = PoseMsg; // Position2DMsg;
|
||||
buffer[ix++] = object->id; // objectId;
|
||||
buffer[ix++] = PoseMsg; // Position2DMsg;
|
||||
buffer[ix++] = object->id; // objectId;
|
||||
buffer[ix++] = Pose_Position | Pose_Orientation;
|
||||
SendSpherical16(buffer, &ix, worldPosition);
|
||||
SendSwingTwist(buffer, &ix, worldOrientation);
|
||||
@ -291,7 +292,8 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid,
|
||||
object->updated = false;
|
||||
}
|
||||
|
||||
void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid* roboid) {
|
||||
void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
|
||||
// SERIALPORT.printf("old send pose?\n");
|
||||
Polar velocity = roboid->propulsion->GetVelocity();
|
||||
Vector2 worldVelocity2 =
|
||||
Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
|
||||
@ -317,7 +319,7 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid* roboid) {
|
||||
const unsigned int bufferSize = 3 + 12 + 12;
|
||||
unsigned char buffer[bufferSize] = {
|
||||
PoseMsg,
|
||||
0, // objectId;
|
||||
0, // objectId;
|
||||
Pose_LinearVelocity | Pose_AngularVelocity,
|
||||
};
|
||||
unsigned char ix = 3;
|
||||
@ -339,7 +341,7 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid* roboid) {
|
||||
// #endif
|
||||
// }
|
||||
|
||||
void NetworkSync::SendInvestigate(InterestingThing* thing) {
|
||||
void NetworkSync::SendInvestigate(InterestingThing *thing) {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = InvestigateMsg;
|
||||
buffer[ix++] = thing->networkId;
|
||||
@ -350,7 +352,7 @@ void NetworkSync::SendInvestigate(InterestingThing* thing) {
|
||||
#endif
|
||||
}
|
||||
|
||||
void NetworkSync::SendText(const char* s) {
|
||||
void NetworkSync::SendText(const char *s) {
|
||||
unsigned char length;
|
||||
for (length = 0; length < 253; length++) {
|
||||
if (s[length] == '\0')
|
||||
@ -389,8 +391,7 @@ void NetworkSync::SendInt(const int x) {
|
||||
|
||||
// Low-level functions
|
||||
|
||||
void NetworkSync::SendVector3(unsigned char* data,
|
||||
unsigned char* startIndex,
|
||||
void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
|
||||
const Vector3 v) {
|
||||
SendSingle100(data, *startIndex, v.Right());
|
||||
(*startIndex) += 4;
|
||||
@ -400,8 +401,7 @@ void NetworkSync::SendVector3(unsigned char* data,
|
||||
(*startIndex) += 4;
|
||||
}
|
||||
|
||||
void NetworkSync::SendQuaternion(unsigned char* data,
|
||||
const int startIndex,
|
||||
void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
|
||||
const Quaternion q) {
|
||||
Vector3 angles = Quaternion::ToAngles(q);
|
||||
int ix = startIndex;
|
||||
@ -410,30 +410,26 @@ void NetworkSync::SendQuaternion(unsigned char* data,
|
||||
SendAngle8(data, ix++, angles.Forward());
|
||||
}
|
||||
|
||||
void NetworkSync::SendPolar(unsigned char* data,
|
||||
unsigned char* startIndex,
|
||||
void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex,
|
||||
Polar p) {
|
||||
SendAngle8(data, *startIndex, (const float)p.angle.InDegrees());
|
||||
SendSingle100(data, (*startIndex) + 1, p.distance);
|
||||
}
|
||||
|
||||
void NetworkSync::SendSpherical16(unsigned char* data,
|
||||
unsigned char* startIndex,
|
||||
Spherical16 s) {
|
||||
void NetworkSync::SendSpherical16(unsigned char *data,
|
||||
unsigned char *startIndex, Spherical16 s) {
|
||||
SendAngle8(data, (*startIndex)++, s.direction.horizontal.InDegrees());
|
||||
SendAngle8(data, (*startIndex)++, s.direction.vertical.InDegrees());
|
||||
SendFloat16(data, startIndex, s.distance);
|
||||
}
|
||||
|
||||
void NetworkSync::SendSwingTwist(unsigned char* data,
|
||||
unsigned char* ix,
|
||||
void NetworkSync::SendSwingTwist(unsigned char *data, unsigned char *ix,
|
||||
const SwingTwist16 r) {
|
||||
Quaternion q = r.ToQuaternion();
|
||||
SendQuat32(buffer, ix, q);
|
||||
}
|
||||
|
||||
void NetworkSync::SendQuat32(unsigned char* data,
|
||||
unsigned char* startIndex,
|
||||
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);
|
||||
@ -452,8 +448,7 @@ void NetworkSync::SendQuat32(unsigned char* data,
|
||||
data[(*startIndex)++] = qw;
|
||||
}
|
||||
|
||||
void NetworkSync::SendAngle8(unsigned char* data,
|
||||
unsigned int startIndex,
|
||||
void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
|
||||
const float angle) {
|
||||
Angle8 packedAngle2 = Angle8::Degrees(angle);
|
||||
data[startIndex] = packedAngle2.GetBinary();
|
||||
@ -480,16 +475,14 @@ void NetworkSync::SendAngle8(unsigned char* data,
|
||||
// // data[startIndex + 3]);
|
||||
// }
|
||||
|
||||
void NetworkSync::SendSingle100(unsigned char* data,
|
||||
unsigned int startIndex,
|
||||
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);
|
||||
}
|
||||
|
||||
void NetworkSync::SendFloat16(unsigned char* data,
|
||||
unsigned char* startIndex,
|
||||
void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex,
|
||||
float value) {
|
||||
float16 value16 = float16(value);
|
||||
short binary = value16.getBinary();
|
||||
@ -498,11 +491,10 @@ void NetworkSync::SendFloat16(unsigned char* data,
|
||||
data[(*startIndex)++] = binary & 0xFF;
|
||||
}
|
||||
|
||||
void NetworkSync::SendInt32(unsigned char* data,
|
||||
unsigned int startIndex,
|
||||
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];
|
||||
data[startIndex++] = ((unsigned char *)&value)[ix];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
104
Roboid.cpp
104
Roboid.cpp
@ -20,23 +20,20 @@ Roboid::Roboid() : Thing(0) {
|
||||
this->perception->roboid = this;
|
||||
this->propulsion = nullptr;
|
||||
this->networkSync = nullptr;
|
||||
// this->actuation = nullptr;
|
||||
|
||||
this->worldPosition = Spherical16::zero;
|
||||
this->worldOrientation = SwingTwist16::identity;
|
||||
// this->worldOrientation = Quaternion::identity;
|
||||
// this->worldAngleAxis = AngleAxisOf<float>();
|
||||
}
|
||||
|
||||
Roboid::Roboid(Propulsion* propulsion) : Roboid() {
|
||||
Roboid::Roboid(Propulsion *propulsion) : Roboid() {
|
||||
this->propulsion = propulsion;
|
||||
if (propulsion != nullptr)
|
||||
propulsion->roboid = this;
|
||||
}
|
||||
|
||||
void Passer::RoboidControl::Roboid::SetName(char* name) {
|
||||
this->name = name;
|
||||
}
|
||||
void Passer::RoboidControl::Roboid::SetName(char *name) { this->name = name; }
|
||||
|
||||
#include <Arduino.h>
|
||||
void Roboid::Update(unsigned long currentTimeMs) {
|
||||
if (perception != nullptr)
|
||||
perception->Update(currentTimeMs);
|
||||
@ -46,13 +43,28 @@ void Roboid::Update(unsigned long currentTimeMs) {
|
||||
|
||||
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
|
||||
|
||||
SetPosition(this->worldPosition +
|
||||
this->worldOrientation * Spherical16::forward *
|
||||
this->propulsion->GetVelocity().distance * deltaTime);
|
||||
SetOrientation(this->worldOrientation *
|
||||
SwingTwist16::AngleAxis(
|
||||
this->propulsion->GetAngularVelocity() * deltaTime,
|
||||
Direction16::up));
|
||||
// Conversion from old units
|
||||
Polar polarVelocity = this->propulsion->GetVelocity();
|
||||
this->linearVelocity = Spherical16(
|
||||
polarVelocity.distance,
|
||||
Angle16::Degrees(polarVelocity.angle.InDegrees()), Angle16());
|
||||
float oldAngular = this->propulsion->GetAngularVelocity();
|
||||
this->angularVelocity =
|
||||
Spherical16(oldAngular, Angle16(), Angle16::Degrees(90));
|
||||
|
||||
SetPosition(this->position + this->orientation * Spherical16::forward *
|
||||
this->linearVelocity.distance * deltaTime);
|
||||
this->worldPosition = this->position; // assuming the roboid is the root
|
||||
|
||||
SwingTwist16 rotation = SwingTwist16::AngleAxis(
|
||||
this->angularVelocity.distance * deltaTime, Direction16::up);
|
||||
if (perception != nullptr)
|
||||
perception->UpdatePose(rotation);
|
||||
|
||||
this->orientation = this->orientation * rotation;
|
||||
|
||||
this->worldOrientation =
|
||||
this->orientation; // assuming the roboid is the root
|
||||
}
|
||||
|
||||
if (childCount > 0 && children != nullptr) {
|
||||
@ -66,32 +78,9 @@ void Roboid::Update(unsigned long currentTimeMs) {
|
||||
lastUpdateTimeMs = currentTimeMs;
|
||||
}
|
||||
|
||||
Spherical16 Roboid::GetPosition() {
|
||||
return this->worldPosition;
|
||||
}
|
||||
// Vector2 Roboid::GetPosition2D() {
|
||||
// return Vector2(this->worldPosition.Right(), this->worldPosition.Forward());
|
||||
// }
|
||||
Spherical16 Roboid::GetPosition() { return this->worldPosition; }
|
||||
|
||||
SwingTwist16 Roboid::GetOrientation() {
|
||||
// Vector3 axis = this->worldAngleAxis.axis.ToVector3();
|
||||
// SwingTwist16 q = SwingTwist16::AngleAxis(this->worldAngleAxis.angle, axis);
|
||||
return this->worldOrientation;
|
||||
}
|
||||
|
||||
// float Roboid::GetOrientation2D() {
|
||||
// float maxAngle = 90 - Float::epsilon; // note: range vertical angle =
|
||||
// -90..90
|
||||
|
||||
// // rotation axis is vertical, so we have a simple 2D orientation
|
||||
// if (this->worldAngleAxis.axis.vertical.InDegrees() > maxAngle)
|
||||
// return this->worldAngleAxis.angle;
|
||||
// if (this->worldAngleAxis.axis.vertical.InDegrees() < -maxAngle)
|
||||
// return -this->worldAngleAxis.angle;
|
||||
|
||||
// SwingTwist16 q = GetOrientation();
|
||||
// return Quaternion::GetAngleAround(Vector3::up, q);
|
||||
// }
|
||||
SwingTwist16 Roboid::GetOrientation() { return this->worldOrientation; }
|
||||
|
||||
void Roboid::SetPosition(Spherical16 newWorldPosition) {
|
||||
SwingTwist16 roboidOrientation = this->GetOrientation();
|
||||
@ -100,44 +89,33 @@ void Roboid::SetPosition(Spherical16 newWorldPosition) {
|
||||
Angle16 angle = Spherical16::SignedAngleBetween(
|
||||
roboidOrientation * Spherical16::forward, translation, Spherical16::up);
|
||||
Polar16 polarTranslation = Polar16(
|
||||
distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance));
|
||||
distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance));
|
||||
if (perception != nullptr)
|
||||
perception->UpdatePose(polarTranslation);
|
||||
this->worldPosition = newWorldPosition;
|
||||
|
||||
if (networkSync != nullptr)
|
||||
// networkSync->SendPosition(this->worldPosition);
|
||||
networkSync->SendPose(this->worldPosition, roboidOrientation);
|
||||
this->position = newWorldPosition; // roboid is the root?
|
||||
// World position should be set in the update recursion
|
||||
// this->worldPosition = newWorldPosition;
|
||||
|
||||
// if (networkSync != nullptr)
|
||||
// // networkSync->SendPosition(this->worldPosition);
|
||||
// networkSync->SendPose(this->worldPosition, roboidOrientation);
|
||||
}
|
||||
|
||||
#include <math.h>
|
||||
void Roboid::SetOrientation(SwingTwist16 worldOrientation) {
|
||||
// float angle;
|
||||
// Vector3 axis;
|
||||
// worldOrientation.ToAngleAxis(&angle, &axis);
|
||||
|
||||
void Roboid::SetOrientation(SwingTwist16 newOrientation) {
|
||||
SwingTwist16 delta =
|
||||
SwingTwist16::Inverse(GetOrientation()) * worldOrientation;
|
||||
SwingTwist16::Inverse(this->orientation) * newOrientation;
|
||||
if (perception != nullptr)
|
||||
perception->UpdatePose(delta);
|
||||
|
||||
// AngleAxisOf<float> angleAxis =
|
||||
// AngleAxisOf<float>(angle, DirectionOf<float>(axis));
|
||||
// this->worldAngleAxis = angleAxis;
|
||||
this->orientation = newOrientation;
|
||||
}
|
||||
|
||||
// void Roboid::SetOrientation2D(float angle) {
|
||||
// this->worldAngleAxis = AngleAxisOf<float>(angle, DirectionOf<float>::up);
|
||||
// }
|
||||
|
||||
Vector3 Passer::RoboidControl::Roboid::GetVelocity() {
|
||||
return Vector3();
|
||||
}
|
||||
|
||||
void Roboid::AddChild(Thing* child) {
|
||||
void Roboid::AddChild(Thing *child) {
|
||||
Thing::AddChild(child);
|
||||
if (child->IsSensor()) {
|
||||
Sensor* childSensor = (Sensor*)child;
|
||||
Sensor *childSensor = (Sensor *)child;
|
||||
this->perception->AddSensor(childSensor);
|
||||
}
|
||||
}
|
||||
|
||||
28
Roboid.h
28
Roboid.h
@ -12,16 +12,16 @@ class NetworkSync;
|
||||
|
||||
/// @brief A Roboid is used to control autonomous robots
|
||||
class Roboid : public Thing {
|
||||
public:
|
||||
public:
|
||||
/// @brief Default constructor for a Roboid
|
||||
Roboid();
|
||||
/// @brief Creates a Roboid with Perception and Propulsion abilities
|
||||
/// @param perception The Perception implementation to use for this Roboid
|
||||
/// @param propulsion The Propulsion implementation to use for this Roboid
|
||||
Roboid(Propulsion* propulsion);
|
||||
Roboid(Propulsion *propulsion);
|
||||
|
||||
char* name = nullptr;
|
||||
void SetName(char* name);
|
||||
char *name = nullptr;
|
||||
void SetName(char *name);
|
||||
|
||||
/// @brief Update the state of the Roboid
|
||||
/// @param currentTimeMs The time in milliseconds when calling this
|
||||
@ -29,13 +29,13 @@ class Roboid : public Thing {
|
||||
void Update(unsigned long currentTimeMs);
|
||||
|
||||
/// @brief The Perception module of this Roboid
|
||||
Perception* perception = nullptr;
|
||||
Perception *perception = nullptr;
|
||||
/// @brief The Propulsion module of this Roboid
|
||||
Propulsion* propulsion = nullptr;
|
||||
Propulsion *propulsion = nullptr;
|
||||
// ServoMotor *actuation = nullptr;
|
||||
|
||||
/// @brief The reference to the module to synchronize states across a network
|
||||
NetworkSync* networkSync = nullptr;
|
||||
NetworkSync *networkSync = nullptr;
|
||||
|
||||
/// @brief Retrieve the current position of the roboid
|
||||
/// @return The position in carthesian coordinates in world space
|
||||
@ -50,7 +50,6 @@ class Roboid : public Thing {
|
||||
/// used. This value will be Quaternion::identity unless an orientation is
|
||||
/// received though network synchronization
|
||||
virtual SwingTwist16 GetOrientation();
|
||||
// float GetOrientation2D();
|
||||
|
||||
/// @brief Update the current position of the roboid
|
||||
/// @param worldPosition The position of the roboid in carthesian coordinates
|
||||
@ -66,18 +65,15 @@ class Roboid : public Thing {
|
||||
/// perceived objects by the roboid (roboid->perception->perceivedObjets),
|
||||
/// as these are local to the roboid' orientation.
|
||||
virtual void SetOrientation(SwingTwist16 worldOrientation);
|
||||
// void SetOrientation2D(float angle);
|
||||
|
||||
virtual Vector3 GetVelocity();
|
||||
virtual void AddChild(Thing *child) override;
|
||||
|
||||
virtual void AddChild(Thing* child) override;
|
||||
|
||||
private:
|
||||
private:
|
||||
/// @brief The position of the roboid in carthesian coordinates in world space
|
||||
/// @details This position may be set when NetworkSync is used to receive
|
||||
/// positions from an external tracking system. These values should not be set
|
||||
/// directly, but SetPosition should be used instead.
|
||||
Spherical16 worldPosition = Spherical16::zero;
|
||||
// Spherical16 worldPosition = Spherical16::zero;
|
||||
/// @brief The orientation of the roboid in world space
|
||||
/// @details The position may be set when NetworkSync is used to receive
|
||||
/// orientations from an external tracking system. This value should not be
|
||||
@ -89,7 +85,7 @@ class Roboid : public Thing {
|
||||
unsigned long lastUpdateTimeMs = 0;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
|
||||
using namespace Passer::RoboidControl;
|
||||
27
Thing.h
27
Thing.h
@ -10,7 +10,7 @@ namespace RoboidControl {
|
||||
|
||||
/// @brief A thing is a functional component on a robot
|
||||
class Thing {
|
||||
public:
|
||||
public:
|
||||
/// @brief Default constructor for a Thing
|
||||
Thing(unsigned char id);
|
||||
|
||||
@ -47,6 +47,7 @@ class Thing {
|
||||
/// @remark When this Thing has a parent, the position is relative to the
|
||||
/// parent's position and orientation
|
||||
Spherical16 position;
|
||||
Spherical16 worldPosition;
|
||||
/// @brief The orientation of this Thing
|
||||
/// @remark When this Thing has a parent, the orientation is relative to the
|
||||
/// parent's orientation
|
||||
@ -59,27 +60,27 @@ class Thing {
|
||||
/// @brief Sets the parent Thing
|
||||
/// @param parent The Thing which should become the parnet
|
||||
/// @remark This is equivalent to calling parent->AddChild(this);
|
||||
virtual void SetParent(Thing* parent);
|
||||
virtual void SetParent(Thing *parent);
|
||||
/// @brief Gets the parent Thing
|
||||
/// @return The parent Thing
|
||||
Thing* GetParent();
|
||||
Thing *GetParent();
|
||||
|
||||
/// @brief Add a child Thing to this Thing
|
||||
/// @param child The Thing which should become a child
|
||||
/// @remark When the Thing is already a child, it will not be added again
|
||||
virtual void AddChild(Thing* child);
|
||||
virtual void AddChild(Thing *child);
|
||||
/// @brief Get the child at the given index
|
||||
/// @param childIx The index of the child
|
||||
/// @return The child at the given index or nullptr when the index is invalid
|
||||
/// or the child could not be found
|
||||
Thing* GetChild(unsigned char childIx);
|
||||
Thing *GetChild(unsigned char childIx);
|
||||
|
||||
/// @brief Sets the location from where the 3D model of this Thing can be
|
||||
/// loaded from
|
||||
/// @param url The url of the model
|
||||
/// @remark Although the roboid implementation is not dependent on the model,
|
||||
/// the only official supported model format is .obj
|
||||
void SetModel(const char* url);
|
||||
void SetModel(const char *url);
|
||||
|
||||
/// @brief Updates the state of the thing
|
||||
/// @param currentTimeMs The current clock time in milliseconds
|
||||
@ -87,9 +88,9 @@ class Thing {
|
||||
|
||||
unsigned char childCount = 0;
|
||||
|
||||
const char* modelUrl = nullptr;
|
||||
const char *modelUrl = nullptr;
|
||||
|
||||
protected:
|
||||
protected:
|
||||
/// @brief Bitmask for Motor type
|
||||
static const unsigned int MotorType = 0x8000;
|
||||
/// @brief Bitmap for Sensor type
|
||||
@ -112,14 +113,14 @@ class Thing {
|
||||
ExternalSensor,
|
||||
};
|
||||
|
||||
Thing* parent = nullptr;
|
||||
Thing** children = nullptr;
|
||||
Thing *parent = nullptr;
|
||||
Thing **children = nullptr;
|
||||
|
||||
public:
|
||||
public:
|
||||
Spherical16 angularVelocity = Spherical16::zero;
|
||||
Spherical16 linearVelocity = Spherical16::zero;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
using namespace Passer::RoboidControl;
|
||||
Loading…
x
Reference in New Issue
Block a user