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

things with spherical position and an orientation

Browse Source
This commit is contained in:
Pascal Serrarens 2024-05-31 09:53:21 +02:00
parent 186ccd92e5
commit bc136362f2
11 changed files with 256 additions and 222 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
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.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)

4
Motor.cpp
View File

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

414
NetworkSync.cpp
View File

@ -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 =
@ -433,3 +333,111 @@ void NetworkSync::SendText(const char *s) {
SendBuffer(ix);
}
// 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) {}

5
NetworkSync.h
View File

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

5
Perception.cpp
View File

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

6
Roboid.cpp
View File

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

5
Sensor.cpp
View File

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

3
ServoMotor.cpp
View File

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

1
ServoMotor.h
View File

@ -9,6 +9,7 @@ class ServoMotor : public Thing {
public:
ServoMotor();
Vector3 rotationAxis = Vector3::up;
float minAngle = -90.0F;
float maxAngle = 90.0F;

10
Thing.cpp
View File

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

15
Thing.h
View File

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