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

AR mode works, but reaction time is slow

Browse Source
This commit is contained in:
Pascal Serrarens 2025-05-21 15:59:11 +02:00
parent 1b5fef15e6
commit 140138977a
29 changed files with 313 additions and 182 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Arduino/ArduinoParticipant.cpp
View File

@ -82,19 +82,6 @@ void ParticipantUDP::Receive() {
senderAddress.toCharArray(sender_ipAddress, 16);
unsigned int sender_port = udp->remotePort();
// Participant* remoteParticipant = this->Get(sender_ipAddress,
// sender_port); if (remoteParticipant == nullptr) {
// remoteParticipant = this->Add(sender_ipAddress,
// sender_port);
// // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
// // << "\n";
// // std::cout << "New remote participant " <<
// remoteParticipant->ipAddress
// // << ":" << remoteParticipant->port << " "
// // << (int)remoteParticipant->networkId << "\n";
// }
// ReceiveData(packetSize, remoteParticipant);
ReceiveData(packetSize, sender_ipAddress, sender_port);
packetSize = udp->parsePacket();
}
@ -107,6 +94,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
// << remoteParticipant->port << "\n";
int n = 0;
int r = 0;
do {
if (n > 0) {
#if !defined(NO_STD)
@ -115,9 +103,13 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
delay(10);
}
n++;
udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
udp->write((unsigned char*)buffer, bufferSize);
} while (udp->endPacket() == 0 && n < 10);
r = udp->endPacket();
// On an Uno R4 WiFi, endPacket blocks for 10 seconds the first time
// It is not cleary yet why
} while (r == 0 && n < 10);
#endif
return true;

18
Arduino/Things/DRV8833.cpp
View File

@ -7,14 +7,14 @@ namespace Arduino {
#pragma region DRV8833
DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(parent) {
DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) {
this->pinStandby = config.standby;
if (pinStandby != 255)
pinMode(pinStandby, OUTPUT);
this->motorA = new DRV8833Motor(*this, config.AIn1, config.AIn2);
this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
this->motorA->SetName("Motor A");
this->motorB = new DRV8833Motor(*this, config.BIn1, config.BIn2);
this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2);
this->motorB->SetName("Motor B");
}
@ -23,11 +23,11 @@ DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(parent) {
#pragma region Differential drive
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
Thing& parent)
: RoboidControl::DifferentialDrive(parent), drv8833(config) {
this->leftWheel = this->drv8833.motorA;
this->rightWheel = this->drv8833.motorB;
}
Thing* parent)
: RoboidControl::DifferentialDrive(this->drv8833.motorA,
this->drv8833.motorB,
parent),
drv8833(config, this) {}
void DRV8833::DifferentialDrive::Update(bool recurse) {
RoboidControl::DifferentialDrive::Update(recurse);
@ -42,7 +42,7 @@ void DRV8833::DifferentialDrive::Update(bool recurse) {
uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
#endif
DRV8833Motor::DRV8833Motor(DRV8833& driver,
DRV8833Motor::DRV8833Motor(DRV8833* driver,
unsigned char pinIn1,
unsigned char pinIn2,
bool reverse)

11
Arduino/Things/DRV8833.h
View File

@ -18,12 +18,11 @@ class DRV8833 : public Thing {
int AIn2;
int BIn1;
int BIn2;
int standby = 255;
int standby;
};
/// @brief Setup a DRV8833 motor controller
// DRV8833(Configuration config, Participant* owner = nullptr);
DRV8833(Configuration config, Thing& parent = Thing::LocalRoot());
DRV8833(Configuration config, Thing* parent = Thing::LocalRoot());
DRV8833Motor* motorA = nullptr;
DRV8833Motor* motorB = nullptr;
@ -39,9 +38,7 @@ class DRV8833 : public Thing {
class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
public:
// DifferentialDrive(DRV8833::Configuration config,
// Participant* participant = nullptr);
DifferentialDrive(DRV8833::Configuration config, Thing& parent = Thing::LocalRoot());
DifferentialDrive(DRV8833::Configuration config, Thing* parent = Thing::LocalRoot());
virtual void Update(bool recurse = false) override;
@ -60,7 +57,7 @@ class DRV8833Motor : public Motor {
/// @param pinIn1 the pin number for the in1 signal
/// @param pinIn2 the pin number for the in2 signal
/// @param direction the forward turning direction of the motor
DRV8833Motor(DRV8833& driver,
DRV8833Motor(DRV8833* driver,
unsigned char pinIn1,
unsigned char pinIn2,
bool reverse = false);

6
Arduino/Things/DigitalInput.cpp
View File

@ -7,7 +7,7 @@ namespace Arduino {
#pragma region Digital input
DigitalInput::DigitalInput(unsigned char pin, Thing& parent) : Thing(parent) {
DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(Type::Undetermined, parent) {
this->pin = pin;
pinMode(this->pin, INPUT);
std::cout << "digital input start\n";
@ -23,7 +23,7 @@ void DigitalInput::Update(bool recursive) {
#pragma region Touch sensor
DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing& parent)
DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
: RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
void DigitalInput::TouchSensor::Update(bool recursive) {
@ -39,7 +39,7 @@ volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0;
volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0;
DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
Thing& parent)
Thing* parent)
: RoboidControl::RelativeEncoder(parent),
digitalInput(config.pin, parent),
pulsesPerRevolution(config.pulsesPerRevolution) {}

8
Arduino/Things/DigitalInput.h
View File

@ -12,9 +12,9 @@ class DigitalInput : public Thing {
/// @brief Create a new digital input
/// @param participant The participant to use
/// @param pin The digital pin
DigitalInput(Participant* participant, unsigned char pin);
//DigitalInput(Participant* participant, unsigned char pin);
// DigitalInput(Thing* parent, unsigned char pin);
DigitalInput(unsigned char pin, Thing& parent);
DigitalInput(unsigned char pin, Thing* parent);
bool isHigh = false;
bool isLow = false;
@ -35,7 +35,7 @@ class DigitalInput : public Thing {
class DigitalInput::TouchSensor : public RoboidControl::TouchSensor {
public:
TouchSensor(unsigned char pin, Thing& parent);
TouchSensor(unsigned char pin, Thing* parent);
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(bool recurse = false) override;
@ -55,7 +55,7 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder {
unsigned char pulsesPerRevolution;
};
RelativeEncoder(Configuration config, Thing& parent = Thing::LocalRoot());
RelativeEncoder(Configuration config, Thing* parent = Thing::LocalRoot());
unsigned char pulsesPerRevolution;

17
Arduino/Things/UltrasonicSensor.cpp
View File

@ -6,8 +6,9 @@
namespace RoboidControl {
namespace Arduino {
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing& parent)
: Thing(parent) {
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
: Thing(Type::Undetermined, parent) {
this->name = "Ultrasonic sensor";
this->pinTrigger = config.triggerPin;
this->pinEcho = config.echoPin;
@ -57,16 +58,14 @@ void UltrasonicSensor::Update(bool recursive) {
#pragma region Touch sensor
UltrasonicSensor::TouchSensor::TouchSensor(
UltrasonicSensor::Configuration config,
Thing& parent)
: RoboidControl::TouchSensor(parent), ultrasonic(config) {}
UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
: RoboidControl::TouchSensor(parent), ultrasonic(config, this) {}
void UltrasonicSensor::TouchSensor::Update(bool recursive) {
this->ultrasonic.Update(false);
this->touchedSomething = (this->ultrasonic.distance > 0 &&
this->ultrasonic.distance <= this->touchDistance);
RoboidControl::TouchSensor::Update(recursive);
this->ultrasonic.Update(false);
this->touchedSomething |= (this->ultrasonic.distance > 0 &&
this->ultrasonic.distance <= this->touchDistance);
}
#pragma region Touch sensor

4
Arduino/Things/UltrasonicSensor.h
View File

@ -13,7 +13,7 @@ class UltrasonicSensor : Thing {
int echoPin;
};
UltrasonicSensor(Configuration config, Thing& parent = Thing::LocalRoot());
UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());
// parameters
@ -47,7 +47,7 @@ class UltrasonicSensor : Thing {
class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
public:
TouchSensor(UltrasonicSensor::Configuration config,
Thing& parent = Thing::LocalRoot());
Thing* parent = Thing::LocalRoot());
float touchDistance = 0.2f;

12
Messages/ThingMsg.cpp
View File

@ -17,19 +17,11 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) {
if (thing->IsRoot())
this->parentId = 0;
else {
Thing parent = thing->GetParent();
this->parentId = parent.id;
Thing* parent = thing->GetParent();
this->parentId = parent->id;
}
}
// ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
// unsigned char thingType, unsigned char parentId) {
// this->networkId = networkId;
// this->thingId = thingId;
// this->thingType = thingType;
// this->parentId = parentId;
// }
ThingMsg::~ThingMsg() {}
unsigned char ThingMsg::Serialize(char* buffer) {

41
Participant.cpp
View File

@ -8,14 +8,26 @@ namespace RoboidControl {
ParticipantRegistry Participant::registry;
//Participant Participant::LocalParticipant = Participant("0.0.0.0", 0);
Participant* Participant::LocalParticipant = new Participant();
Participant& Participant::LocalParticipant() {
static Participant localParticipant("0.0.0.0", 0);
return localParticipant;
}
void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
LocalParticipant = &newParticipant;
std::cout << "Replaced local participant" << std::endl;
}
Participant::Participant() {
std::cout << "P\n";
//this->root.name = "Isolated";
this->root = new Thing(this);
this->root->name = "Root";
this->Add(this->root);
}
Participant::Participant(const char* ipAddress, int port) {
// Add the root thing to the list of things, because we could not do that
// earlier (I think)
this->Add(this->root);
// make a copy of the ip address string
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
@ -33,7 +45,7 @@ Participant::Participant(const char* ipAddress, int port) {
}
Participant::~Participant() {
registry.Remove(this);
// registry.Remove(this);
delete[] this->ipAddress;
}
@ -56,9 +68,7 @@ Thing* Participant::Get(unsigned char thingId) {
}
void Participant::Add(Thing* thing, bool checkId) {
if (checkId && thing->id == 0) {
// allocate a new thing ID
#if defined(NO_STD)
thing->id = this->thingCount + 1;
@ -128,8 +138,8 @@ Participant* ParticipantRegistry::Get(const char* ipAddress,
continue;
if (strcmp(participant->ipAddress, ipAddress) == 0 &&
participant->port == port) {
std::cout << "found participant " << participant->ipAddress << ":"
<< (int)participant->port << std::endl;
// std::cout << "found participant " << participant->ipAddress << ":"
// << (int)participant->port << std::endl;
return participant;
}
}
@ -165,18 +175,21 @@ void ParticipantRegistry::Add(Participant* participant) {
if (foundParticipant == nullptr) {
#if defined(NO_STD)
//this->things[this->thingCount++] = thing;
// this->things[this->thingCount++] = thing;
#else
ParticipantRegistry::participants.push_back(participant);
#endif
// std::cout << "Add participant " << participant->ipAddress << ":"
// << participant->port << "[" << (int)participant->networkId
// << "]\n";
// std::cout << "participants " << ParticipantRegistry::participants.size()
// std::cout << "participants " <<
// ParticipantRegistry::participants.size()
// << "\n";
// } else {
// std::cout << "Did not add, existing participant " << participant->ipAddress
// << ":" << participant->port << "[" << (int)participant->networkId
// std::cout << "Did not add, existing participant " <<
// participant->ipAddress
// << ":" << participant->port << "[" <<
// (int)participant->networkId
// << "]\n";
}
}

12
Participant.h
View File

@ -1,4 +1,5 @@
#pragma once
#include "Thing.h"
namespace RoboidControl {
@ -36,6 +37,7 @@ class ParticipantRegistry {
public:
Participant** GetAll() const;
int count = 0;
private:
Participant** participants;
#else
@ -43,6 +45,7 @@ class ParticipantRegistry {
/// @brief Get all participants
/// @return All participants
const std::list<Participant*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<Participant*> participants;
@ -57,6 +60,9 @@ class ParticipantRegistry {
/// reference to remote participants.
class Participant {
public:
/// @brief The name of the participant
const char* name = "Participant";
/// @brief The Ip Address of a participant.
const char* ipAddress = "0.0.0.0";
/// @brief The port number for UDP communication with the participant.
@ -65,6 +71,7 @@ class Participant {
/// @brief The network Id to identify the participant
unsigned char networkId = 0;
Participant();
/// @brief Create a new participant with the given communcation info
/// @param ipAddress The IP address of the participant
/// @param port The UDP port of the participant
@ -72,7 +79,10 @@ class Participant {
/// @brief Destructor for the participant
~Participant();
static Participant& LocalParticipant();
static Participant* LocalParticipant;
static void ReplaceLocalParticipant(Participant& newParticipant);
Thing* root = new Thing(this);
public:
#if defined(NO_STD)

50
Participants/ParticipantUDP.cpp
View File

@ -1,5 +1,6 @@
#include "ParticipantUDP.h"
#include "Participant.h"
#include "Thing.h"
#include "Arduino/ArduinoParticipant.h"
@ -11,20 +12,38 @@
namespace RoboidControl {
#pragma region Init
ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) {
this->name = "ParticipantUDP";
this->remoteSite = nullptr;
if (this->port == 0)
this->isIsolated = true;
Participant::registry.Add(this);
this->root = Thing::LocalRoot(); //::LocalParticipant->root;
this->root->owner = this;
this->root->name = "UDP Root";
this->Add(this->root);
Participant::ReplaceLocalParticipant(*this);
}
ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
: Participant("127.0.0.1", localPort) {
this->name = "ParticipantUDP";
if (this->port == 0)
this->isIsolated = true;
else
this->remoteSite = new Participant(ipAddress, port);
Participant::registry.Add(this);
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
this->root->owner = this;
this->root->name = "UDP Root";
this->Add(this->root);
Participant::ReplaceLocalParticipant(*this);
}
static ParticipantUDP* isolatedParticipant = nullptr;
@ -64,11 +83,19 @@ void ParticipantUDP::SetupUDP(int localPort,
this->connected = true;
}
#pragma endregion Init
#pragma region Update
// The update order
// 1. receive external messages
// 2. update the state
// 3. send out the updated messages
void ParticipantUDP::Update() {
unsigned long currentTimeMs = Thing::GetTimeMs();
PrepMyThings();
if (this->isIsolated == false) {
if (this->connected == false)
begin();
@ -91,11 +118,22 @@ void ParticipantUDP::Update() {
UpdateOtherThings();
}
void ParticipantUDP::PrepMyThings() {
for (Thing* thing : this->things) {
if (thing == nullptr)
continue;
thing->PrepareForUpdate();
}
}
void ParticipantUDP::UpdateMyThings() {
// std::cout << this->things.size() << std::endl;
for (Thing* thing : this->things) {
if (thing == nullptr) // || thing->GetParent() != nullptr)
continue;
// std::cout << thing->name << "\n";
if (thing->hierarchyChanged) {
if (!(this->isIsolated || this->networkId == 0)) {
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
@ -110,12 +148,14 @@ void ParticipantUDP::UpdateMyThings() {
}
}
// std::cout << "B\n";
// Why don't we do recursive?
// Because when a thing creates a thing in the update,
// that new thing is not sent out (because of hierarchyChanged)
// before it is updated itself: it is immediatedly updated!
thing->Update(false);
// std::cout << "C\n";
if (!(this->isIsolated || this->networkId == 0)) {
if (thing->terminate) {
DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
@ -136,8 +176,10 @@ void ParticipantUDP::UpdateMyThings() {
delete binaryMsg;
}
}
// std::cout << "D\n";
if (thing->terminate)
this->Remove(thing);
// std::cout << "E\n";
}
}
@ -200,8 +242,8 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
if (bufferSize <= 0)
return true;
// std::cout << "send msg " << (int)this->buffer[0] << " to "
// << remoteParticipant->ipAddress << std::endl;
// std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
// << " to " << remoteParticipant->ipAddress << std::endl;
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
@ -378,14 +420,14 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process SiteMsg " << (int)this->networkId
std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
<< " -> " << (int)msg->networkId << "\n";
#endif
if (this->networkId != msg->networkId) {
this->networkId = msg->networkId;
// std::cout << this->things.size() << " things\n";
std::cout << this->things.size() << " things\n";
for (Thing* thing : this->things)
this->SendThingInfo(sender, thing);
}

5
Participants/ParticipantUDP.h
View File

@ -78,9 +78,6 @@ class ParticipantUDP : public Participant {
/// local network
long publishInterval = 3000; // 3 seconds
/// @brief The name of the participant
const char* name = "ParticipantUDP";
protected:
char buffer[1024];
@ -105,6 +102,8 @@ class ParticipantUDP : public Participant {
protected:
unsigned long nextPublishMe = 0;
/// @brief Prepare the local things for the next update
virtual void PrepMyThings();
virtual void UpdateMyThings();
virtual void UpdateOtherThings();

7
Participants/SiteServer.cpp
View File

@ -73,10 +73,13 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) {
Thing* thing = sender->Get(msg->thingId);
if (thing == nullptr)
// new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
Thing::Reconstruct(sender, msg->thingType, msg->thingId);
// Thing::Reconstruct(sender, msg->thingType, msg->thingId);
//thing = new Thing(msg->thingType, sender->root);
;
thing->id = msg->thingId;
if (msg->parentId != 0) {
thing->SetParent(*Get(msg->parentId));
thing->SetParent(Get(msg->parentId));
if (thing->IsRoot())
// if (thing->GetParent() != nullptr)
#if defined(NO_STD)

84
Thing.cpp
View File

@ -20,12 +20,31 @@ namespace RoboidControl {
#pragma region Init
Thing& Thing::LocalRoot() {
static Thing localRoot(Thing::Type::Undetermined, localRoot);
Thing* Thing::LocalRoot() {
Participant* p = Participant::LocalParticipant;
Thing* localRoot = p->root;
return localRoot;
}
Thing::Thing(unsigned char thingType, Thing& parent) {
// Only use this for root things
Thing::Thing(Participant* owner) {
this->type = Type::Roboid; // should become root
this->position = Spherical::zero;
this->positionUpdated = true;
this->orientation = SwingTwist::identity;
this->orientationUpdated = true;
this->hierarchyChanged = true;
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
this->owner = owner;
//this->owner->Add(this, true);
std::cout << this->owner->name << ": New root thing " << std::endl;
}
Thing::Thing(unsigned char thingType, Thing* parent) {
this->type = thingType;
this->position = Spherical::zero;
@ -37,9 +56,16 @@ Thing::Thing(unsigned char thingType, Thing& parent) {
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
this->owner = &Participant::LocalParticipant();
this->owner = parent->owner;
this->owner->Add(this, true);
this->SetParent(parent);
std::cout << this->owner->name << ": New thing for " << parent->name
<< std::endl;
}
Thing::~Thing() {
std::cout << "Destroy thing " << this->name << std::endl;
}
// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType,
@ -66,28 +92,28 @@ void Thing::SetModel(const char* url) {
#pragma region Hierarchy
void Thing::SetParent(Thing* parent) {
if (parent == nullptr) {
Thing* parentThing = this->parent;
if (parentThing != nullptr)
parentThing->RemoveChild(this);
this->parent = nullptr;
} else
parent->AddChild(this);
this->hierarchyChanged = true;
}
// void Thing::SetParent(Thing* parent) {
// if (parent == nullptr) {
// Thing* parentThing = this->parent;
// if (parentThing != nullptr)
// parentThing->RemoveChild(this);
// this->parent = nullptr;
// } else
// parent->AddChild(this);
// this->hierarchyChanged = true;
// }
void Thing::SetParent(Thing& parent) {
parent.AddChild(this);
this->hierarchyChanged = true;
}
const Thing& Thing::GetParent() {
return *this->parent;
}
// const Thing& Thing::GetParent() {
// return *this->parent;
// }
bool Thing::IsRoot() const {
return this == &LocalRoot(); //&Thing::Root;
return this == LocalRoot() || this->parent == nullptr; //&Thing::Root;
}
// void Thing::SetParent(Thing* root, const char* name) {
@ -96,9 +122,9 @@ bool Thing::IsRoot() const {
// this->SetParent(thing);
// }
// Thing* Thing::GetParent() {
// return this->parent;
// }
Thing* Thing::GetParent() {
return this->parent;
}
Thing* Thing::GetChildByIndex(unsigned char ix) {
return this->children[ix];
@ -110,11 +136,11 @@ void Thing::AddChild(Thing* child) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
newChildren[childIx] = this->children[childIx];
// if (this->children[childIx] == child) {
// // child is already present, stop copying do not update the children
// delete[] newChildren;
// return;
// }
if (this->children[childIx] == child) {
// child is already present, stop copying do not update the children
delete[] newChildren;
return;
}
}
newChildren[this->childCount] = child;
@ -143,7 +169,7 @@ Thing* Thing::RemoveChild(Thing* child) {
}
}
child->parent = &Thing::LocalRoot();
child->parent = Thing::LocalRoot();
delete[] this->children;
this->children = newChildren;
@ -248,6 +274,8 @@ unsigned long Thing::GetTimeMs() {
// Update(GetTimeMs(), recursive);
// }
void Thing::PrepareForUpdate() {}
void Thing::Update(bool recursive) {
// if (this->positionUpdated || this->orientationUpdated)
// OnPoseChanged callback

29
Thing.h
View File

@ -41,8 +41,15 @@ class Thing {
};
#pragma region Init
static Thing& LocalRoot();
static Thing* LocalRoot();
private:
// Special constructor to create a root thing
Thing(Participant* parent);
// Which can only be used by the Participant
friend class Participant;
public:
/// @brief Create a new thing
/// @param thingType The type of thing (can use Thing::Type)
/// @param parent (optional) The parent thing
@ -50,7 +57,7 @@ class Thing {
/// be Participant::LocalParticipant if the parent is not specified. A thing
/// without a parent will be a root thing.
Thing(unsigned char thingType = Thing::Type::Undetermined,
Thing& parent = LocalRoot());
Thing* parent = LocalRoot());
/// @brief Create a new child thing
/// @param parent The parent thing
@ -59,6 +66,8 @@ class Thing {
/// an ID
/// @note The owner will be the same as the owner of the parent thing
~Thing();
static Thing Reconstruct(Participant* owner,
unsigned char thingType,
unsigned char thingId);
@ -73,6 +82,7 @@ class Thing {
/// @brief The participant managing this thing
Participant* owner = nullptr;
/// @brief The ID of the thing
unsigned char id = 0;
@ -108,11 +118,11 @@ class Thing {
/// @brief Sets the parent of this Thing
/// @param parent The Thing which should become the parent
// virtual void SetParent(Thing* parent);
void SetParent(Thing& parent);
void SetParent(Thing* parent);
/// @brief Gets the parent of this Thing
/// @return The parent Thing
// Thing* GetParent();
const Thing& GetParent();
Thing* GetParent();
bool IsRoot() const;
@ -215,12 +225,7 @@ class Thing {
#pragma region Update
public:
/// @brief Get the current time in milliseconds
/// @return The current time in milliseconds
static unsigned long GetTimeMs();
/// @brief Updates the state of the thing
// void Update(bool recursive = false);
virtual void PrepareForUpdate();
/// @brief Updates the state of the thing
/// @param currentTimeMs The current clock time in milliseconds; if this is
@ -230,6 +235,10 @@ class Thing {
static void UpdateThings();
/// @brief Get the current time in milliseconds
/// @return The current time in milliseconds
static unsigned long GetTimeMs();
#pragma endregion Update
public:

14
Things/ControlledMotor.cpp
View File

@ -2,9 +2,9 @@
namespace RoboidControl {
ControlledMotor::ControlledMotor(Motor& motor,
RelativeEncoder& encoder,
Thing& parent)
ControlledMotor::ControlledMotor(Motor* motor,
RelativeEncoder* encoder,
Thing* parent)
: Motor(parent), motor(motor), encoder(encoder) {
this->type = Type::ControlledMotor;
// encoder.SetParent(*this);
@ -19,9 +19,9 @@ void ControlledMotor::SetTargetVelocity(float velocity) {
}
void ControlledMotor::Update(bool recurse) {
encoder.Update(false);
encoder->Update(false);
this->actualVelocity = (int)rotationDirection * encoder.rotationSpeed;
this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
unsigned long currentTimeMs = GetTimeMs();
float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
@ -31,10 +31,10 @@ void ControlledMotor::Update(bool recurse) {
float acceleration =
error * timeStep * pidP; // Just P is used at this moment
std::cout << "motor acc. " << acceleration << std::endl;
motor.SetTargetVelocity(targetVelocity +
motor->SetTargetVelocity(targetVelocity +
acceleration); // or something like that
motor.Update(false);
motor->Update(false);
}
// float ControlledMotor::GetActualVelocity() {

6
Things/ControlledMotor.h
View File

@ -10,7 +10,7 @@ namespace RoboidControl {
/// The speed is measured in revolutions per second.
class ControlledMotor : public Motor {
public:
ControlledMotor(Motor& motor, RelativeEncoder& encoder, Thing& parent = Thing::LocalRoot());
ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot());
float pidP = 1;
float pidD = 0;
@ -28,8 +28,8 @@ class ControlledMotor : public Motor {
/// @param speed the target velocity in revolutions per second
virtual void SetTargetVelocity(float velocity) override;
Motor& motor;
RelativeEncoder& encoder;
Motor* motor;
RelativeEncoder* encoder;
protected:
float lastUpdateTime;

41
Things/DifferentialDrive.cpp
View File

@ -1,11 +1,27 @@
#include "DifferentialDrive.h"
#include "Messages/LowLevelMessages.h"
namespace RoboidControl {
DifferentialDrive::DifferentialDrive(Thing& parent)
DifferentialDrive::DifferentialDrive(Thing* parent)
: Thing(Type::DifferentialDrive, parent) {
this->leftWheel = new Motor();
this->rightWheel = new Motor();
this->name = "Differential drive";
this->leftWheel = new Motor(this);
this->leftWheel->name = "Left motor";
this->rightWheel = new Motor(this);
this->rightWheel->name = "Right motor";
}
DifferentialDrive::DifferentialDrive(Motor* leftMotor,
Motor* rightMotor,
Thing* parent)
: Thing(Type::DifferentialDrive, parent) {
this->name = "Differential drive";
this->leftWheel = leftMotor;
this->rightWheel = rightMotor;
}
void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
@ -23,13 +39,13 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
}
Motor& DifferentialDrive::GetMotorLeft() {
return *this->leftWheel;
}
// Motor& DifferentialDrive::GetMotorLeft() {
// return *this->leftWheel;
// }
Motor& DifferentialDrive::GetMotorRight() {
return *this->rightWheel;
}
// Motor& DifferentialDrive::GetMotorRight() {
// return *this->rightWheel;
// }
void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) {
float distance = this->wheelSeparation / 2;
@ -78,4 +94,11 @@ void DifferentialDrive::Update(bool recursive) {
Thing::Update(recursive);
}
int DifferentialDrive::GenerateBinary(char* data, unsigned char* ix) {
data[(*ix)++] = this->leftWheel->id;
data[(*ix)++] = this->rightWheel->id;
LowLevelMessages::SendFloat16(data, ix, this->wheelRadius);
return 4;
}
} // namespace RoboidControl

29
Things/DifferentialDrive.h
View File

@ -14,7 +14,9 @@ class DifferentialDrive : public Thing {
/// @param parent The parent thing
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
DifferentialDrive(Thing& parent = Thing::LocalRoot());
DifferentialDrive(Thing* parent = Thing::LocalRoot());
DifferentialDrive(Motor* leftMotor, Motor* rightMotor, Thing* parent = Thing::LocalRoot());
/// @brief Configures the dimensions of the drive
/// @param wheelDiameter The diameter of the wheels in meters
@ -25,8 +27,8 @@ class DifferentialDrive : public Thing {
/// @sa SetLinearVelocity SetAngularVelocity
void SetDriveDimensions(float wheelDiameter, float wheelSeparation);
Motor& GetMotorLeft();
Motor& GetMotorRight();
// Motor& GetMotorLeft();
// Motor& GetMotorRight();
/// @brief Congures the motors for the wheels
/// @param leftWheel The motor for the left wheel
/// @param rightWheel The motor for the right wheel
@ -42,20 +44,23 @@ class DifferentialDrive : public Thing {
/// @copydoc RoboidControl::Thing::Update(unsigned long)
virtual void Update(bool recursive = true) override;
protected:
/// @brief The radius of a wheel in meters
float wheelRadius = 1.0f;
/// @brief The distance between the wheels in meters
float wheelSeparation = 1.0f;
/// @brief Convert revolutions per second to meters per second
float rpsToMs = 1.0f;
int GenerateBinary(char* bytes, unsigned char* ix) override;
// virtual void ProcessBinary(char* bytes) override;
/// @brief The left wheel
Motor* leftWheel = nullptr;
/// @brief The right wheel
Motor* rightWheel = nullptr;
protected:
/// @brief The radius of a wheel in meters
float wheelRadius = 0.0f;
/// @brief The distance between the wheels in meters
float wheelSeparation = 0.0f;
/// @brief Convert revolutions per second to meters per second
float rpsToMs = 1.0f;
};
} // namespace RoboidControl

2
Things/DigitalSensor.cpp
View File

@ -13,7 +13,7 @@ namespace RoboidControl {
// DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {}
// DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {}
DigitalSensor::DigitalSensor(Thing& parent) : Thing(Type::Switch, parent) {}
DigitalSensor::DigitalSensor(Thing* parent) : Thing(Type::Switch, parent) {}
int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
bytes[(*ix)++] = state ? 1 : 0;

2
Things/DigitalSensor.h
View File

@ -19,7 +19,7 @@ class DigitalSensor : public Thing {
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
// DigitalSensor(Thing* parent); //, unsigned char thingId = 0);
DigitalSensor(Thing& parent = Thing::LocalRoot());
DigitalSensor(Thing* parent = Thing::LocalRoot());
/// @brief The sigital state
bool state = 0;

15
Things/Motor.cpp
View File

@ -2,16 +2,15 @@
namespace RoboidControl {
// RoboidControl::Motor::Motor(Participant* owner)
// : Thing(owner, Type::UncontrolledMotor) {}
Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {}
// RoboidControl::Motor::Motor(Thing* parent)
// : Thing(parent, Type::UncontrolledMotor) {}
Motor::Motor(Thing& parent) : Thing(Type::UncontrolledMotor, parent) {}
void RoboidControl::Motor::SetTargetVelocity(float targetSpeed) {
void Motor::SetTargetVelocity(float targetSpeed) {
this->targetVelocity = targetSpeed;
}
int Motor::GenerateBinary(char* data, unsigned char* ix) {
data[(*ix)++] = this->targetVelocity * 127.0f;
return 1;
}
} // namespace RoboidControl

9
Things/Motor.h
View File

@ -6,9 +6,7 @@ namespace RoboidControl {
class Motor : public Thing {
public:
Motor(Participant* owner);
// Motor(Thing* parent);
Motor(Thing& parent = Thing::LocalRoot());
Motor(Thing* parent = Thing::LocalRoot());
/// @brief Motor turning direction
enum class Direction { Clockwise = 1, CounterClockwise = -1 };
@ -17,7 +15,10 @@ class Motor : public Thing {
virtual void SetTargetVelocity(float velocity); // -1..0..1
protected:
int GenerateBinary(char* bytes, unsigned char* ix) override;
// virtual void ProcessBinary(char* bytes) override;
//protected:
float targetVelocity = 0;
};

2
Things/RelativeEncoder.cpp
View File

@ -2,7 +2,7 @@
namespace RoboidControl {
RelativeEncoder::RelativeEncoder(Thing& parent)
RelativeEncoder::RelativeEncoder(Thing* parent)
: Thing(Type::IncrementalEncoder, parent) {}
float RelativeEncoder::GetRotationSpeed() {

2
Things/RelativeEncoder.h
View File

@ -16,7 +16,7 @@ class RelativeEncoder : public Thing {
/// rotation
RelativeEncoder(Participant* owner);
// RelativeEncoder(Thing* parent);
RelativeEncoder(Thing& parent = Thing::LocalRoot());
RelativeEncoder(Thing* parent = Thing::LocalRoot());
/// @brief Get the rotation speed
/// @return The speed in revolutions per second

2
Things/TemperatureSensor.cpp
View File

@ -10,7 +10,7 @@ namespace RoboidControl {
// TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {}
TemperatureSensor::TemperatureSensor(Thing& parent) : Thing(Type::TemperatureSensor, parent) {}
TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(Type::TemperatureSensor, parent) {}
// TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {}

2
Things/TemperatureSensor.h
View File

@ -14,7 +14,7 @@ class TemperatureSensor : public Thing {
/// @param thingId The ID of the thing
TemperatureSensor(Participant* participant); //, unsigned char thingId);
// TemperatureSensor(Thing* parent);
TemperatureSensor(Thing& parent = Thing::LocalRoot());
TemperatureSensor(Thing* parent = Thing::LocalRoot());
/// @brief The measured temperature
float temperature = 0;

20
Things/TouchSensor.cpp
View File

@ -2,15 +2,29 @@
namespace RoboidControl {
TouchSensor::TouchSensor(Thing& parent) : Thing(Type::TouchSensor, parent) {}
TouchSensor::TouchSensor(Thing* parent) : Thing(Type::TouchSensor, parent) {
this->name = "Touch sensor";
}
void TouchSensor::PrepareForUpdate() {
this->touchedSomething = this->externalTouch;
}
void TouchSensor::Update(bool recursive) {
Thing::Update(recursive);
}
int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
bytes[(*ix)++] = touchedSomething ? 1 : 0;
bytes[(*ix)++] = this->touchedSomething ? 1 : 0;
return 1;
}
void TouchSensor::ProcessBinary(char* bytes) {
this->touchedSomething |= (bytes[0] == 1);
this->externalTouch = (bytes[0] == 1);
if (this->externalTouch)
std::cout << "touching!\n";
else
std::cout << "not touching\n";
}
} // namespace RoboidControl

7
Things/TouchSensor.h
View File

@ -12,12 +12,15 @@ class TouchSensor : public Thing {
/// @param parent The parent thing
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
TouchSensor(Thing& parent = Thing::LocalRoot());
TouchSensor(Thing* parent = Thing::LocalRoot());
/// @brief Value which is true when the sensor is touching something, false
/// otherwise
bool touchedSomething = false;
virtual void PrepareForUpdate() override;
virtual void Update(bool recursive) override;
/// @brief Function used to generate binary data for this touch sensor
/// @param buffer The byte array for thw binary data
/// @param ix The starting position for writing the binary data
@ -25,6 +28,8 @@ class TouchSensor : public Thing {
/// @brief Function used to process binary data received for this touch sensor
/// @param bytes The binary data to process
virtual void ProcessBinary(char* bytes) override;
protected:
bool externalTouch = false;
};
} // namespace RoboidControl