diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index 2b9330a..a787d5a 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -1,5 +1,9 @@ #include "ArduinoParticipant.h" +#if !defined(NO_STD) +#include +#endif + #if defined(ARDUINO) #if defined(ARDUINO_ARCH_ESP8266) #include @@ -19,17 +23,19 @@ namespace RoboidControl { namespace Arduino { -void ParticipantUDP::Setup(int localPort, - const char* remoteIpAddress, - int remotePort) { #if defined(ARDUINO) && defined(HAS_WIFI) - this->remoteIpAddress = remoteIpAddress; - this->remotePort = remotePort; +WiFiUDP* udp; +#endif + +void ParticipantUDP::Setup() { +#if defined(ARDUINO) && defined(HAS_WIFI) GetBroadcastAddress(); #if defined(UNO_R4) if (WiFi.status() == WL_NO_MODULE) { +#if !defined(NO_STD) std::cout << "No network available!\n"; +#endif return; } #else @@ -38,9 +44,16 @@ void ParticipantUDP::Setup(int localPort, return; } #endif - udp.begin(localPort); - std::cout << "Wifi sync started to port " << this->remotePort << "\n"; + udp = new WiFiUDP(); + udp->begin(this->port); + +#if !defined(NO_STD) + std::cout << "Wifi sync started local " << this->port; + if (this->remoteSite != nullptr) + std::cout << ", remote " << this->remoteSite->ipAddress << ":" + << this->remoteSite->port << "\n"; +#endif #endif } @@ -52,36 +65,25 @@ void ParticipantUDP::GetBroadcastAddress() { this->broadcastIpAddress = new char[broadcastIpString.length() + 1]; broadcastIpString.toCharArray(this->broadcastIpAddress, broadcastIpString.length() + 1); +#if !defined(NO_STD) std::cout << "Broadcast address: " << broadcastIpAddress << "\n"; #endif +#endif } void ParticipantUDP::Receive() { #if defined(ARDUINO) && defined(HAS_WIFI) - int packetSize = udp.parsePacket(); + int packetSize = udp->parsePacket(); while (packetSize > 0) { - udp.read(buffer, packetSize); + udp->read(buffer, packetSize); - String senderAddress = udp.remoteIP().toString(); + String senderAddress = udp->remoteIP().toString(); char sender_ipAddress[16]; senderAddress.toCharArray(sender_ipAddress, 16); - unsigned int sender_port = udp.remotePort(); + unsigned int sender_port = udp->remotePort(); - // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, - // sender_port); if (remoteParticipant == nullptr) { - // remoteParticipant = this->AddParticipant(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(); + packetSize = udp->parsePacket(); } #endif } @@ -92,15 +94,22 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { // << remoteParticipant->port << "\n"; int n = 0; + int r = 0; do { if (n > 0) { +#if !defined(NO_STD) std::cout << "Retry sending\n"; +#endif delay(10); } n++; - udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port); - udp.write((unsigned char*)buffer, bufferSize); - } while (udp.endPacket() == 0 && n < 10); + + udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port); + udp->write((unsigned char*)buffer, bufferSize); + 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; @@ -112,9 +121,9 @@ bool ParticipantUDP::Publish(IMessage* msg) { if (bufferSize <= 0) return true; - udp.beginPacket(this->broadcastIpAddress, this->remotePort); - udp.write((unsigned char*)buffer, bufferSize); - udp.endPacket(); + udp->beginPacket(this->broadcastIpAddress, this->port); + udp->write((unsigned char*)buffer, bufferSize); + udp->endPacket(); // std::cout << "Publish to " << this->broadcastIpAddress << ":" // << this->remotePort << "\n"; diff --git a/Arduino/ArduinoParticipant.h b/Arduino/ArduinoParticipant.h index 94196d7..5a9396f 100644 --- a/Arduino/ArduinoParticipant.h +++ b/Arduino/ArduinoParticipant.h @@ -2,29 +2,19 @@ #include "Participants/ParticipantUDP.h" -#if defined(HAS_WIFI) -#include -#endif - namespace RoboidControl { namespace Arduino { class ParticipantUDP : public RoboidControl::ParticipantUDP { public: - void Setup(int localPort, const char* remoteIpAddress, int remotePort); + void Setup(); void Receive(); bool Send(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: -#if defined(HAS_WIFI) - const char* remoteIpAddress = nullptr; - unsigned short remotePort = 0; char* broadcastIpAddress = nullptr; - WiFiUDP udp; -#endif - void GetBroadcastAddress(); }; diff --git a/Arduino/Examples/BB2A/BB2A.cpp b/Arduino/Examples/BB2A/BB2A.cpp new file mode 100644 index 0000000..440fc87 --- /dev/null +++ b/Arduino/Examples/BB2A/BB2A.cpp @@ -0,0 +1,95 @@ +#include "Arduino.h" + +#include "Things/ControlledMotor.h" +#include "Things/DifferentialDrive.h" +#include "Things/RelativeEncoder.h" +#include "Things/TouchSensor.h" + +#include "Arduino/Things/DRV8833.h" +#include "Arduino/Things/DigitalInput.h" +#include "Arduino/Things/UltrasonicSensor.h" +#include "Arduino/ArduinoUtils.h" + +#include "Participants/ParticipantUDP.h" + +#include "configuration.h" + +#include + +using namespace RoboidControl; +using namespace RoboidControl::Arduino; + +ParticipantUDP* localParticipant; + +DifferentialDrive* bb2b; +TouchSensor* touchLeft; +TouchSensor* touchRight; + +// RelativeEncoder* encoderLeft; +// RelativeEncoder* encoderRight; + +void setup() { + Serial.begin(115200); + delay(3000); + Serial.println("started"); + + StartWifi("serrarens", "192.168.76.44"); + localParticipant = new ParticipantUDP("192.168.77.76"); + + bb2b = new DifferentialDrive(); + touchLeft = new TouchSensor(bb2b); + touchRight = new TouchSensor(bb2b); + + // bb2b = new DRV8833::DifferentialDrive(driveConfig); + // touchLeft = new UltrasonicSensor::TouchSensor(leftTouchConfig, bb2b); + // touchRight = new UltrasonicSensor::TouchSensor(rightTouchConfig, bb2b); + + touchLeft->name = "Left Touch Sensor"; + touchLeft->SetPosition(Spherical::Degrees(0.15, -30, 0)); + touchRight->name = "Right Touch Sensor"; + touchRight->SetPosition(Spherical::Degrees(0.15, 30, 0)); + + // encoderLeft = new DigitalInput::RelativeEncoder(leftEncoderConfig); + // encoderRight = new DigitalInput::RelativeEncoder(rightEncoderConfig); + + // bb2b->leftWheel = new ControlledMotor(bb2b->leftWheel, encoderLeft, bb2b); + // bb2b->rightWheel = new ControlledMotor(bb2b->rightWheel, encoderRight, bb2b); +} + +void loop() { + + // std::cout << touchLeft->touchedSomething << " | " + // << touchRight->touchedSomething << std::endl; + // std::cout << encoderLeft->rotationSpeed << " : " + // << encoderRight->rotationSpeed << std::endl; + // std::cout << bb2b->leftWheel->encoder->rotationSpeed + // << " :: " << bb2b->rightWheel->encoder->rotationSpeed << std::endl; + + + // The left wheel turns forward when nothing is touched on the right side + // and turn backward when the roboid hits something on the right + float leftMotorVelocity = (touchRight->IsTouching()) ? -1.0f : 1.0f; + // The right wheel does the same, but instead is controlled by + // touches on the left side + float rightMotorVelocity = (touchLeft->IsTouching()) ? -1.0f : 1.0f; + // When both sides are touching something, both wheels will turn backward + // and the roboid will move backwards + + bb2b->leftWheel->SetTargetVelocity(leftMotorVelocity); + bb2b->rightWheel->SetTargetVelocity(rightMotorVelocity); + // std::cout << " " << bb2b->leftWheel->GetTargetVelocity() << " : " << bb2b->rightWheel->GetTargetVelocity() << std::endl; + + // float leftWheelVelocity = (touchRight->touchedSomething) ? -1.0f : 1.0f; + // float rightWheelVelocity = (touchLeft->touchedSomething) ? -1.0f : 1.0f; + // bb2b->SetWheelVelocity(leftWheelVelocity, rightWheelVelocity); + // std::cout << " " << leftWheelVelocity << " # " << rightWheelVelocity << std::endl; + + // std::cout << leftMotor->actualVelocity << std::endl; + + //Serial.println("."); + // Update the roboid state + localParticipant->Update(); + + // and sleep for 100ms + delay(10); +} diff --git a/Arduino/Examples/BB2A/configuration.h b/Arduino/Examples/BB2A/configuration.h new file mode 100644 index 0000000..dc4e4e1 --- /dev/null +++ b/Arduino/Examples/BB2A/configuration.h @@ -0,0 +1,52 @@ +#pragma once + +#include "Arduino/Things/UltrasonicSensor.h" +#include "Arduino/Things/DRV8833.h" +#include "Arduino/Things/DigitalInput.h" + +using namespace RoboidControl::Arduino; + +#if defined(ESP32) +static constexpr DRV8833::Configuration driveConfig = { + 17, // AIn1 + 16, // AIn2 + 14, // BIn1 + 27 // BIn2 +}; +static constexpr UltrasonicSensor::Configuration leftTouchConfig = { + 25, // Trigger + 33 // Echo +}; +static constexpr UltrasonicSensor::Configuration rightTouchConfig = { + 15, // Trigger + 5 // Echo +}; + +#elif defined(UNO) || defined(UNO_R4) +static constexpr DRV8833::Configuration driveConfig = { + 5, // AIn1 + 6, // AIn2 + 7, // BIn1 + 10 // BIn2 +}; + +static constexpr UltrasonicSensor::Configuration leftTouchConfig = { + A0, // Trigger + 12 // Echo +}; +static constexpr UltrasonicSensor::Configuration rightTouchConfig = { + A1, // Trigger + 11 // Echo +}; + +static constexpr DigitalInput::RelativeEncoder::Configuration + leftEncoderConfig = { + 2, // Input pin + 80 // Pulses per revolution +}; +static constexpr DigitalInput::RelativeEncoder::Configuration + rightEncoderConfig = { + 3, // Input pin + 80 // Pulses per revolution +}; +#endif \ No newline at end of file diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index d90492e..9ab9839 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -5,49 +5,83 @@ namespace RoboidControl { namespace Arduino { -DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse) - : Thing(participant) { +#pragma region DRV8833 + +DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) { + this->type = Type::Undetermined; + this->name = "DRV8833"; + this->pinStandby = config.standby; + if (pinStandby != 255) + pinMode(pinStandby, OUTPUT); + + 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->SetName("Motor B"); +} + +#pragma endregion DRV8833 + +#pragma region Differential drive + +DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, + Thing* parent) + : RoboidControl::DifferentialDrive(nullptr, nullptr, parent) { + this->drv8833 = new DRV8833(config, this); + this->leftWheel = this->drv8833->motorA; + this->rightWheel = this->drv8833->motorB; +} + +void DRV8833::DifferentialDrive::Update(bool recurse) { + RoboidControl::DifferentialDrive::Update(recurse); + this->drv8833->Update(false); +} + +#pragma endregion Differential drive + +#pragma region Motor + +#if (ESP32) +uint8_t DRV8833Motor::nextAvailablePwmChannel = 0; +#endif + +DRV8833Motor::DRV8833Motor(DRV8833* driver, + unsigned char pinIn1, + unsigned char pinIn2, + bool reverse) + : Motor(driver) { + this->pinIn1 = pinIn1; this->pinIn2 = pinIn2; #if (ESP32) - in1Ch = nextAvailablePwmChannel++; + in1Ch = DRV8833Motor::nextAvailablePwmChannel++; ledcSetup(in1Ch, 500, 8); ledcAttachPin(pinIn1, in1Ch); - in2Ch = nextAvailablePwmChannel++; + + in2Ch = DRV8833Motor::nextAvailablePwmChannel++; ledcSetup(in2Ch, 500, 8); ledcAttachPin(pinIn2, in2Ch); + #else pinMode(pinIn1, OUTPUT); // configure the in1 pin to output mode pinMode(pinIn2, OUTPUT); // configure the in1 pin to output mode #endif - this->reverse = reverse; + // this->reverse = reverse; } -void DRV8833Motor::SetMaxRPM(unsigned int rpm) { - this->maxRpm = rpm; -} +// void DRV8833Motor::SetMaxRPM(unsigned int rpm) { +// this->maxRpm = rpm; +// } -void DRV8833Motor::SetAngularVelocity(Spherical velocity) { - Thing::SetAngularVelocity(velocity); - // ignoring rotation axis for now. - // Spherical angularVelocity = this->GetAngularVelocity(); - float angularSpeed = velocity.distance; // in degrees/sec +void DRV8833Motor::SetTargetVelocity(float motorSpeed) { + Motor::SetTargetVelocity(motorSpeed); - float rpm = angularSpeed / 360 * 60; - float motorSpeed = rpm / this->maxRpm; + uint8_t motorSignal = + (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255); - uint8_t motorSignal = (uint8_t)(motorSpeed * 255); - // if (direction == RotationDirection::CounterClockwise) - // speed = -speed; - // Determine the rotation direction - if (velocity.direction.horizontal.InDegrees() < 0) - motorSpeed = -motorSpeed; - if (this->reverse) - motorSpeed = -motorSpeed; - - // std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm " << rpm + // std::cout << "moto speed " << this->name << " = " << motorSpeed // << ", motor signal = " << (int)motorSignal << "\n"; #if (ESP32) @@ -97,24 +131,7 @@ void DRV8833Motor::SetAngularVelocity(Spherical velocity) { #endif } -DRV8833::DRV8833(Participant* participant, - unsigned char pinAIn1, - unsigned char pinAIn2, - unsigned char pinBIn1, - unsigned char pinBIn2, - unsigned char pinStandby, - bool reverseA, - bool reverseB) - : Thing(participant) { - this->pinStandby = pinStandby; - if (pinStandby != 255) - pinMode(pinStandby, OUTPUT); - - this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, reverseA); - this->motorA->name = "Motor A"; - this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, reverseB); - this->motorB->name = "Motor B"; -} +#pragma endregion Motor } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index cbbfb3c..9cc7d1a 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -1,60 +1,87 @@ #pragma once +#include +#include "Participants/IsolatedParticipant.h" #include "Thing.h" #include "Things/DifferentialDrive.h" +#include "Things/Motor.h" namespace RoboidControl { namespace Arduino { -/// @brief Support for a DRV8833 motor controller -class DRV8833Motor : public Thing { - public: - /// @brief Motor turning direction - enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 }; - - /// @brief Setup the DC 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(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false); - void SetMaxRPM(unsigned int rpm); - - virtual void SetAngularVelocity(Spherical velocity) override; - - bool reverse = false; - - protected: - unsigned char pinIn1 = 255; - unsigned char pinIn2 = 255; - unsigned int maxRpm = 200; -}; +class DRV8833Motor; class DRV8833 : public Thing { public: + struct Configuration { + int AIn1; + int AIn2; + int BIn1; + int BIn2; + int standby = 255; + + constexpr Configuration(int a1, int a2, int b1, int b2, int standby = 255) : AIn1(a1), AIn2(a2), BIn1(b1), BIn2(b2), standby(standby) {} + }; + /// @brief Setup a DRV8833 motor controller - /// @param pinAIn1 The pin number connected to the AIn1 port - /// @param pinAIn2 The pin number connected to the AIn2 port - /// @param pinBIn1 The pin number connected to the BIn1 port - /// @param pinBIn2 The pin number connceted to the BIn2 port - /// @param pinStandby The pin number connected to the standby port, 255 - /// indicated that the port is not connected - /// @param reverseA The forward turning direction of motor A - /// @param reverseB The forward turning direction of motor B - DRV8833(Participant* participant, - unsigned char pinAIn1, - unsigned char pinAIn2, - unsigned char pinBIn1, - unsigned char pinBIn2, - unsigned char pinStandby = 255, - bool reverseA = false, - bool reverseB = false); + DRV8833(Configuration config, Thing* parent = Thing::LocalRoot()); DRV8833Motor* motorA = nullptr; DRV8833Motor* motorB = nullptr; protected: unsigned char pinStandby = 255; + + public: + class DifferentialDrive; }; +#pragma region Differential drive + +class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { + public: + DifferentialDrive(DRV8833::Configuration config, Thing* parent = Thing::LocalRoot()); + + virtual void Update(bool recurse = false) override; + + protected: + DRV8833* drv8833 = nullptr; +}; + +#pragma endregion Differential drive + +#pragma region Motor + +/// @brief Support for a DRV8833 motor controller +class DRV8833Motor : public Motor { + public: + /// @brief Setup the DC 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, + unsigned char pinIn1, + unsigned char pinIn2, + bool reverse = false); + // void SetMaxRPM(unsigned int rpm); + + // virtual void SetAngularVelocity(Spherical velocity) override; + virtual void SetTargetVelocity(float targetSpeed) override; + // bool reverse = false; + + protected: + unsigned char pinIn1 = 255; + unsigned char pinIn2 = 255; + // unsigned int maxRpm = 200; + +#if (ESP32) + uint8_t in1Ch; + uint8_t in2Ch; + static uint8_t nextAvailablePwmChannel; +#endif +}; + +#pragma endregion Motor + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index d972ddb..e65aa1e 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -5,19 +5,126 @@ namespace RoboidControl { namespace Arduino { -DigitalInput::DigitalInput(Participant* participant, unsigned char pin) - : TouchSensor(participant) { +#pragma region Digital input + +DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(parent) { + this->type = Type::Switch; + this->name = "Digital Input"; this->pin = pin; - - pinMode(pin, INPUT); + pinMode(this->pin, INPUT); + std::cout << "digital input start\n"; } -void DigitalInput::Update(unsigned long currentTimeMs, bool recursive) { - this->touchedSomething = digitalRead(pin) == LOW; - // std::cout << "DigitalINput pin " << (int)this->pin << ": " << - // this->touchedSomething << "\n"; - Thing::Update(currentTimeMs, recursive); +void DigitalInput::Update(bool recursive) { + this->isHigh = digitalRead(this->pin); + this->isLow = !this->isHigh; + Thing::Update(recursive); } +#pragma endregion Digital input + +#pragma region Touch sensor + +DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent) + : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {} + +void DigitalInput::TouchSensor::Update(bool recursive) { + this->internalTouch = digitalInput.isLow; +} + +#pragma endregion Touch sensor + +#pragma region Relative encoder + +int DigitalInput::RelativeEncoder::interruptCount = 0; +volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0; +volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0; + +DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, + Thing* parent) + : RoboidControl::RelativeEncoder(parent), + digitalInput(config.pin, parent), + pulsesPerRevolution(config.pulsesPerRevolution) {} + +void DigitalInput::RelativeEncoder::Start() { + // We support up to 2 pulse counters + if (interruptCount == 0) { + std::cout << "pin interrupt 1 activated" << std::endl; + attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0, + RISING); + } else if (interruptCount == 1) { + std::cout << "pin interrupt 2 activated" << std::endl; + attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1, + RISING); + } else { + // maximum interrupt count reached + std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached" + << std::endl; + return; + } + interruptIx = interruptCount; + interruptCount++; + std::cout << "pin ints. " << interruptCount << std::endl; +} + +int DigitalInput::RelativeEncoder::GetPulseCount() { + if (interruptIx == 0) { + int pulseCount = pulseCount0; + pulseCount0 = 0; + return pulseCount; + } else if (interruptIx == 1) { + int pulseCount = pulseCount1; + pulseCount1 = 0; + return pulseCount; + } + return 0; +} + +void DigitalInput::RelativeEncoder::Update(bool recursive) { + unsigned long currentTimeMs = GetTimeMs(); + if (this->lastUpdateTime == 0) { + this->Start(); + this->lastUpdateTime = currentTimeMs; + return; + } + + float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f; + if (timeStep <= 0) + return; + + int pulseCount = GetPulseCount(); + netPulseCount += pulseCount; + + this->pulseFrequency = pulseCount / timeStep; + this->rotationSpeed = pulseFrequency / pulsesPerRevolution; + + // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency + // << " timestep " << timeStep << std::endl; + + this->lastUpdateTime = currentTimeMs; +} + +#if defined(ESP8266) || defined(ESP32) +void ICACHE_RAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt0() { + pulseCount0++; +} +#else +void DigitalInput::RelativeEncoder::PulseInterrupt0() { + pulseCount0++; +} +#endif + +#if defined(ESP8266) || defined(ESP32) +void IRAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt1() { + pulseCount1++; +} +#else +void DigitalInput::RelativeEncoder::PulseInterrupt1() { + pulseCount1++; +} +#endif + +#pragma endregion Relative encoder + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/DigitalInput.h b/Arduino/Things/DigitalInput.h index 6099918..e529dbc 100644 --- a/Arduino/Things/DigitalInput.h +++ b/Arduino/Things/DigitalInput.h @@ -1,26 +1,92 @@ #pragma once +#include "Things/RelativeEncoder.h" #include "Things/TouchSensor.h" namespace RoboidControl { namespace Arduino { /// @brief A digital input represents the stat of a digital GPIO pin -class DigitalInput : public TouchSensor { +class DigitalInput : public Thing { public: /// @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); + + bool isHigh = false; + bool isLow = false; /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) - virtual void Update(unsigned long currentTimeMs, - bool recursive = false) override; + virtual void Update(bool recursive = false) override; protected: /// @brief The pin used for digital input unsigned char pin = 0; + + public: + class TouchSensor; + class RelativeEncoder; }; +#pragma region Touch sensor + +class DigitalInput::TouchSensor : public RoboidControl::TouchSensor { + public: + TouchSensor(unsigned char pin, Thing* parent); + + /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) + virtual void Update(bool recurse = false) override; + + protected: + DigitalInput digitalInput; +}; + +#pragma endregion Touch sensor + +#pragma region Incremental encoder + +class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder { + public: + struct Configuration { + unsigned char pin; + unsigned char pulsesPerRevolution; + }; + + RelativeEncoder(Configuration config, Thing* parent = Thing::LocalRoot()); + + unsigned char pulsesPerRevolution; + + /// @brief The current pulse frequency in Hz + float pulseFrequency = 0; + + /// @copydoc RoboidControl::Thing::Update() + virtual void Update(bool recurse = false) override; + + protected: + DigitalInput digitalInput; + + int interruptIx = 0; + + static int interruptCount; + + volatile static int pulseCount0; + static void PulseInterrupt0(); + + volatile static int pulseCount1; + static void PulseInterrupt1(); + + int GetPulseCount(); + long netPulseCount = 0; + unsigned long lastUpdateTime = 0; + + private: + void Start(); +}; + +#pragma endregion Incremental encoder + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index d6deb64..8fa940d 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -1,16 +1,17 @@ #include "UltrasonicSensor.h" #include +#include namespace RoboidControl { namespace Arduino { -UltrasonicSensor::UltrasonicSensor(Participant* participant, - unsigned char pinTrigger, - unsigned char pinEcho) - : TouchSensor(participant) { - this->pinTrigger = pinTrigger; - this->pinEcho = pinEcho; +UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) + : Thing(parent) { + this->type = Type::DistanceSensor; + this->name = "Ultrasonic sensor"; + this->pinTrigger = config.trigger; + this->pinEcho = config.echo; pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode pinMode(pinEcho, INPUT); // configure the echo pin to input mode @@ -19,14 +20,14 @@ UltrasonicSensor::UltrasonicSensor(Participant* participant, float UltrasonicSensor::GetDistance() { // Start the ultrasonic 'ping' digitalWrite(pinTrigger, LOW); - delayMicroseconds(5); + delayMicroseconds(2); digitalWrite(pinTrigger, HIGH); delayMicroseconds(10); digitalWrite(pinTrigger, LOW); // Measure the duration of the pulse on the echo pin - float duration_us = - pulseIn(pinEcho, HIGH, 100000); // the result is in microseconds + unsigned long duration_us = + pulseIn(pinEcho, HIGH, 10000); // the result is in microseconds // Calculate the distance: // * Duration should be divided by 2, because the ping goes to the object @@ -37,32 +38,60 @@ float UltrasonicSensor::GetDistance() { // * Now we calculate the distance based on the speed of sound (340 m/s): // distance = duration_sec * 340; // * The result calculation is therefore: - this->distance = duration_us / 2 / 1000000 * 340; + this->distance = (float)duration_us / 2 / 1000000 * 340; + + // Serial.println(this->distance); + + // std::cout << "US distance " << this->distance << std::endl; // Filter faulty measurements. The sensor can often give values > 30 m which // are not correct // if (distance > 30) // distance = 0; - this->touchedSomething |= (this->distance <= this->touchDistance); - - // std::cout << "Ultrasonic " << this->distance << " " << - // this->touchedSomething << "\n"; - - return distance; + return this->distance; } -void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) { - this->touchedSomething = false; +void UltrasonicSensor::Update(bool recursive) { GetDistance(); - Thing::Update(currentTimeMs, recursive); + Thing::Update(recursive); } -// void UltrasonicSensor::ProcessBinary(char* bytes) { -// this->touchedSomething = (bytes[0] == 1); -// if (this->touchedSomething) -// std::cout << "Touching something!\n"; -// } +#pragma region Distance sensor + +UltrasonicSensor::DistanceSensor::DistanceSensor( + UltrasonicSensor::Configuration config, + Thing* parent) + : RoboidControl::DistanceSensor(parent), ultrasonic(config, this) {} + +void UltrasonicSensor::DistanceSensor::Update(bool recursive) { + RoboidControl::DistanceSensor::Update(recursive); + this->ultrasonic.Update(false); + if (this->ultrasonic.distance > 0) + this->internalDistance = this->ultrasonic.distance; + else +#if ARDUNIO + this->internalDistance = INFINITY; +#else + this->internalDistance = std::numeric_limits::infinity(); +#endif +} + +#pragma endregion Distance sensor + +#pragma region Touch sensor + +UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent) + : RoboidControl::TouchSensor(parent), ultrasonic(config, this) {} + +void UltrasonicSensor::TouchSensor::Update(bool recursive) { + RoboidControl::TouchSensor::Update(recursive); + this->ultrasonic.Update(false); + this->internalTouch = (this->ultrasonic.distance > 0 && + this->ultrasonic.distance <= this->touchDistance); +} + +#pragma endregion Touch sensor } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index a80be2b..c991e23 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -1,25 +1,25 @@ #pragma once +#include "Things/DistanceSensor.h" #include "Things/TouchSensor.h" namespace RoboidControl { namespace Arduino { /// @brief An HC-SR04 ultrasonic distance sensor -class UltrasonicSensor : public TouchSensor { +class UltrasonicSensor : Thing { public: - /// @brief Setup an ultrasonic sensor - /// @param participant The participant to use - /// @param pinTrigger The pin number of the trigger signal - /// @param pinEcho The pin number of the echo signal - UltrasonicSensor(Participant* participant, - unsigned char pinTrigger, - unsigned char pinEcho); + struct Configuration { + int trigger; + int echo; + }; + + UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot()); // parameters /// @brief The distance at which the object is considered to be touched - float touchDistance = 0.2f; + // float touchDistance = 0.2f; // state @@ -31,15 +31,52 @@ class UltrasonicSensor : public TouchSensor { float GetDistance(); /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) - virtual void Update(unsigned long currentTimeMs, - bool recursive = false) override; + virtual void Update(bool recursive = false) override; protected: /// @brief The pin number of the trigger signal unsigned char pinTrigger = 0; /// @brief The pin number of the echo signal unsigned char pinEcho = 0; + + public: + class DistanceSensor; + class TouchSensor; }; +#pragma region Distance sensor + +class UltrasonicSensor::DistanceSensor : public RoboidControl::DistanceSensor { + public: + DistanceSensor(UltrasonicSensor::Configuration config, + Thing* parent = Thing::LocalRoot()); + + /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) + virtual void Update(bool recursive = false) override; + + protected: + UltrasonicSensor ultrasonic; +}; + +#pragma endregion Distance sensor + +#pragma region Touch sensor + +class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { + public: + TouchSensor(UltrasonicSensor::Configuration config, + Thing* parent = Thing::LocalRoot()); + + float touchDistance = 0.2f; + + /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) + virtual void Update(bool recursive = false) override; + + protected: + UltrasonicSensor ultrasonic; +}; + +#pragma endregion Touch sensor + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 09c1b91..75b9504 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,46 +19,27 @@ if(ESP_PLATFORM) REQUIRES esp_netif esp_wifi ) else() - project(RoboidControl) - add_subdirectory(LinearAlgebra) - add_subdirectory(Examples) - set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard set(CMAKE_POSITION_INDEPENDENT_CODE ON) - add_compile_definitions(GTEST) - include(FetchContent) - FetchContent_Declare( - googletest - DOWNLOAD_EXTRACT_TIMESTAMP ON - URL https://github.com/google/googletest/archive/refs/heads/main.zip - ) - - # For Windows: Prevent overriding the parent project's compiler/linker settings - set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) - FetchContent_MakeAvailable(googletest) - include_directories( . LinearAlgebra ) add_library(RoboidControl STATIC ${srcs}) - + target_include_directories(RoboidControl PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} + ) + + project(RoboidControl) + add_subdirectory(LinearAlgebra) + + # Examples + option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF) + add_subdirectory(examples) + enable_testing() + add_subdirectory(test) - file(GLOB_RECURSE test_srcs test/*_test.cc) - add_executable( - RoboidControlTest - ${test_srcs} - ) - target_link_libraries( - RoboidControlTest - gtest_main - RoboidControl - LinearAlgebra - ) - - include(GoogleTest) - gtest_discover_tests(RoboidControlTest) endif() diff --git a/DoxyGen/Doxyfile b/DoxyGen/Doxyfile index 853c02a..d551ef0 100644 --- a/DoxyGen/Doxyfile +++ b/DoxyGen/Doxyfile @@ -48,7 +48,7 @@ PROJECT_NAME = "Roboid Control for C++" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = +PROJECT_NUMBER = 0.4 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/Examples/CMakeLists.txt b/Examples/CMakeLists.txt deleted file mode 100644 index 00282a2..0000000 --- a/Examples/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -# examples/CMakeLists.txt - -# Specify the minimum CMake version -cmake_minimum_required(VERSION 3.10) - -# Specify the path to the main project directory -set(MAIN_PROJECT_DIR "${CMAKE_SOURCE_DIR}/..") - -# Set the project name -project(Examples) - -include_directories(..) - -# Add the executable for the main project -#add_executable(MainExecutable ${SOURCES}) -# Find the main project library (assuming it's defined in the root CMakeLists.txt) -#find_package(RoboidControl REQUIRED) # Replace MyLibrary with your actual library name - -# Add example executables -add_executable(BB2B BB2B.cpp) -target_link_libraries( - BB2B - RoboidControl - LinearAlgebra -) diff --git a/LinearAlgebra/Direction.cpp b/LinearAlgebra/Direction.cpp index f9c5e40..86fa77c 100644 --- a/LinearAlgebra/Direction.cpp +++ b/LinearAlgebra/Direction.cpp @@ -9,6 +9,7 @@ #include +namespace LinearAlgebra { template DirectionOf::DirectionOf() { this->horizontal = AngleOf(); @@ -98,5 +99,6 @@ void DirectionOf::Normalize() { } } -template class DirectionOf; -template class DirectionOf; +template class LinearAlgebra::DirectionOf; +template class LinearAlgebra::DirectionOf; +} \ No newline at end of file diff --git a/LinearAlgebra/Direction.h b/LinearAlgebra/Direction.h index 57f6647..8c58b6c 100644 --- a/LinearAlgebra/Direction.h +++ b/LinearAlgebra/Direction.h @@ -99,6 +99,4 @@ using Direction = DirectionSingle; } // namespace LinearAlgebra -using namespace LinearAlgebra; - #endif \ No newline at end of file diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index b725f55..c7e53c7 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,5 +1,7 @@ #include "Matrix.h" +#if !defined(NO_STD) #include +#endif namespace LinearAlgebra { @@ -61,7 +63,9 @@ Matrix2::Matrix2(const Matrix2& m) this->data = nullptr; else { this->data = new float[this->nValues]; - std::copy(m.data, m.data + nValues, this->data); + + for (int ix = 0; ix < this->nValues; ++ix) + this->data[ix] = m.data[ix]; } } @@ -76,7 +80,8 @@ Matrix2& Matrix2::operator=(const Matrix2& m) { this->data = nullptr; else { this->data = new float[this->nValues]; - std::copy(m.data, m.data + this->nValues, this->data); + for (int ix = 0; ix < this->nValues; ++ix) + this->data[ix] = m.data[ix]; } } return *this; @@ -89,7 +94,8 @@ Matrix2::~Matrix2() { Matrix2 Matrix2::Clone() const { Matrix2 r = Matrix2(this->nRows, this->nCols); - std::copy(this->data, this->data + this->nValues, r.data); + for (int ix = 0; ix < this->nValues; ++ix) + r.data[ix] = this->data[ix]; return r; } @@ -133,7 +139,7 @@ Matrix2 Matrix2::Identity(int size) { } Matrix2 Matrix2::Diagonal(float f, int size) { - Matrix2 r = Matrix2(size, size); + Matrix2 r = Matrix2::Zero(size, size); float* data = r.data; int valueIx = 0; for (int ix = 0; ix < size; ix++) { @@ -158,8 +164,8 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) { Matrix2 Matrix2::Transpose() const { Matrix2 r = Matrix2(this->nCols, this->nRows); - for (uint rowIx = 0; rowIx < this->nRows; rowIx++) { - for (uint colIx = 0; colIx < this->nCols; colIx++) + for (int rowIx = 0; rowIx < this->nRows; rowIx++) { + for (int colIx = 0; colIx < this->nCols; colIx++) r.data[colIx * this->nCols + rowIx] = this->data[rowIx * this->nCols + colIx]; } @@ -200,16 +206,16 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { int BColOffset = i * BCols; // BColOffset is constant for each row of B for (int j = 0; j < BCols; ++j) { float sum = 0; - // std::cout << " 0"; + std::cout << " 0"; int BIndex = j; for (int k = 0; k < ACols; ++k) { - // std::cout << " + " << this->data[ARowOffset + k] << " * " - // << B.data[BIndex]; + std::cout << " + " << this->data[ARowOffset + k] << " * " + << B.data[BIndex]; sum += this->data[ARowOffset + k] * B.data[BIndex]; BIndex += BCols; } r.data[BColOffset + j] = sum; - // std::cout << " = " << sum << " ix: " << BColOffset + j << "\n"; + std::cout << " = " << sum << " ix: " << BColOffset + j << "\n"; } } return r; diff --git a/LinearAlgebra/Polar.cpp b/LinearAlgebra/Polar.cpp index 9dea130..15ac40f 100644 --- a/LinearAlgebra/Polar.cpp +++ b/LinearAlgebra/Polar.cpp @@ -175,5 +175,5 @@ PolarOf PolarOf::Rotate(const PolarOf& v, AngleOf angle) { return r; } -template class PolarOf; -template class PolarOf; \ No newline at end of file +template class LinearAlgebra::PolarOf; +template class LinearAlgebra::PolarOf; \ No newline at end of file diff --git a/LinearAlgebra/Spherical.cpp b/LinearAlgebra/Spherical.cpp index 1a50357..0371f92 100644 --- a/LinearAlgebra/Spherical.cpp +++ b/LinearAlgebra/Spherical.cpp @@ -5,6 +5,8 @@ #include +namespace LinearAlgebra { + template SphericalOf::SphericalOf() { this->distance = 0.0f; @@ -301,3 +303,5 @@ SphericalOf SphericalOf::RotateVertical(const SphericalOf& v, template class SphericalOf; template class SphericalOf; + +} // namespace LinearAlgebra \ No newline at end of file diff --git a/LinearAlgebra/Spherical.h b/LinearAlgebra/Spherical.h index 5cad52c..309db03 100644 --- a/LinearAlgebra/Spherical.h +++ b/LinearAlgebra/Spherical.h @@ -186,7 +186,6 @@ using Spherical = SphericalSingle; #endif } // namespace LinearAlgebra -using namespace LinearAlgebra; #include "Polar.h" #include "Vector3.h" diff --git a/LinearAlgebra/SwingTwist.cpp b/LinearAlgebra/SwingTwist.cpp index 58905c7..deca73c 100644 --- a/LinearAlgebra/SwingTwist.cpp +++ b/LinearAlgebra/SwingTwist.cpp @@ -4,6 +4,8 @@ #include "SwingTwist.h" +namespace LinearAlgebra { + template SwingTwistOf::SwingTwistOf() { this->swing = DirectionOf(AngleOf(), AngleOf()); @@ -165,4 +167,6 @@ void SwingTwistOf::Normalize() { } template class SwingTwistOf; -template class SwingTwistOf; \ No newline at end of file +template class SwingTwistOf; + +} \ No newline at end of file diff --git a/LinearAlgebra/test/Direction_test.cc b/LinearAlgebra/test/Direction_test.cc index 461140c..6489caa 100644 --- a/LinearAlgebra/test/Direction_test.cc +++ b/LinearAlgebra/test/Direction_test.cc @@ -6,6 +6,8 @@ #include "Direction.h" +using namespace LinearAlgebra; + #define FLOAT_INFINITY std::numeric_limits::infinity() TEST(Direction16, Compare) { diff --git a/LinearAlgebra/test/Polar_test.cc b/LinearAlgebra/test/Polar_test.cc index 9e244f2..ba65946 100644 --- a/LinearAlgebra/test/Polar_test.cc +++ b/LinearAlgebra/test/Polar_test.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include "Polar.h" #include "Spherical.h" diff --git a/LinearAlgebra/test/Spherical16_test.cc b/LinearAlgebra/test/Spherical16_test.cc index e9fbc40..8eea700 100644 --- a/LinearAlgebra/test/Spherical16_test.cc +++ b/LinearAlgebra/test/Spherical16_test.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include "Spherical.h" #include "Vector3.h" diff --git a/LinearAlgebra/test/SphericalSingle_test.cc b/LinearAlgebra/test/SphericalSingle_test.cc index 7840d77..3dedb31 100644 --- a/LinearAlgebra/test/SphericalSingle_test.cc +++ b/LinearAlgebra/test/SphericalSingle_test.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include "Spherical.h" diff --git a/Messages/BinaryMsg.cpp b/Messages/BinaryMsg.cpp index 0b0b940..c831753 100644 --- a/Messages/BinaryMsg.cpp +++ b/Messages/BinaryMsg.cpp @@ -6,7 +6,8 @@ BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; this->thingId = thing->id; this->thing = thing; - unsigned char ix = BinaryMsg::length; + unsigned char ix = 0; //BinaryMsg::length; + this->data = new char[255]; this->dataLength = this->thing->GenerateBinary(this->data, &ix); } @@ -40,6 +41,8 @@ unsigned char BinaryMsg::Serialize(char* buffer) { buffer[ix++] = this->networkId; buffer[ix++] = this->thingId; buffer[ix++] = this->dataLength; + for (int dataIx = 0; dataIx < this->dataLength; dataIx++) + buffer[ix++] = this->data[dataIx]; return this->length + this->dataLength; } diff --git a/Messages/BinaryMsg.h b/Messages/BinaryMsg.h index 10adb90..81838c0 100644 --- a/Messages/BinaryMsg.h +++ b/Messages/BinaryMsg.h @@ -1,15 +1,17 @@ #pragma once -#include "Messages.h" +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { -/// @brief Message to send thing-specific data +/// @brief A message containing binary data for custom communication class BinaryMsg : public IMessage { public: /// @brief The message ID static const unsigned char id = 0xB1; - /// @brief The length of the message without the binary data itslef + /// @brief The length of the message in bytes, excluding the binary data + /// For the total size of the message this.bytes.Length should be added to this value. static const unsigned length = 4; /// @brief The network ID of the thing @@ -23,7 +25,7 @@ class BinaryMsg : public IMessage { /// @brief The binary data which is communicated char* data = nullptr; - /// @brief Create a new message for sending + /// @brief Create a BinaryMsg /// @param networkId The network ID of the thing /// @param thing The thing for which binary data is sent BinaryMsg(unsigned char networkId, Thing* thing); diff --git a/Messages/DestroyMsg.cpp b/Messages/DestroyMsg.cpp index 6141582..948b1fe 100644 --- a/Messages/DestroyMsg.cpp +++ b/Messages/DestroyMsg.cpp @@ -2,19 +2,22 @@ namespace RoboidControl { -DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) { +DestroyMsg::DestroyMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; this->thingId = thing->id; } -DestroyMsg::DestroyMsg(char* buffer) {} +DestroyMsg::DestroyMsg(char* buffer) { + this->networkId = buffer[1]; + this->thingId = buffer[2]; +} DestroyMsg::~DestroyMsg() {} -unsigned char DestroyMsg::Serialize(char *buffer) { - #if defined(DEBUG) - std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" << (int)this->thingId - << "] " << std::endl; +unsigned char DestroyMsg::Serialize(char* buffer) { +#if defined(DEBUG) + std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" + << (int)this->thingId << "] " << std::endl; #endif unsigned char ix = 0; buffer[ix++] = this->id; @@ -23,4 +26,4 @@ unsigned char DestroyMsg::Serialize(char *buffer) { return ix; } -} // namespace RoboidControl +} // namespace RoboidControl diff --git a/Messages/DestroyMsg.h b/Messages/DestroyMsg.h index 74ad2aa..19e1901 100644 --- a/Messages/DestroyMsg.h +++ b/Messages/DestroyMsg.h @@ -1,13 +1,16 @@ -#include "Messages.h" +#pragma once + +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { -/// @brief Message notifiying that a Thing no longer exists +/// @brief A Message notifiying that a Thing no longer exists class DestroyMsg : public IMessage { public: /// @brief The message ID static const unsigned char id = 0x20; - /// @brief The length of the message + /// @brief The length of the message in bytes static const unsigned length = 3; /// @brief The network ID of the thing unsigned char networkId; diff --git a/Messages/Messages.cpp b/Messages/IMessage.cpp similarity index 73% rename from Messages/Messages.cpp rename to Messages/IMessage.cpp index d9f1a8c..86ccef7 100644 --- a/Messages/Messages.cpp +++ b/Messages/IMessage.cpp @@ -1,7 +1,4 @@ -#include "Messages.h" - -#include "LowLevelMessages.h" -#include "string.h" +#include "IMessage.h" namespace RoboidControl { diff --git a/Messages/IMessage.h b/Messages/IMessage.h new file mode 100644 index 0000000..632287e --- /dev/null +++ b/Messages/IMessage.h @@ -0,0 +1,16 @@ +#pragma once + +namespace RoboidControl { + +/// @brief Root structure for all communcation messages +class IMessage { + public: + IMessage(); + /// @brief Serialize the message into a byte array for sending + /// @param buffer The buffer to serilize into + /// @return The length of the message in the buffer + virtual unsigned char Serialize(char* buffer); + +}; + +} // namespace RoboidControl diff --git a/Messages/InvestigateMsg.cpp b/Messages/InvestigateMsg.cpp index 6420e42..7b94c8d 100644 --- a/Messages/InvestigateMsg.cpp +++ b/Messages/InvestigateMsg.cpp @@ -7,9 +7,9 @@ InvestigateMsg::InvestigateMsg(char* buffer) { this->networkId = buffer[ix++]; this->thingId = buffer[ix++]; } -InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) { +InvestigateMsg::InvestigateMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; - this->thingId = thingId; + this->thingId = thing->id; } InvestigateMsg::~InvestigateMsg() {} diff --git a/Messages/InvestigateMsg.h b/Messages/InvestigateMsg.h index 3179ec4..cff3c32 100644 --- a/Messages/InvestigateMsg.h +++ b/Messages/InvestigateMsg.h @@ -1,4 +1,7 @@ -#include "Messages.h" +#pragma once + +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { @@ -14,10 +17,10 @@ class InvestigateMsg : public IMessage { /// @brief the ID of the thing unsigned char thingId; - /// @brief Create a new message for sending + /// @brief Create an investigate message /// @param networkId The network ID for the thing - /// @param thingId The ID of the thing - InvestigateMsg(unsigned char networkId, unsigned char thingId); + /// @param thing The thing for which the details are requested + InvestigateMsg(unsigned char networkId, Thing* thing); /// @copydoc RoboidControl::IMessage::IMessage(char*) InvestigateMsg(char* buffer); /// @brief Destructor for the message diff --git a/Messages/LowLevelMessages.cpp b/Messages/LowLevelMessages.cpp index a186511..4526a04 100644 --- a/Messages/LowLevelMessages.cpp +++ b/Messages/LowLevelMessages.cpp @@ -5,6 +5,83 @@ namespace RoboidControl { +void LowLevelMessages::SendSpherical(char* buffer, + unsigned char* ix, + Spherical s) { + SendFloat16(buffer, ix, s.distance); + SendAngle8(buffer, ix, s.direction.horizontal.InDegrees()); + SendAngle8(buffer, ix, s.direction.vertical.InDegrees()); +} +Spherical LowLevelMessages::ReceiveSpherical(const char* buffer, + unsigned char* startIndex) { + float distance = ReceiveFloat16(buffer, startIndex); + + Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex); + Angle horizontal = Angle::Radians(horizontal8.InRadians()); + + Angle8 vertical8 = ReceiveAngle8(buffer, startIndex); + Angle vertical = Angle::Radians(vertical8.InRadians()); + + Spherical s = Spherical(distance, horizontal, vertical); + return s; +} + +void LowLevelMessages::SendSwingTwist(char* buffer, + unsigned char* ix, + SwingTwist s) { + SendAngle8(buffer, ix, s.swing.horizontal.InDegrees()); + SendAngle8(buffer, ix, s.swing.vertical.InDegrees()); + SendAngle8(buffer, ix, s.twist.InDegrees()); +} + +SwingTwist LowLevelMessages::ReceiveSwingTwist(const char* buffer, + unsigned char* startIndex) { + Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex); + Angle horizontal = Angle::Radians(horizontal8.InRadians()); + + Angle8 vertical8 = ReceiveAngle8(buffer, startIndex); + Angle vertical = Angle::Radians(vertical8.InRadians()); + + Angle8 twist8 = ReceiveAngle8(buffer, startIndex); + Angle twist = Angle::Radians(twist8.InRadians()); + + SwingTwist s = SwingTwist(horizontal, vertical, twist); + return s; +} + +void LowLevelMessages::SendQuat32(char* buffer, + unsigned char* ix, + SwingTwist rotation) { + Quaternion q = rotation.ToQuaternion(); + 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; + } + // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw + // << "\n"; + buffer[(*ix)++] = qx; + buffer[(*ix)++] = qy; + buffer[(*ix)++] = qz; + buffer[(*ix)++] = qw; +} + +SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer, + unsigned char* ix) { + float qx = (buffer[(*ix)++] - 128.0F) / 127.0F; + float qy = (buffer[(*ix)++] - 128.0F) / 127.0F; + float qz = (buffer[(*ix)++] - 128.0F) / 127.0F; + float qw = buffer[(*ix)++] / 255.0F; + Quaternion q = Quaternion(qx, qy, qz, qw); + SwingTwist s = SwingTwist::FromQuaternion(q); + return s; +} + void LowLevelMessages::SendAngle8(char* buffer, unsigned char* ix, const float angle) { @@ -42,58 +119,5 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer, return (float)f.toFloat(); } -void LowLevelMessages::SendSpherical(char* buffer, - unsigned char* ix, - Spherical s) { - SendFloat16(buffer, ix, s.distance); - SendAngle8(buffer, ix, s.direction.horizontal.InDegrees()); - SendAngle8(buffer, ix, s.direction.vertical.InDegrees()); -} -Spherical LowLevelMessages::ReceiveSpherical(const char* buffer, - unsigned char* startIndex) { - float distance = ReceiveFloat16(buffer, startIndex); - - Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex); - Angle horizontal = Angle::Radians(horizontal8.InRadians()); - - Angle8 vertical8 = ReceiveAngle8(buffer, startIndex); - Angle vertical = Angle::Radians(vertical8.InRadians()); - - Spherical s = Spherical(distance, horizontal, vertical); - return s; -} - -void LowLevelMessages::SendQuat32(char* buffer, - unsigned char* ix, - SwingTwist rotation) { - Quaternion q = rotation.ToQuaternion(); - 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; - } - // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw - // << "\n"; - buffer[(*ix)++] = qx; - buffer[(*ix)++] = qy; - buffer[(*ix)++] = qz; - buffer[(*ix)++] = qw; -} - -SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer, - unsigned char* ix) { - float qx = (buffer[(*ix)++] - 128.0F) / 127.0F; - float qy = (buffer[(*ix)++] - 128.0F) / 127.0F; - float qz = (buffer[(*ix)++] - 128.0F) / 127.0F; - float qw = buffer[(*ix)++] / 255.0F; - Quaternion q = Quaternion(qx, qy, qz, qw); - SwingTwist s = SwingTwist::FromQuaternion(q); - return s; -} } // namespace RoboidControl \ No newline at end of file diff --git a/Messages/LowLevelMessages.h b/Messages/LowLevelMessages.h index bffe010..c21b32d 100644 --- a/Messages/LowLevelMessages.h +++ b/Messages/LowLevelMessages.h @@ -1,3 +1,5 @@ +#pragma once + #include "LinearAlgebra/Spherical.h" #include "LinearAlgebra/SwingTwist.h" @@ -5,18 +7,22 @@ namespace RoboidControl { class LowLevelMessages { public: + static void SendSpherical(char* buffer, unsigned char* ix, Spherical s); + static Spherical ReceiveSpherical(const char* buffer, + unsigned char* startIndex); + + static void SendSwingTwist(char* buffer, unsigned char* ix, SwingTwist s); + static SwingTwist ReceiveSwingTwist(const char* buffer, + unsigned char* startIndex); + + static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q); + static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix); + static void SendAngle8(char* buffer, unsigned char* ix, const float angle); static Angle8 ReceiveAngle8(const char* buffer, unsigned char* startIndex); static void SendFloat16(char* buffer, unsigned char* ix, float value); static float ReceiveFloat16(const char* buffer, unsigned char* startIndex); - - static void SendSpherical(char* buffer, unsigned char* ix, Spherical s); - static Spherical ReceiveSpherical(const char* buffer, - unsigned char* startIndex); - - static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q); - static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix); }; } // namespace RoboidControl diff --git a/Messages/Messages.h b/Messages/Messages.h deleted file mode 100644 index df22544..0000000 --- a/Messages/Messages.h +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once - -#include "LinearAlgebra/Spherical.h" -#include "LinearAlgebra/SwingTwist.h" -#include "Thing.h" - -namespace RoboidControl { - -class ParticipantUDP; - -class IMessage { - public: - IMessage(); - virtual unsigned char Serialize(char* buffer); - - static unsigned char* ReceiveMsg(unsigned char packetSize); - - // bool Publish(ParticipantUDP *participant); - // bool SendTo(ParticipantUDP *participant); -}; - -} // namespace RoboidControl diff --git a/Messages/ModelUrlMsg.h b/Messages/ModelUrlMsg.h index 7c493eb..9759aa0 100644 --- a/Messages/ModelUrlMsg.h +++ b/Messages/ModelUrlMsg.h @@ -1,4 +1,7 @@ -#include "Messages.h" +#pragma once + +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { @@ -26,8 +29,6 @@ class ModelUrlMsg : public IMessage { ModelUrlMsg(unsigned char networkId, Thing* thing); /// @copydoc RoboidControl::IMessage::IMessage(char*) ModelUrlMsg(const char* buffer); - // ModelUrlMsg(unsigned char networkId, unsigned char thingId, - // unsigned char urlLegth, const char *url, float scale = 1); /// @brief Destructor for the message virtual ~ModelUrlMsg(); diff --git a/Messages/NameMsg.cpp b/Messages/NameMsg.cpp index da91c79..84aba94 100644 --- a/Messages/NameMsg.cpp +++ b/Messages/NameMsg.cpp @@ -7,15 +7,16 @@ namespace RoboidControl { NameMsg::NameMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; this->thingId = thing->id; - if (thing->name == nullptr) + const char* thingName = thing->GetName(); + if (thingName == nullptr) this->nameLength = 0; else - this->nameLength = (unsigned char)strlen(thing->name); + this->nameLength = (unsigned char)strlen(thingName); // the name string in the buffer is not \0 terminated! char* name = new char[this->nameLength + 1]; for (int i = 0; i < this->nameLength; i++) - name[i] = thing->name[i]; + name[i] = thingName[i]; name[this->nameLength] = '\0'; this->name = name; } diff --git a/Messages/NameMsg.h b/Messages/NameMsg.h index 0d1bebc..c283a59 100644 --- a/Messages/NameMsg.h +++ b/Messages/NameMsg.h @@ -1,4 +1,7 @@ -#include "Messages.h" +#pragma once + +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { @@ -22,9 +25,6 @@ class NameMsg : public IMessage { /// @param networkId The network ID of the thing /// @param thing The ID of the thing NameMsg(unsigned char networkId, Thing* thing); - // NameMsg(unsigned char networkId, unsigned char thingId, const char *name, - // unsigned char nameLength); - /// @copydoc RoboidControl::IMessage::IMessage(char*) NameMsg(const char* buffer); /// @brief Destructor for the message diff --git a/Messages/NetworkIdMsg.cpp b/Messages/NetworkIdMsg.cpp new file mode 100644 index 0000000..df9c9cf --- /dev/null +++ b/Messages/NetworkIdMsg.cpp @@ -0,0 +1,27 @@ +#include "NetworkIdMsg.h" + +#include + +namespace RoboidControl { + +NetworkIdMsg::NetworkIdMsg(const char* buffer) { + this->networkId = buffer[1]; +} + +NetworkIdMsg::NetworkIdMsg(unsigned char networkId) { + this->networkId = networkId; +} + +NetworkIdMsg::~NetworkIdMsg() {} + +unsigned char NetworkIdMsg::Serialize(char* buffer) { +#if defined(DEBUG) + std::cout << "Send NetworkIdMsg [" << (int)this->networkId << "] " << std::endl; +#endif + unsigned char ix = 0; + buffer[ix++] = this->id; + buffer[ix++] = this->networkId; + return NetworkIdMsg::length; +} + +} // namespace RoboidControl diff --git a/Messages/SiteMsg.h b/Messages/NetworkIdMsg.h similarity index 78% rename from Messages/SiteMsg.h rename to Messages/NetworkIdMsg.h index b6e433f..de78906 100644 --- a/Messages/SiteMsg.h +++ b/Messages/NetworkIdMsg.h @@ -1,9 +1,11 @@ -#include "Messages.h" +#pragma once + +#include "IMessage.h" namespace RoboidControl { /// @brief A message communicating the network ID for that participant -class SiteMsg : public IMessage { +class NetworkIdMsg : public IMessage { public: /// @brief The message ID static const unsigned char id = 0xA1; @@ -14,11 +16,11 @@ public: /// @brief Create a new message for sending /// @param networkId The network ID for the participant - SiteMsg(unsigned char networkId); + NetworkIdMsg(unsigned char networkId); /// @copydoc RoboidControl::IMessage::IMessage(char*) - SiteMsg(const char *buffer); + NetworkIdMsg(const char *buffer); /// @brief Destructor for the message - virtual ~SiteMsg(); + virtual ~NetworkIdMsg(); /// @copydoc RoboidControl::IMessage::Serialize virtual unsigned char Serialize(char *buffer) override; diff --git a/Messages/ParticipantMsg.cpp b/Messages/ParticipantMsg.cpp index c91fce9..7fb1439 100644 --- a/Messages/ParticipantMsg.cpp +++ b/Messages/ParticipantMsg.cpp @@ -1,5 +1,9 @@ #include "ParticipantMsg.h" +#if !defined(NO_STD) +#include +#endif + namespace RoboidControl { ParticipantMsg::ParticipantMsg(char networkId) { @@ -13,7 +17,7 @@ ParticipantMsg::ParticipantMsg(const char* buffer) { ParticipantMsg::~ParticipantMsg() {} unsigned char ParticipantMsg::Serialize(char* buffer) { -#if defined(DEBUG) +#if defined(DEBUG) && !defined(NO_STD) std::cout << "Send ParticipantMsg [" << (int)this->networkId << "] " << std::endl; #endif diff --git a/Messages/ParticipantMsg.h b/Messages/ParticipantMsg.h index 28f9eb5..105881d 100644 --- a/Messages/ParticipantMsg.h +++ b/Messages/ParticipantMsg.h @@ -1,6 +1,6 @@ #pragma once -#include "Messages.h" +#include "IMessage.h" namespace RoboidControl { diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index c6d41aa..d72e453 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) { this->thingId = thing->id; this->poseType = 0; - if (thing->positionUpdated || force) { + if (thing->positionUpdated || (force || thing->IsRoot())) { this->position = thing->GetPosition(); this->poseType |= Pose_Position; } - if (thing->orientationUpdated || force) { + if (thing->orientationUpdated || (force || thing->IsRoot())) { this->orientation = thing->GetOrientation(); this->poseType |= Pose_Orientation; } @@ -34,7 +34,7 @@ PoseMsg::PoseMsg(const char* buffer) { this->thingId = buffer[ix++]; this->poseType = buffer[ix++]; this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix); - this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix); + this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix); // linearVelocity // angularVelocity } @@ -45,7 +45,7 @@ unsigned char PoseMsg::Serialize(char* buffer) { if (this->poseType == 0) return 0; -#if defined(DEBUG) +#if defined(DEBUG) && DEBUG > 1 std::cout << "Send PoseMsg [" << (int)this->networkId << "/" << (int)this->thingId << "] " << (int)this->poseType << std::endl; #endif @@ -57,7 +57,7 @@ unsigned char PoseMsg::Serialize(char* buffer) { if ((this->poseType & Pose_Position) != 0) LowLevelMessages::SendSpherical(buffer, &ix, this->position); if ((this->poseType & Pose_Orientation) != 0) - LowLevelMessages::SendQuat32(buffer, &ix, this->orientation); + LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation); if ((this->poseType & Pose_LinearVelocity) != 0) LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity); if ((this->poseType & Pose_AngularVelocity) != 0) diff --git a/Messages/PoseMsg.h b/Messages/PoseMsg.h index bc0c336..fe702da 100644 --- a/Messages/PoseMsg.h +++ b/Messages/PoseMsg.h @@ -1,4 +1,6 @@ -#include "Messages.h" +#pragma once +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { @@ -9,7 +11,7 @@ class PoseMsg : public IMessage { public: /// @brief The message ID static const unsigned char id = 0x10; - /// @brief The length of the message + /// @brief The length of the message in bytes unsigned char length = 4 + 4 + 4; /// @brief The network ID of the thing @@ -40,7 +42,8 @@ class PoseMsg : public IMessage { /// @brief Create a new message for sending /// @param networkId he network ID of the thing - /// @param thing The thing for which the pose shouldbe sent + /// @param thing The thing for which the pose should be sent + /// @param force If true, position and orientation are always included, even when they are not updated PoseMsg(unsigned char networkId, Thing* thing, bool force = false); /// @copydoc RoboidControl::IMessage::IMessage(char*) diff --git a/Messages/SiteMsg.cpp b/Messages/SiteMsg.cpp deleted file mode 100644 index 598a841..0000000 --- a/Messages/SiteMsg.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "SiteMsg.h" - -namespace RoboidControl { - -SiteMsg::SiteMsg(const char* buffer) { - this->networkId = buffer[1]; -} - -SiteMsg::SiteMsg(unsigned char networkId) { - this->networkId = networkId; -} - -SiteMsg::~SiteMsg() {} - -unsigned char SiteMsg::Serialize(char* buffer) { -#if defined(DEBUG) - std::cout << "Send SiteMsg [" << (int)this->networkId << "] " << std::endl; -#endif - unsigned char ix = 0; - buffer[ix++] = this->id; - buffer[ix++] = this->networkId; - return SiteMsg::length; -} - -} // namespace RoboidControl diff --git a/Messages/TextMsg.cpp b/Messages/TextMsg.cpp index 119a907..942e112 100644 --- a/Messages/TextMsg.cpp +++ b/Messages/TextMsg.cpp @@ -1,5 +1,9 @@ #include "TextMsg.h" +#if !defined(NO_STD) +#include +#endif + namespace RoboidControl { TextMsg::TextMsg(const char* text, unsigned char textLength) { @@ -24,7 +28,7 @@ unsigned char TextMsg::Serialize(char* buffer) { if (this->textLength == 0 || this->text == nullptr) return 0; -#if defined(DEBUG) +#if defined(DEBUG) && !defined(NO_STD) std::cout << "Send TextMsg " << (int)this->textLength << " " << this->text << std::endl; #endif unsigned char ix = 0; diff --git a/Messages/TextMsg.h b/Messages/TextMsg.h index 5b25fca..a5763e6 100644 --- a/Messages/TextMsg.h +++ b/Messages/TextMsg.h @@ -1,4 +1,4 @@ -#include "Messages.h" +#include "IMessage.h" namespace RoboidControl { @@ -9,10 +9,6 @@ class TextMsg : public IMessage { static const unsigned char id = 0xB0; /// @brief The length of the message without the text itself static const unsigned char length = 2; - /// @brief The network ID of the thing - unsigned char networkId; - /// @brief the ID of the thing - unsigned char thingId; /// @brief The text without the null terminator const char* text; /// @brief The length of the text diff --git a/Messages/ThingMsg.cpp b/Messages/ThingMsg.cpp index 932530f..be0c41c 100644 --- a/Messages/ThingMsg.cpp +++ b/Messages/ThingMsg.cpp @@ -14,27 +14,21 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; this->thingId = thing->id; this->thingType = thing->type; - Thing* parent = thing->GetParent(); - if (parent != nullptr) - this->parentId = parent->id; - else + if (thing->IsRoot()) this->parentId = 0; + else { + 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) { #if defined(DEBUG) - std::cout << "Send ThingMsg [" << (int)this->networkId << "/" << (int)this->thingId - << "] " << (int)this->thingType << " " << (int)this->parentId << std::endl; + std::cout << "Send ThingMsg [" << (int)this->networkId << "/" + << (int)this->thingId << "] " << (int)this->thingType << " " + << (int)this->parentId << std::endl; #endif unsigned char ix = 0; buffer[ix++] = this->id; diff --git a/Messages/ThingMsg.h b/Messages/ThingMsg.h index c40fb97..a26b60d 100644 --- a/Messages/ThingMsg.h +++ b/Messages/ThingMsg.h @@ -1,8 +1,9 @@ -#include "Messages.h" +#include "IMessage.h" +#include "Thing.h" namespace RoboidControl { -/// @brief Message providing generic information about a Thing +/// @brief Message providing generic details about a Thing class ThingMsg : public IMessage { public: /// @brief The message ID @@ -13,17 +14,15 @@ class ThingMsg : public IMessage { unsigned char networkId; /// @brief The ID of the thing unsigned char thingId; - /// @brief The Thing.Type of the thing + /// @brief The type of thing unsigned char thingType; - /// @brief The parent of the thing in the hierarachy. This is null for root Things + /// @brief The ID of the parent thing in the hierarchy. This is zero for root things unsigned char parentId; /// @brief Create a message for sending /// @param networkId The network ID of the thing /// @param thing The thing ThingMsg(unsigned char networkId, Thing* thing); - // ThingMsg(unsigned char networkId, unsigned char thingId, - // unsigned char thingType, unsigned char parentId); /// @copydoc RoboidControl::IMessage::IMessage(char*) ThingMsg(const char* buffer); diff --git a/Participant.cpp b/Participant.cpp index 65a1f51..3f1adc6 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -1,12 +1,33 @@ #include "Participant.h" #include +#include "Arduino/ArduinoParticipant.h" +#include "EspIdf/EspIdfParticipant.h" +#include "Posix/PosixParticipant.h" +#include "Windows/WindowsParticipant.h" namespace RoboidControl { -Participant::Participant() {} +#pragma region Participant + +ParticipantRegistry Participant::registry; + +Participant* Participant::LocalParticipant = new Participant(); + +void Participant::ReplaceLocalParticipant(Participant& newParticipant) { + LocalParticipant = &newParticipant; + std::cout << "Replaced local participant" << std::endl; +} + +Participant::Participant() { + Thing::CreateRoot(this); + //this->Add(this->root); +} Participant::Participant(const char* ipAddress, int port) { + Thing::CreateRoot(this); + //this->Add(this->root); + // make a copy of the ip address string int addressLength = (int)strlen(ipAddress); int stringLength = addressLength + 1; @@ -24,23 +45,52 @@ Participant::Participant(const char* ipAddress, int port) { } Participant::~Participant() { + // registry.Remove(this); delete[] this->ipAddress; } -void Participant::Update(unsigned long currentTimeMs) { +void Participant::Update() { for (Thing* thing : this->things) { if (thing != nullptr) - thing->Update(currentTimeMs, true); + thing->Update(true); } } +bool Participant::Send(IMessage* msg) { + int bufferSize = msg->Serialize(this->buffer); + if (bufferSize <= 0) + return true; + + // std::cout << "send msg " << (static_cast(this->buffer[0]) & 0xff) + // << " to " << this->ipAddress << std::endl; + +#if defined(_WIN32) || defined(_WIN64) + Windows::ParticipantUDP* thisWindows = + static_cast(this); + return thisWindows->Send(this, bufferSize); +#elif defined(__unix__) || defined(__APPLE__) + Posix::ParticipantUDP* thisPosix = static_cast(this); + return thisPosix->Send(this, bufferSize); +#elif defined(ARDUINO) + Arduino::ParticipantUDP* thisArduino = + static_cast(this); + return thisArduino->Send(this, bufferSize); +#elif defined(IDF_VER) + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); + return thisEspIdf->Send(remoteParticipant, bufferSize); +#else + return false; +#endif +} + Thing* Participant::Get(unsigned char thingId) { for (Thing* thing : this->things) { if (thing->id == thingId) return thing; } - // std::cout << "Could not find thing " << this->ipAddress << ":" << this->port - // << "[" << (int)thingId << "]\n"; + std::cout << "Could not find thing " << this->ipAddress << ":" << this->port + << "[" << (int)thingId << "]\n"; return nullptr; } @@ -51,7 +101,15 @@ void Participant::Add(Thing* thing, bool checkId) { thing->id = this->thingCount + 1; this->things[this->thingCount++] = thing; #else - thing->id = (unsigned char)this->things.size() + 1; + // find highest id + int highestIx = 0; + for (Thing* thing : this->things) { + if (thing == nullptr) + continue; + if (thing->id > highestIx) + highestIx = thing->id; + } + thing->id = highestIx + 1; this->things.push_back(thing); #endif // std::cout << "Add thing with generated ID " << this->ipAddress << ":" @@ -64,7 +122,8 @@ void Participant::Add(Thing* thing, bool checkId) { #else this->things.push_back(thing); #endif - // std::cout << "Add thing " << this->ipAddress << ":" << this->port << "[" + // std::cout << "Add thing " << this->ipAddress << ":" << this->port << + // "[" // << (int)thing->id << "]\n"; } else { // std::cout << "Did not add, existing thing " << this->ipAddress << ":" @@ -89,22 +148,93 @@ void Participant::Remove(Thing* thing) { this->thingCount = lastThingIx; #else this->things.remove_if([thing](Thing* obj) { return obj == thing; }); - std::cout << "Removing " << thing->networkId << "/" << thing->id - << " list size = " << this->things.size() << "\n"; + // std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id + // << "] list size = " << this->things.size() << "\n"; #endif } -// void Participant::UpdateAll(unsigned long currentTimeMs) { -// // Not very efficient, but it works for now. +#pragma endregion -// for (Thing* thing : this->things) { -// if (thing != nullptr && thing->GetParent() == nullptr) { // update all -// root things -// // std::cout << " update " << (int)ix << " thingid " << (int)thing->id -// // << "\n"; -// thing->Update(currentTimeMs); -// } -// } -// } +#pragma region ParticipantRegistry + +Participant* ParticipantRegistry::Get(const char* ipAddress, + unsigned int port) { +#if !defined(NO_STD) + for (Participant* participant : ParticipantRegistry::participants) { + if (participant == nullptr) + continue; + if (strcmp(participant->ipAddress, ipAddress) == 0 && + participant->port == port) { + // std::cout << "found participant " << participant->ipAddress << ":" + // << (int)participant->port << std::endl; + return participant; + } + } + std::cout << "Could not find participant " << ipAddress << ":" << (int)port + << std::endl; +#endif + return nullptr; +} + +Participant* ParticipantRegistry::Get(unsigned char participantId) { +#if !defined(NO_STD) + for (Participant* participant : ParticipantRegistry::participants) { + if (participant == nullptr) + continue; + if (participant->networkId == participantId) + return participant; + } + std::cout << "Could not find participant " << (int)participantId << std::endl; +#endif + return nullptr; +} + +Participant* ParticipantRegistry::Add(const char* ipAddress, + unsigned int port) { + Participant* participant = new Participant(ipAddress, port); + Add(participant); + return participant; +} + +void ParticipantRegistry::Add(Participant* participant) { + Participant* foundParticipant = + Get(participant->ipAddress, participant->port); + + if (foundParticipant == nullptr) { +#if defined(NO_STD) + // 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() + // << "\n"; + // } else { + // std::cout << "Did not add, existing participant " << + // participant->ipAddress + // << ":" << participant->port << "[" << + // (int)participant->networkId + // << "]\n"; + } +} + +void ParticipantRegistry::Remove(Participant* participant) { + // participants.remove(participant); +} + +#if defined(NO_STD) +Participant** ParticipantRegistry::GetAll() const { + return ParticipantRegistry::participants; +} +#else +const std::list& ParticipantRegistry::GetAll() const { + return ParticipantRegistry::participants; +} +#endif + +#pragma endregion ParticipantRegistry } // namespace RoboidControl diff --git a/Participant.h b/Participant.h index 5fdffc4..6e1217d 100644 --- a/Participant.h +++ b/Participant.h @@ -1,10 +1,58 @@ #pragma once + +#include "Messages/IMessage.h" #include "Thing.h" namespace RoboidControl { constexpr int MAX_THING_COUNT = 256; +/// @brief class which manages all known participants +class ParticipantRegistry { + public: + /// @brief Retrieve a participant by its address + /// @param ipAddress The IP address of the participant + /// @param port The port number of the participant + /// @return The participant or a nullptr when it could not be found + Participant* Get(const char* ipAddress, unsigned int port); + /// @brief Retrieve a participant by its network ID + /// @param networkID The network ID of the participant + /// @return The participant or a nullptr when it could not be found + Participant* Get(unsigned char networkID); + + /// @brief Add a participant with the given details + /// @param ipAddress The IP address of the participant + /// @param port The port number of the participant + /// @return The added participant + Participant* Add(const char* ipAddress, unsigned int port); + /// @brief Add a participant + /// @param participant The participant to add + void Add(Participant* participant); + + /// @brief Remove a participant + /// @param participant The participant to remove + void Remove(Participant* participant); + + private: +#if defined(NO_STD) + public: + Participant** GetAll() const; + int count = 0; + + private: + Participant** participants; +#else + public: + /// @brief Get all participants + /// @return All participants + const std::list& GetAll() const; + + private: + /// @brief The list of known participants + std::list participants; +#endif +}; + /// @brief A participant is a device which manages things. /// It can communicate with other participant to synchronise the state of /// things. This class is used to register the things the participant is @@ -12,53 +60,94 @@ constexpr int MAX_THING_COUNT = 256; /// participant. It is used as a basis for the local participant, but also as a /// reference to remote participants. class Participant { +#pragma region Init + public: - /// @brief The Ip Address of a participant. When the participant is local, - /// this contains 0.0.0.0 - const char* ipAddress = "0.0.0.0"; - /// @brief The port number for UDP communication with the participant. This is - /// 0 for isolated participants. - int port = 0; - - /// @brief The network Id to identify the participant. - /// @note This field is likely to disappear in future versions - unsigned char networkId = 0; - - /// @brief Default constructor + /// @brief Create a generic participant Participant(); /// @brief Create a new participant with the given communcation info /// @param ipAddress The IP address of the participant - /// @param port The port of the participant + /// @param port The UDP port of the participant + /// @remarks This does not belong here, it should move to ParticipantUDP or + /// something like that in the future Participant(const char* ipAddress, int port); /// @brief Destructor for the participant ~Participant(); - virtual void Update(unsigned long currentTimeMs = 0); + /// @brief The local participant for this application + static Participant* LocalParticipant; + /// @brief Replace the local participant + /// @param newParticipant The new local Participant + static void ReplaceLocalParticipant(Participant& newParticipant); - protected: +#pragma endregion Init + +#pragma region Properties + + public: + /// @brief The name of the participant + const char* name = "Participant"; + + /// @brief The Ip Address of a participant. + /// @remarks This does not belong here, it should move to ParticipantUDP or + /// something like that in the future + const char* ipAddress = "0.0.0.0"; + /// @brief The port number for UDP communication with the participant. + /// @remarks This does not belong here, it should move to ParticipantUDP or + /// something like that in the future + unsigned int port = 0; + + /// @brief The network Id to identify the participant + unsigned char networkId = 0; + + /// @brief The root thing for this participant + Thing* root = nullptr; + + public: #if defined(NO_STD) unsigned char thingCount = 0; Thing* things[MAX_THING_COUNT]; #else - /// @brief The list of things managed by this participant + /// @brief The things managed by this participant std::list things; #endif - - public: /// @brief Find a thing managed by this participant - /// @param networkId The network ID for the thing /// @param thingId The ID of the thing - /// @return The thing if found or nullptr when no thing has been found - /// @note The use of the network ID is likely to disappear in future versions. + /// @return The thing if found, nullptr when no thing has been found Thing* Get(unsigned char thingId); /// @brief Add a new thing for this participant. /// @param thing The thing to add - /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing - /// Id will be assigned. + /// @param checkId If true, the thing.id is regenerated if it is zero void Add(Thing* thing, bool checkId = true); /// @brief Remove a thing for this participant /// @param thing The thing to remove void Remove(Thing* thing); + +#pragma endregion Properties + +#pragma region Update + + public: + /// @brief Update all things for this participant + virtual void Update(); + +#pragma endregion Update + +#pragma region Send + + public: + char buffer[1024]; + + virtual bool Send(IMessage* msg); + +#pragma endregion Send + +#pragma region Participant Registry + + public: + static ParticipantRegistry registry; + +#pragma endregion Participant Registry }; } // namespace RoboidControl diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 8ead868..dc26caf 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -1,56 +1,53 @@ #include "ParticipantUDP.h" +#include "Participant.h" #include "Thing.h" #include "Arduino/ArduinoParticipant.h" #include "EspIdf/EspIdfParticipant.h" - -#if defined(_WIN32) || defined(_WIN64) -#include -#include -#include "Windows/WindowsParticipant.h" -#pragma comment(lib, "ws2_32.lib") - -#elif defined(__unix__) || defined(__APPLE__) -#include -#include // For fcntl -#include -#include -#include -#include #include "Posix/PosixParticipant.h" -#endif +#include "Windows/WindowsParticipant.h" #include namespace RoboidControl { -ParticipantUDP::ParticipantUDP(int port) { - this->ipAddress = "0.0.0.0"; - this->port = port; +#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); -static ParticipantUDP* isolatedParticipant = nullptr; + this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root; + this->root->owner = this; + this->root->name = "UDP Root"; + this->Add(this->root); -ParticipantUDP* ParticipantUDP::Isolated() { - if (isolatedParticipant == nullptr) - isolatedParticipant = new ParticipantUDP(0); - return isolatedParticipant; + Participant::ReplaceLocalParticipant(*this); } void ParticipantUDP::begin() { - if (this->isIsolated) + if (this->isIsolated || this->remoteSite == nullptr) return; SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port); @@ -69,7 +66,7 @@ void ParticipantUDP::SetupUDP(int localPort, #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = static_cast(this); - thisArduino->Setup(localPort, remoteIpAddress, remotePort); + thisArduino->Setup(); #elif defined(IDF_VER) EspIdf::ParticipantUDP* thisEspIdf = static_cast(this); @@ -78,9 +75,16 @@ void ParticipantUDP::SetupUDP(int localPort, this->connected = true; } -void ParticipantUDP::Update(unsigned long currentTimeMs) { - if (currentTimeMs == 0) - currentTimeMs = Thing::GetTimeMs(); +#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(); if (this->isIsolated == false) { if (this->connected == false) @@ -91,7 +95,8 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { if (this->remoteSite == nullptr) this->Publish(msg); else - this->Send(this->remoteSite, msg); + this->Send(msg); + delete msg; this->nextPublishMe = currentTimeMs + this->publishInterval; @@ -100,107 +105,85 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { this->ReceiveUDP(); } + UpdateMyThings(); + UpdateOtherThings(); +} + +void ParticipantUDP::UpdateMyThings() { for (Thing* thing : this->things) { - if (thing == nullptr) + if (thing == nullptr) // || thing->GetParent() != nullptr) continue; - std::cout << "Update thing " << (int)thing->id << std::endl; - if (this->isIsolated == false) { - PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); - this->Send(thing->owner, poseMsg); - BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); - this->Send(thing->owner, binaryMsg); - delete poseMsg; - } - thing->Update(currentTimeMs, true); + // 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); + + if (thing->terminate) + this->Remove(thing); } } -void ParticipantUDP::ReceiveUDP() { -#if defined(_WIN32) || defined(_WIN64) - Windows::ParticipantUDP* thisWindows = - static_cast(this); - thisWindows->Receive(); -#elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = static_cast(this); - thisPosix->Receive(); -#elif defined(ARDUINO) - Arduino::ParticipantUDP* thisArduino = - static_cast(this); - thisArduino->Receive(); -#elif defined(IDF_VER) - EspIdf::ParticipantUDP* thisEspIdf = - static_cast(this); - thisEspIdf->Receive(); -#endif -} - -Participant* ParticipantUDP::GetParticipant(const char* ipAddress, int port) { - for (Participant* sender : this->senders) { - if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port) - return sender; - } - return nullptr; -} - -Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) { - // std::cout << "New Participant " << ipAddress << ":" << port << "\n"; - Participant* participant = new Participant(ipAddress, port); +void ParticipantUDP::UpdateOtherThings() { #if defined(NO_STD) - participant->networkId = this->senderCount; - this->senders[this->senderCount++] = participant; + Participant** participants = Participant::registry.GetAll(); + for (int ix = 0; ix < Participant::registry.count; ix++) { + Participant* participant = participants[ix]; #else - participant->networkId = (unsigned char)this->senders.size(); - this->senders.push_back(participant); + for (Participant* participant : Participant::registry.GetAll()) { #endif - return participant; + if (participant == nullptr || participant == this) + continue; + + // Call only the Participant version of the Update. + // This is to deal with the function where one of the (remote) + // participants is actually a local participant + participant->Participant::Update(); + if (this->isIsolated) + continue; + + for (Thing* thing : participant->things) { + PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing); + participant->Send(poseMsg); + delete poseMsg; + BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing); + participant->Send(binaryMsg); + delete binaryMsg; + } + } } +// Update +#pragma endregion + #pragma region Send void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, Thing* thing) { // std::cout << "Send thing info [" << (int)thing->id << "] \n"; ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); - this->Send(remoteParticipant, thingMsg); + remoteParticipant->Send(thingMsg); delete thingMsg; NameMsg* nameMsg = new NameMsg(this->networkId, thing); - this->Send(remoteParticipant, nameMsg); + remoteParticipant->Send(nameMsg); delete nameMsg; ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing); - this->Send(remoteParticipant, modelMsg); + remoteParticipant->Send(modelMsg); delete modelMsg; PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true); - this->Send(remoteParticipant, poseMsg); + remoteParticipant->Send(poseMsg); delete poseMsg; - BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing); - this->Send(remoteParticipant, customMsg); - delete customMsg; + BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); + remoteParticipant->Send(binaryMsg); + delete binaryMsg; } -bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { - int bufferSize = msg->Serialize(this->buffer); - if (bufferSize <= 0) - return true; - -#if defined(_WIN32) || defined(_WIN64) - Windows::ParticipantUDP* thisWindows = - static_cast(this); - return thisWindows->Send(remoteParticipant, bufferSize); -#elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = static_cast(this); - return thisPosix->Send(remoteParticipant, bufferSize); -#elif defined(ARDUINO) - Arduino::ParticipantUDP* thisArduino = - static_cast(this); - return thisArduino->Send(remoteParticipant, bufferSize); -#elif defined(IDF_VER) - EspIdf::ParticipantUDP* thisEspIdf = - static_cast(this); - return thisEspIdf->Send(remoteParticipant, bufferSize); -#else - return false; -#endif +bool ParticipantUDP::Send(IMessage* msg) { + if (this->remoteSite != nullptr) + return this->remoteSite->Send(msg); + + return true; } void ParticipantUDP::PublishThingInfo(Thing* thing) { @@ -225,6 +208,7 @@ void ParticipantUDP::PublishThingInfo(Thing* thing) { } bool ParticipantUDP::Publish(IMessage* msg) { + // std::cout << "publish msg\n"; #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = static_cast(this); @@ -250,14 +234,37 @@ bool ParticipantUDP::Publish(IMessage* msg) { #pragma region Receive +void ParticipantUDP::ReceiveUDP() { +#if defined(_WIN32) || defined(_WIN64) + Windows::ParticipantUDP* thisWindows = + static_cast(this); + thisWindows->Receive(); +#elif defined(__unix__) || defined(__APPLE__) + Posix::ParticipantUDP* thisPosix = static_cast(this); + thisPosix->Receive(); +#elif defined(ARDUINO) + Arduino::ParticipantUDP* thisArduino = + static_cast(this); + thisArduino->Receive(); +#elif defined(IDF_VER) + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); + thisEspIdf->Receive(); +#endif +} + void ParticipantUDP::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) { - Participant* sender = this->GetParticipant(senderIpAddress, senderPort); + // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort + // << std::endl; + Participant* sender = this->registry.Get(senderIpAddress, senderPort); if (sender == nullptr) { - sender = this->AddParticipant(senderIpAddress, senderPort); + sender = this->registry.Add(senderIpAddress, senderPort); +#if !defined(NO_STD) std::cout << "New remote participant " << sender->ipAddress << ":" << sender->port << std::endl; +#endif } ReceiveData(packetSize, sender); @@ -275,8 +282,8 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize, Process(sender, msg); delete msg; } break; - case SiteMsg::id: { - SiteMsg* msg = new SiteMsg(this->buffer); + case NetworkIdMsg::id: { + NetworkIdMsg* msg = new NetworkIdMsg(this->buffer); bufferSize -= msg->length; Process(sender, msg); delete msg; @@ -316,11 +323,25 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize, Process(sender, msg); delete msg; } break; + case TextMsg::id: { + TextMsg* msg = new TextMsg(this->buffer); + bufferSize -= msg->length + msg->textLength; + Process(sender, msg); + delete msg; + } break; + case DestroyMsg::id: { + DestroyMsg* msg = new DestroyMsg(this->buffer); + bufferSize -= msg->length; + Process(sender, msg); + delete msg; + } break; }; - // Check if the buffer has been read completely +// Check if the buffer has been read completely +#if !defined(NO_STD) if (bufferSize > 0) std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n"; +#endif } void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { @@ -330,15 +351,16 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { #endif } -void ParticipantUDP::Process(Participant* sender, SiteMsg* 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); } @@ -380,11 +402,15 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { nameLength); // Leave space for null terminator #endif thingName[nameLength] = '\0'; - thing->name = thingName; + thing->SetName(thingName); - std::cout << thing->name; +#if !defined(NO_STD) + std::cout << thing->GetName(); +#endif } +#if !defined(NO_STD) std::cout << std::endl; +#endif } void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) { @@ -395,26 +421,66 @@ void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) { } void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { -#if defined(DEBUG) +#if !defined(DEBUG) && !defined(NO_STD) std::cout << this->name << ": process PoseMsg [" << (int)this->networkId - << "/" << (int)msg->networkId << "]\n"; + << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n"; #endif + Participant* owner = Participant::registry.Get(msg->networkId); + if (owner == nullptr) + return; + + Thing* thing = owner->Get(msg->thingId); + if (thing == nullptr) + return; + + if ((msg->poseType & PoseMsg::Pose_Position) != 0) + thing->SetPosition(msg->position); + if ((msg->poseType & PoseMsg::Pose_Orientation) != 0) + thing->SetOrientation(msg->orientation); + if ((msg->poseType & PoseMsg::Pose_LinearVelocity) != 0) + thing->SetLinearVelocity(msg->linearVelocity); + if ((msg->poseType & PoseMsg::Pose_AngularVelocity) != 0) + thing->SetAngularVelocity(msg->angularVelocity); } void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId - << "/" << (int)msg->thingId << "] "; + << "/" << (int)msg->thingId << "]\n"; +#endif + + Participant* owner = Participant::registry.Get(msg->networkId); + if (owner != nullptr) { + Thing* thing = owner->Get(msg->thingId); + if (thing != nullptr) + thing->ProcessBinary(msg->data); +#if !defined(NO_STD) + else { +#if defined(DEBUG) + std::cout << " unknown thing [" << (int)msg->networkId << "/" + << (int)msg->thingId << "]"; +#endif + } +#endif + } +} + +void ParticipantUDP::Process(Participant* sender, TextMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " " + << (int)msg->text << "\n"; +#endif +} + +void ParticipantUDP::Process(Participant* sender, DestroyMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/" + << (int)msg->thingId << "]\n"; #endif Thing* thing = sender->Get(msg->thingId); if (thing != nullptr) - thing->ProcessBinary(msg->data); - else { - std::cout << " unknown thing [" << (int)msg->networkId << "/" - << (int)msg->thingId << "]"; - } - std::cout << std::endl; + this->Remove(thing); } // Receive diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index ec93af9..a23bb6b 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -1,13 +1,15 @@ #pragma once #include "Messages/BinaryMsg.h" +#include "Messages/DestroyMsg.h" #include "Messages/InvestigateMsg.h" #include "Messages/ModelUrlMsg.h" #include "Messages/NameMsg.h" #include "Messages/ParticipantMsg.h" #include "Messages/PoseMsg.h" -#include "Messages/SiteMsg.h" +#include "Messages/NetworkIdMsg.h" #include "Messages/ThingMsg.h" +#include "Messages/TextMsg.h" #include "Participant.h" #if !defined(NO_STD) @@ -29,7 +31,8 @@ namespace RoboidControl { constexpr int MAX_SENDER_COUNT = 256; -/// @brief A local participant is the local device which can communicate with +/// @brief A participant using UDP communication +/// A local participant is the local device which can communicate with /// other participants It manages all local things and communcation with other /// participants. Each application has a local participant which is usually /// explicit in the code. An participant can be isolated. In that case it is @@ -41,6 +44,9 @@ constexpr int MAX_SENDER_COUNT = 256; /// RoboidControl::IsolatedParticipant::Isolated(). /// @sa RoboidControl::Thing::Thing() class ParticipantUDP : public Participant { + +#pragma region Init + public: /// @brief Create a participant without connecting to a site /// @param port The port on which the participant communicates @@ -51,41 +57,28 @@ class ParticipantUDP : public Participant { /// @brief Create a participant which will try to connect to a site. /// @param ipAddress The IP address of the site /// @param port The port used by the site - ParticipantUDP(const char* ipAddress, - int port = 7681, - int localPort = 7681); - // Note to self: one cannot specify the port used by the local participant - // now!! + /// @param localPort The port used by the local participant + ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681); - /// @brief Isolated participant is used when the application is run without - /// networking - /// @return A participant without networking support - static ParticipantUDP* Isolated(); +#pragma endregion Init +#pragma region Properties + +public: /// @brief True if the participant is running isolated. /// Isolated participants do not communicate with other participants bool isIsolated = false; + /// @brief The remote site when this participant is connected to a site + Participant* remoteSite = nullptr; + /// The interval in milliseconds for publishing (broadcasting) data on the /// local network long publishInterval = 3000; // 3 seconds - /// @brief The name of the participant - const char* name = "ParticipantUDP"; - - // int localPort = 0; - - /// @brief The remote site when this participant is connected to a site - Participant* remoteSite = nullptr; - -#if defined(ARDUINO) - // const char* remoteIpAddress = nullptr; - // unsigned short remotePort = 0; - // char* broadcastIpAddress = nullptr; - - // WiFiUDP udp; -#else + protected: +#if !defined(ARDUINO) #if defined(__unix__) || defined(__APPLE__) int sock; #elif defined(_WIN32) || defined(_WIN64) @@ -93,81 +86,63 @@ class ParticipantUDP : public Participant { sockaddr_in server_addr; sockaddr_in broadcast_addr; #endif - #endif - + public: void begin(); bool connected = false; - virtual void Update(unsigned long currentTimeMs = 0) override; +#pragma endregion Properties + +#pragma region Update + + public: + virtual void Update() override; + + protected: + unsigned long nextPublishMe = 0; + + /// @brief Prepare the local things for the next update + //virtual void PrepMyThings(); + virtual void UpdateMyThings(); + virtual void UpdateOtherThings(); + +#pragma endregion Update + +#pragma region Send void SendThingInfo(Participant* remoteParticipant, Thing* thing); void PublishThingInfo(Thing* thing); - bool Send(Participant* remoteParticipant, IMessage* msg); + virtual bool Send(IMessage* msg) override; bool Publish(IMessage* msg); +#pragma endregion Send + +#pragma region Receive + +protected: void ReceiveData(unsigned char bufferSize, char* senderIpAddress, unsigned int senderPort); void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant); -#if defined(NO_STD) - unsigned char senderCount = 0; - Participant* senders[MAX_SENDER_COUNT]; -#else - std::list senders; -#endif - - protected: - unsigned long nextPublishMe = 0; - - char buffer[1024]; - void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort); - Participant* GetParticipant(const char* ipAddress, int port); - Participant* AddParticipant(const char* ipAddress, int port); - void ReceiveUDP(); virtual void Process(Participant* sender, ParticipantMsg* msg); - virtual void Process(Participant* sender, SiteMsg* msg); + virtual void Process(Participant* sender, NetworkIdMsg* msg); virtual void Process(Participant* sender, InvestigateMsg* msg); virtual void Process(Participant* sender, ThingMsg* msg); virtual void Process(Participant* sender, NameMsg* msg); virtual void Process(Participant* sender, ModelUrlMsg* msg); virtual void Process(Participant* sender, PoseMsg* msg); virtual void Process(Participant* sender, BinaryMsg* msg); + virtual void Process(Participant* sender, TextMsg* msg); + virtual void Process(Participant* sender, DestroyMsg* msg); -#if !defined(NO_STD) -// public: -// using ThingConstructor = std::function; +#pragma endregion Receive -// template -// void Register(unsigned char thingType) { -// thingMsgProcessors[thingType] = [](Participant* participant, -// unsigned char networkId, -// unsigned char thingId) { -// return new ThingClass(participant, networkId, thingId); -// }; -// }; - -// template -// void Register2(unsigned char thingType, ThingConstructor f) { -// thingMsgProcessors[thingType] = [f](Participant* participant, -// unsigned char networkId, -// unsigned char thingId) { -// return f(participant, networkId, thingId); -// }; -// }; - -// protected: -// std::unordered_map thingMsgProcessors; - -#endif }; } // namespace RoboidControl diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 8bde494..cc61639 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -9,58 +9,89 @@ namespace RoboidControl { -SiteServer::SiteServer(int port) { +#pragma region Init + +SiteServer::SiteServer(int port) : ParticipantUDP(port) { this->name = "Site Server"; this->publishInterval = 0; - this->ipAddress = "0.0.0.0"; - this->port = port; - -#if defined(NO_STD) - this->senders[this->senderCount++] = this; -#else - this->senders.push_back(this); -#endif - SetupUDP(port, ipAddress, 0); - -#if !defined(NO_STD) - // Register((unsigned char)Thing::Type::TemperatureSensor); -#endif } +#pragma endregion Init + +#pragma region Update + +void SiteServer::UpdateMyThings() { + for (Thing* thing : this->things) { + if (thing == nullptr) + continue; + + thing->Update(true); + + if (this->isIsolated == false) { + // Send to all other participants +#if defined(NO_STD) + Participant** participants = Participant::registry.GetAll(); + for (int ix = 0; ix < Participant::registry.count; ix++) { + Participant* participant = participants[ix]; +#else + for (Participant* participant : Participant::registry.GetAll()) { +#endif + if (participant == nullptr || participant == this) + continue; + + PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); + participant->Send(poseMsg); + delete poseMsg; + BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); + participant->Send(binaryMsg); + delete binaryMsg; + } + } + } +} + +#pragma endregion Update + +#pragma region Receive + void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { - if (msg->networkId == 0) { + if (msg->networkId != sender->networkId) { // std::cout << this->name << " received New Client -> " << // sender->ipAddress // << ":" << (int)sender->port << "\n"; - SiteMsg* msg = new SiteMsg(sender->networkId); - this->Send(sender, msg); + NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId); + sender->Send(msg); delete msg; } } -void SiteServer::Process(Participant* sender, SiteMsg* msg) {} +void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {} void SiteServer::Process(Participant* sender, ThingMsg* msg) { Thing* thing = sender->Get(msg->thingId); - if (thing == nullptr) { - // #if defined(NO_STD) - new Thing(sender, (Thing::Type)msg->thingType, - msg->thingId); - // #else - // auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType); - // //Thing* newThing; - // if (thingMsgProcessor != thingMsgProcessors.end()) // found item - // //newThing = - // thingMsgProcessor->second(sender, msg->networkId, - // msg->thingId); - // else - // //newThing = - // new Thing(sender, msg->networkId, msg->thingId, - // (Thing::Type)msg->thingType); - // #endif - } + if (thing == nullptr) + // new Thing(sender, (Thing::Type)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)); + if (thing->IsRoot()) + // if (thing->GetParent() != nullptr) +#if defined(NO_STD) + ; +#else + std::cout << "Could not find parent [" << (int)msg->networkId << "/" + << (int)msg->parentId << "]\n"; +#endif + } else + thing->SetParent(Thing::LocalRoot()); } +#pragma endregion Receive + } // namespace RoboidControl diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h index 0fe2974..87029c5 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -2,45 +2,43 @@ #include "ParticipantUDP.h" -#if !defined(NO_STD) -#include -#include -#include -#endif +// #if !defined(NO_STD) +// #include +// #include +// #include +// #endif namespace RoboidControl { /// @brief A participant is device which can communicate with other participants class SiteServer : public ParticipantUDP { + +#pragma region Init + public: + /// @brief Create a new site server + /// @param port The port of which to receive the messages SiteServer(int port = 7681); - // virtual void Update(unsigned long currentTimeMs = 0) override; +#pragma endregion Init -// #if !defined(NO_STD) -// template -// void Register(unsigned char thingType) { -// thingMsgProcessors[thingType] = [](Participant* participant, -// unsigned char networkId, -// unsigned char thingId) { -// return new ThingClass(participant, networkId, thingId); -// }; -// }; -// #endif +#pragma region Update + + virtual void UpdateMyThings() override; + +#pragma endregion Update + +#pragma region Receive protected: unsigned long nextPublishMe = 0; virtual void Process(Participant* sender, ParticipantMsg* msg) override; - virtual void Process(Participant* sender, SiteMsg* msg) override; + virtual void Process(Participant* sender, NetworkIdMsg* msg) override; virtual void Process(Participant* sender, ThingMsg* msg) override; -// #if !defined(NO_STD) -// using ThingConstructor = std::function; -// std::unordered_map thingMsgProcessors; -// #endif +#pragma endregion Receive + }; } // namespace RoboidControl diff --git a/Posix/PosixParticipant.cpp b/Posix/PosixParticipant.cpp index 88e18af..a01a059 100644 --- a/Posix/PosixParticipant.cpp +++ b/Posix/PosixParticipant.cpp @@ -11,7 +11,7 @@ namespace RoboidControl { namespace Posix { -void Setup(int localPort, const char* remoteIpAddress, int remotePort) { +void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(__unix__) || defined(__APPLE__) // Create a UDP socket @@ -63,7 +63,7 @@ void Setup(int localPort, const char* remoteIpAddress, int remotePort) { #endif } -void Receive() { +void ParticipantUDP::Receive() { #if defined(__unix__) || defined(__APPLE__) sockaddr_in client_addr; socklen_t len = sizeof(client_addr); @@ -74,9 +74,9 @@ void Receive() { unsigned int sender_port = ntohs(client_addr.sin_port); ReceiveData(packetSize, sender_ipAddress, sender_port); - // RoboidControl::Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port); + // RoboidControl::Participant* remoteParticipant = this->Get(sender_ipAddress, sender_port); // if (remoteParticipant == nullptr) { - // remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port); + // remoteParticipant = this->Add(sender_ipAddress, sender_port); // // std::cout << "New sender " << sender_ipAddress << ":" << sender_port // // << "\n"; // // std::cout << "New remote participant " << remoteParticipant->ipAddress @@ -90,7 +90,7 @@ void Receive() { #endif } -bool Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { #if defined(__unix__) || defined(__APPLE__) // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port) // << "\n"; @@ -113,7 +113,7 @@ bool Send(Participant* remoteParticipant, int bufferSize) { return true; } -bool Publish(IMessage* msg) { +bool ParticipantUDP::Publish(IMessage* msg) { #if defined(__unix__) || defined(__APPLE__) int bufferSize = msg->Serialize(this->buffer); if (bufferSize <= 0) diff --git a/Posix/PosixParticipant.h b/Posix/PosixParticipant.h index ec16d32..98302f3 100644 --- a/Posix/PosixParticipant.h +++ b/Posix/PosixParticipant.h @@ -11,6 +11,13 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { void Receive(); bool Send(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); + + protected: +#if defined(__unix__) || defined(__APPLE__) + sockaddr_in remote_addr; + sockaddr_in server_addr; + sockaddr_in broadcast_addr; +#endif }; } // namespace Posix diff --git a/README.md b/README.md index 28e4abd..77862b9 100644 --- a/README.md +++ b/README.md @@ -14,5 +14,60 @@ Supporting: # Basic components - RoboidControl::Thing -- RoboidControl::LocalParticipant -- RoboidControl::SiteServer \ No newline at end of file +- RoboidControl::Participant + +# Installation + +## Core code + +The repository uses cmake for building. You can place it in a subfolder of your project and include it in you `CMakeLists.txt`. +For example if the library is placed in the subfolder `roboidcontrol`: +``` +# Add the path to Roboid Control +add_subdirectory(roboidcontrol) + +# Your source files/executable +add_executable(my_executable main.cpp) + +# Link against RoboidControl +target_link_libraries(my_executable RoboidControl) +``` + +## Arduino (PlatformIO) + +Arduino is only supported in combination with PlatformIO. The Arduino IDE is not (yet?) supported. + +The best way to include support for Roboid Control in PlatformIO is +to clone the Roboid Control for C++ repository into a subfolder of the /lib folder. +Alternatively you can download the zip file and unpack it as a subfolder of the /lib folder. + +## ESP-IDF + +The best way to include support for Roboid Control in PlatformIO is +to clone the Roboid Control for C++ repository into a subfolder of the /components folder. +Alternatively you can download the zip file and unpack it as a subfolder of the /components folder. + +Make sure you have included RoboidControl as a component in your top-level CMakeLists.txt, for example: +``` +list(APPEND EXTRA_COMPONENT_DIRS + components/RoboidControl + ) +``` + +# Get Started + +## Core C++ Examples + +This repository contains examples in the `examples` folder. You can build these using cmake. + +For example, to build the BB2A example: +``` +cmake -B build -D BUILD_EXAMPLE_BB2A=ON +cmake --build build +``` +The resulting executable is then `build/examples/Debug/BB2A.exe` + +## Arduino (PlatformIO) Examples + +Specific examples for the Arduino platform are found in the `Arduino\examples` folder. +To use them you should create a new project in PlatformIO and then copy the example code to your project. \ No newline at end of file diff --git a/Testing/Temporary/LastTest.log b/Testing/Temporary/LastTest.log new file mode 100644 index 0000000..3ab3dd5 --- /dev/null +++ b/Testing/Temporary/LastTest.log @@ -0,0 +1,3 @@ +Start testing: Jun 17 17:17 W. Europe Summer Time +---------------------------------------------------------- +End testing: Jun 17 17:17 W. Europe Summer Time diff --git a/Thing.cpp b/Thing.cpp index 96c888f..efb6bfb 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -5,6 +5,7 @@ #include "Participants/IsolatedParticipant.h" #include +// #include #if defined(ARDUINO) #include "Arduino.h" @@ -17,72 +18,78 @@ namespace RoboidControl { -// LocalParticipant* Thing::CheckHiddenParticipant() { -// if (isolatedParticipant == nullptr) -// isolatedParticipant = new LocalParticipant(0); -// return isolatedParticipant; -// } +#pragma region Init -Thing::Thing(int thingType) - : Thing(IsolatedParticipant::Isolated(), thingType) {} +Thing* Thing::LocalRoot() { + Participant* p = Participant::LocalParticipant; + Thing* localRoot = p->root; + return localRoot; +} -Thing::Thing(Participant* owner, Type thingType, unsigned char thingId) - : Thing(owner, (unsigned char)thingType, thingId) {} - -Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { - this->owner = owner; - this->id = thingId; - this->type = thingType; - this->networkId = 0; +// Only use this for root things +Thing::Thing(Participant* owner) { + this->type = Type::Root; + this->name = "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; - // std::cout << "add thing [" << (int)this->id << "] to owner " - // << this->owner->ipAddress << ":" << this->owner->port << std::endl; + this->owner = owner; + this->owner->Add(this); + // std::cout << this->owner->name << ": New root thing " << std::endl; +} + +void Thing::CreateRoot(Participant* owner) { + owner->root = new Thing(owner); +} + +Thing::Thing(Thing* parent) { + this->type = Type::Undetermined; + + 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 = parent->owner; this->owner->Add(this, true); + this->SetParent(parent); + + std::cout << this->owner->name << ": New thing for " << parent->name + << std::endl; } -// Thing::Thing(Participant* owner, -// Type thingType, -// int thingId) { -// // no participant reference yet.. -// this->owner = owner; -// this->networkId = networkId; -// this->id = thingId; -// this->type = (unsigned char)thingType; - -// this->linearVelocity = Spherical::zero; -// this->angularVelocity = Spherical::zero; -// // std::cout << "Created thing " << (int)this->networkId << "/" << -// // (int)this->id -// // << "\n"; -// owner->Add(this, false); -// } - -void Thing::Terminate() { - // Thing::Remove(this); +Thing::~Thing() { + std::cout << "Destroy thing " << this->name << std::endl; } -Thing* Thing::FindThing(const char* name) { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing* child = this->children[childIx]; - if (child == nullptr || child->name == nullptr) - continue; +#pragma endregion Init - if (strcmp(child->name, name) == 0) - return child; - - Thing* foundChild = child->FindThing(name); - if (foundChild != nullptr) - return foundChild; - } - return nullptr; +void Thing::SetName(const char* name) { + this->name = name; + this->nameChanged = true; } +const char* Thing::GetName() const { + return this->name; +} + +void Thing::SetModel(const char* url) { + this->modelUrl = url; +} + +#pragma region Hierarchy + void Thing::SetParent(Thing* parent) { if (parent == nullptr) { Thing* parentThing = this->parent; @@ -91,18 +98,21 @@ void Thing::SetParent(Thing* parent) { this->parent = nullptr; } else parent->AddChild(this); + this->hierarchyChanged = true; } -void Thing::SetParent(Thing* root, const char* name) { - Thing* thing = root->FindThing(name); - if (thing != nullptr) - this->SetParent(thing); +bool Thing::IsRoot() const { + return this == LocalRoot() || this->parent == nullptr; } Thing* Thing::GetParent() { return this->parent; } +Thing* Thing::GetChildByIndex(unsigned char ix) { + return this->children[ix]; +} + void Thing::AddChild(Thing* child) { unsigned char newChildCount = this->childCount + 1; Thing** newChildren = new Thing*[newChildCount]; @@ -142,7 +152,7 @@ Thing* Thing::RemoveChild(Thing* child) { } } - child->parent = nullptr; + child->parent = Thing::LocalRoot(); delete[] this->children; this->children = newChildren; @@ -151,7 +161,7 @@ Thing* Thing::RemoveChild(Thing* child) { return child; } -Thing* Thing::GetChild(unsigned char id, bool recursive) { +Thing* Thing::GetChild(unsigned char id, bool recurse) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { Thing* child = this->children[childIx]; if (child == nullptr) @@ -159,8 +169,8 @@ Thing* Thing::GetChild(unsigned char id, bool recursive) { if (child->id == id) return child; - if (recursive) { - Thing* foundChild = child->GetChild(id, recursive); + if (recurse) { + Thing* foundChild = child->GetChild(id, recurse); if (foundChild != nullptr) return foundChild; } @@ -168,57 +178,25 @@ Thing* Thing::GetChild(unsigned char id, bool recursive) { return nullptr; } -Thing* Thing::GetChildByIndex(unsigned char ix) { - return this->children[ix]; -} +Thing* Thing::FindChild(const char* name, bool recurse) { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + Thing* child = this->children[childIx]; + if (child == nullptr || child->name == nullptr) + continue; -void Thing::SetModel(const char* url) { - this->modelUrl = url; -} + if (strcmp(child->name, name) == 0) + return child; -unsigned long Thing::GetTimeMs() { -#if defined(ARDUINO) - return millis(); -#else - auto now = std::chrono::steady_clock::now(); - auto ms = std::chrono::duration_cast( - now.time_since_epoch()); - return static_cast(ms.count()); -#endif -} - -void Thing::Update(bool recursive) { - Update(GetTimeMs(), recursive); -} - -void Thing::Update(unsigned long currentTimeMs, bool recursive) { - // if (this->positionUpdated || this->orientationUpdated) - // OnPoseChanged callback - this->positionUpdated = false; - this->orientationUpdated = false; - - if (recursive) { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing* child = this->children[childIx]; - if (child == nullptr) - continue; - child->Update(currentTimeMs, recursive); - } + Thing* foundChild = child->FindChild(name); + if (foundChild != nullptr) + return foundChild; } + return nullptr; } -void Thing::UpdateThings(unsigned long currentTimeMs) { - IsolatedParticipant::Isolated()->Update(currentTimeMs); -} +#pragma endregion Hierarchy -int Thing::GenerateBinary(char* buffer, unsigned char* ix) { - (void)buffer; - (void)ix; - return 0; -} -void Thing::ProcessBinary(char* bytes) { - (void)bytes; -}; +#pragma region Pose void Thing::SetPosition(Spherical position) { this->position = position; @@ -238,8 +216,10 @@ SwingTwist Thing::GetOrientation() { } void Thing::SetLinearVelocity(Spherical linearVelocity) { - this->linearVelocity = linearVelocity; - this->linearVelocityUpdated = true; + if (this->linearVelocity.distance != linearVelocity.distance) { + this->linearVelocity = linearVelocity; + this->linearVelocityUpdated = true; + } } Spherical Thing::GetLinearVelocity() { @@ -247,12 +227,60 @@ Spherical Thing::GetLinearVelocity() { } void Thing::SetAngularVelocity(Spherical angularVelocity) { - this->angularVelocity = angularVelocity; - this->angularVelocityUpdated = true; + if (this->angularVelocity.distance != angularVelocity.distance) { + this->angularVelocity = angularVelocity; + this->angularVelocityUpdated = true; + } } Spherical Thing::GetAngularVelocity() { return this->angularVelocity; } +#pragma endregion Pose + +#pragma region Update + +unsigned long Thing::GetTimeMs() { +#if defined(ARDUINO) + unsigned long ms = millis(); + return ms; +#else + auto now = std::chrono::steady_clock::now(); + auto ms = std::chrono::duration_cast( + now.time_since_epoch()); + return static_cast(ms.count()); +#endif +} + +void Thing::Update(bool recursive) { + this->positionUpdated = false; + this->orientationUpdated = false; + this->linearVelocityUpdated = false; + this->angularVelocityUpdated = false; + this->hierarchyChanged = false; + this->nameChanged = false; + + if (recursive) { + // std::cout << "# children: " << (int)this->childCount << std::endl; + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + Thing* child = this->children[childIx]; + if (child == nullptr) + continue; + child->Update(recursive); + } + } +} + +#pragma endregion Update + +int Thing::GenerateBinary(char* buffer, unsigned char* ix) { + (void)buffer; + (void)ix; + return 0; +} +void Thing::ProcessBinary(char* bytes) { + (void)bytes; +}; + } // namespace RoboidControl \ No newline at end of file diff --git a/Thing.h b/Thing.h index febe7a9..f538c33 100644 --- a/Thing.h +++ b/Thing.h @@ -20,71 +20,117 @@ class ParticipantUDP; class Thing { public: /// @brief Predefined thing types - enum Type { - Undetermined, - // Sensor, - Switch, - DistanceSensor, - DirectionalSensor, - TemperatureSensor, - TouchSensor, - // Motor, - ControlledMotor, - UncontrolledMotor, - Servo, + struct Type { + static const unsigned char Undetermined = 0x00; + // Sensor + static const unsigned char Switch = 0x01; + static const unsigned char DistanceSensor = 0x02; + static const unsigned char DirectionalSensor = 0x03; + static const unsigned char TemperatureSensor = 0x04; + static const unsigned char TouchSensor = 0x05; + // Motor + static const unsigned char ControlledMotor = 0x06; + static const unsigned char UncontrolledMotor = 0x07; + static const unsigned char Servo = 0x08; + static const unsigned char RelativeEncoder = 0x19; // Other - Roboid, - Humanoid, - ExternalSensor, + static const unsigned char Root = 0x10; + static const unsigned char Roboid = 0x09; + static const unsigned char Humanoid = 0x0A; + static const unsigned char ExternalSensor = 0x08; + static const unsigned char Animator = 0x0C; + static const unsigned char DifferentialDrive = 0x0D; }; - /// @brief Create a new thing using an implicit local participant - /// @param thingType The type of thing - Thing(int thingType = Type::Undetermined); - /// @brief Create a new thing of the given type - /// @param thingType The predefined type of thing - Thing(Participant* participant, - Type thingType = Type::Undetermined, - unsigned char thingId = 0); - /// @brief Create a new thing of the give type - /// @param thingType The custom type of the thing - Thing(Participant* participant, int thingType, unsigned char thingId = 0); - /// @brief Create a new thing for the given participant - /// @param participant The participant for which this thing is created - /// @param networkId The network ID of the thing - /// @param thingId The ID of the thing - /// @param thingType The type of thing - // Thing(Participant* participant, - // unsigned char networkId, - // unsigned char thingId, - // Type thingType = Type::Undetermined); +#pragma region Init - /// @brief The participant managing this thing - Participant* owner; - /// @brief The network ID of this thing - /// @note This field will likely disappear in future versions - unsigned char networkId = 0; + private: + public: + /// @brief Create a new Thing + /// @param parent (optional) The parent thing + /// The owner will be the same as the owner of the parent thing, it will + /// be Participant::LocalParticipant if the parent is not specified. A thing + /// without a parent will be connected to the root thing. + Thing(Thing* parent = LocalRoot()); + + private: + /// @brief Constructor to create a root thing + /// @param owner The participant who will own this root thing + /// @remarks This function is private because CreateRoot() should be used + /// instead + Thing(Participant* owener); + + public: + /// @brief Destructor for a Thing + ~Thing(); + + /// @brief Create a root thing for a participant + /// @param owner The participant who will own this root thing + static void CreateRoot(Participant* owner); + + /// @brief The root thing for the local participant + /// @return The root thing for the local participant + static Thing* LocalRoot(); + +#pragma endregion Init + + public: + /// @brief Terminated things are no longer updated + bool terminate = false; + +#pragma region Properties + + public: /// @brief The ID of the thing unsigned char id = 0; /// @brief The type of Thing /// This can be either a Thing::Type of a byte value for custom types - unsigned char type = 0; + unsigned char type = Type::Undetermined; - /// @brief Find a thing by name - /// @param name Rhe name of the thing - /// @return The found thing or nullptr when nothing is found - Thing* FindThing(const char* name); + /// @brief The participant owning this thing + Participant* owner = nullptr; - /// @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); - void SetParent(Thing* root, const char* name); - /// @brief Gets the parent Thing + /// @brief The name of the thing + const char* name = nullptr; + + void SetName(const char* name); + const char* GetName() const; + bool nameChanged = false; + + /// @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 formats are .png (sprite), .gltf and .glb + void SetModel(const char* url); + + /// @brief An URL pointing to the location where a model of the thing can be + /// found + const char* modelUrl = nullptr; + +#pragma endregion Properties + +#pragma region Hierarchy + + /// @brief Sets the parent of this Thing + /// @param parent The Thing which should become the parent + void SetParent(Thing* parent); + /// @brief Gets the parent of this Thing /// @return The parent Thing Thing* GetParent(); + /// @brief Check if this is a root thing + /// @return True is this thing is a root + bool IsRoot() const; + + /// @brief The number of children + unsigned char childCount = 0; + /// @brief Get a child by index + /// @param ix The child index + /// @return The found thing of nullptr when nothing is found + Thing* GetChildByIndex(unsigned char ix); + /// @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 @@ -94,49 +140,46 @@ class Thing { /// @return The removed child or nullptr if the child could not be found Thing* RemoveChild(Thing* child); - /// @brief The number of children - unsigned char childCount = 0; /// @brief Get a child by thing Id /// @param id The thing ID to find - /// @param recursive Look recursively through all descendants + /// @param recurse Look recursively through all descendants /// @return The found thing of nullptr when nothing is found - Thing* GetChild(unsigned char id, bool recursive = false); - /// @brief Get a child by index - /// @param ix The child index - /// @return The found thing of nullptr when nothing is found - Thing* GetChildByIndex(unsigned char ix); + Thing* GetChild(unsigned char id, bool recurse = false); - protected: + /// @brief Find a thing by name + /// @param name The name of the thing + /// @param recurse Look recursively through all descendants + /// @return The found thing or nullptr when nothing is found + Thing* FindChild(const char* name, bool recurse = true); + + /// @brief Indicator that the hierarchy of the thing has changed + bool hierarchyChanged = true; + + private: Thing* parent = nullptr; Thing** children = nullptr; - public: - /// @brief The name of the thing - const char* name = nullptr; - /// @brief An URL pointing to the location where a model of the thing can be - /// found - const char* modelUrl = nullptr; - /// @brief The scale of the model (deprecated I think) - float modelScale = 1; +#pragma endregion Hierarchy +#pragma region Pose + + public: /// @brief Set the position of the thing /// @param position The new position in local space, in meters void SetPosition(Spherical position); /// @brief Get the position of the thing /// @return The position in local space, in meters Spherical GetPosition(); + /// @brief Boolean indicating that the thing has an updated position + bool positionUpdated = false; + /// @brief Set the orientation of the thing /// @param orientation The new orientation in local space void SetOrientation(SwingTwist orientation); /// @brief Get the orientation of the thing /// @return The orienation in local space SwingTwist GetOrientation(); - /// @brief The scale of the thing (deprecated I think) - // float scale = 1; // assuming uniform scale - - /// @brief boolean indicating if the position was updated - bool positionUpdated = false; - /// @brief boolean indicating if the orientation was updated + /// @brief Boolean indicating the thing has an updated orientation bool orientationUpdated = false; /// @brief Set the linear velocity of the thing @@ -146,57 +189,59 @@ class Thing { /// @brief Get the linear velocity of the thing /// @return The linear velocity in local space, in meters per second virtual Spherical GetLinearVelocity(); + /// @brief Boolean indicating the thing has an updated linear velocity + bool linearVelocityUpdated = false; + /// @brief Set the angular velocity of the thing /// @param angularVelocity the new angular velocity in local space virtual void SetAngularVelocity(Spherical angularVelocity); /// @brief Get the angular velocity of the thing /// @return The angular velocity in local space virtual Spherical GetAngularVelocity(); - bool linearVelocityUpdated = false; + /// @brief Boolean indicating the thing has an updated angular velocity bool angularVelocityUpdated = false; private: - /// @brief The position in local space + /// @brief The position of the thing in local space, in meters /// @remark When this Thing has a parent, the position is relative to the /// parent's position and orientation Spherical position; - /// @brief The orientation in local space + /// @brief The orientation of the thing in local space /// @remark When this Thing has a parent, the orientation is relative to the /// parent's orientation SwingTwist orientation; - /// @brief The linear velocity in local space + /// @brief The linear velocity of the thing in local space, in meters per + /// second Spherical linearVelocity; - /// @brief The angular velocity in local spze + /// @brief The angular velocity of the thing in local space, in degrees per + /// second Spherical angularVelocity; +#pragma endregion Pose + +#pragma region Update + public: - /// @brief Terminated things are no longer updated - void Terminate(); - - /// @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); - - static unsigned long GetTimeMs(); - - void Update(bool recursive = false); + //virtual void PrepareForUpdate(); /// @brief Updates the state of the thing - /// @param currentTimeMs The current clock time in milliseconds - virtual void Update(unsigned long currentTimeMs, bool recursive = false); + /// @param recurse When true, this will Update the descendants recursively + virtual void Update(bool recurse = false); - static void UpdateThings(unsigned long currentTimeMs); + /// @brief Get the current time in milliseconds + /// @return The current time in milliseconds + static unsigned long GetTimeMs(); +#pragma endregion Update + + public: /// @brief Function used to generate binary data for this thing /// @param buffer The byte array for thw binary data /// @param ix The starting position for writing the binary data - /// @returns The size of the binary data + /// @return The size of the binary data virtual int GenerateBinary(char* buffer, unsigned char* ix); - // /// @brief FUnction used to process binary data received for this thing + /// @brief Function used to process binary data received for this thing /// @param bytes The binary data virtual void ProcessBinary(char* bytes); }; diff --git a/Things/ControlledMotor.cpp b/Things/ControlledMotor.cpp new file mode 100644 index 0000000..9bc5d88 --- /dev/null +++ b/Things/ControlledMotor.cpp @@ -0,0 +1,68 @@ +#include "ControlledMotor.h" +#include "LinearAlgebra/FloatSingle.h" + +namespace RoboidControl { + +ControlledMotor::ControlledMotor(Motor* motor, + RelativeEncoder* encoder, + Thing* parent) + : Motor(parent), motor(motor), encoder(encoder) { + this->type = Type::ControlledMotor; + //encoder->SetParent(null); + // Thing parent = motor.GetParent(); + // this->SetParent(parent); + this->integral = 0; +} + +void ControlledMotor::SetTargetVelocity(float velocity) { + this->targetVelocity = velocity; + this->rotationDirection = + (targetVelocity < 0) ? Direction::Reverse : Direction::Forward; +} + +void ControlledMotor::Update(bool recurse) { + unsigned long currentTimeMs = GetTimeMs(); + float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f; + this->lastUpdateTime = currentTimeMs; + if (timeStep <= 0) + return; + + // encoder->Update(false); + + this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed; + + float error = this->targetVelocity - this->actualVelocity; + float p_term = error * pidP; + this->integral += error * timeStep; + float i_term = pidI * this->integral; + float d_term = pidD * (error - this->lastError) / timeStep; + this->lastError = error; + + float output = p_term + i_term + d_term; + // std::cout << "target " << this->targetVelocity << " actual " + // << this->actualVelocity << " output = " << output << std::endl; + // float acceleration = + // error * timeStep * pidP; // Just P is used at this moment + // std::cout << "motor acc. " << acceleration << std::endl; + + // float newTargetVelocity = motor->targetVelocity + acceleration; + output = LinearAlgebra::Float::Clamp(output, -1, 1); + motor->SetTargetVelocity(output); // or something like that + //motor->Update(false); +} + +// float ControlledMotor::GetActualVelocity() { +// return (int)rotationDirection * encoder->rotationSpeed; +// } + +// bool ControlledMotor::Drive(float distance) { +// if (!driving) { +// targetDistance = distance; +// startDistance = encoder->GetDistance(); +// driving = true; +// } +// float totalDistance = encoder->GetDistance() - startDistance; +// bool completed = totalDistance > targetDistance; +// return completed; +// } +} // namespace RoboidControl \ No newline at end of file diff --git a/Things/ControlledMotor.h b/Things/ControlledMotor.h new file mode 100644 index 0000000..cfb0229 --- /dev/null +++ b/Things/ControlledMotor.h @@ -0,0 +1,40 @@ +#pragma once + +#include "Motor.h" +#include "RelativeEncoder.h" + +namespace RoboidControl { + +/// @brief A motor with speed control +/// It uses a feedback loop from an encoder to regulate the speed +/// The speed is measured in revolutions per second. +class ControlledMotor : public Motor { + public: + ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot()); + + float pidP = 0.5; + float pidD = 0; + float pidI = 0.2; + + /// @brief The actual velocity in revolutions per second + float actualVelocity; + + enum Direction { Forward = 1, Reverse = -1 }; + Direction rotationDirection; + + virtual void Update(bool recurse = false) override; + + /// @brief Set the target verlocity for the motor controller + /// @param speed the target velocity in revolutions per second + virtual void SetTargetVelocity(float velocity) override; + + Motor* motor; + RelativeEncoder* encoder; + + protected: + float integral = 0; + float lastError = 0; + float lastUpdateTime; +}; + +} // namespace RoboidControl diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index bd09157..875b654 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -1,10 +1,29 @@ #include "DifferentialDrive.h" -namespace RoboidControl { -DifferentialDrive::DifferentialDrive() : Thing() {} +#include "Messages/LowLevelMessages.h" -RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) - : Thing(participant) {} +namespace RoboidControl { + +DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent) { + this->type = Type::DifferentialDrive; + 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(parent) { + this->type = Type::DifferentialDrive; + this->name = "Differential drive"; + this->leftWheel = leftMotor; + this->rightWheel = rightMotor; +} void DifferentialDrive::SetDriveDimensions(float wheelDiameter, float wheelSeparation) { @@ -21,51 +40,66 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter, this->rightWheel->SetPosition(Spherical(distance, Direction::right)); } -void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) { +// Motor& DifferentialDrive::GetMotorLeft() { +// return *this->leftWheel; +// } + +// Motor& DifferentialDrive::GetMotorRight() { +// return *this->rightWheel; +// } + +void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) { float distance = this->wheelSeparation / 2; - this->leftWheel = leftWheel; - ; - if (leftWheel != nullptr) - this->leftWheel->SetPosition(Spherical(distance, Direction::left)); + this->leftWheel = &leftMotor; + this->leftWheel->SetPosition(Spherical(distance, Direction::left)); - this->rightWheel = rightWheel; - if (rightWheel != nullptr) - this->rightWheel->SetPosition(Spherical(distance, Direction::right)); + this->rightWheel = &rightMotor; + this->rightWheel->SetPosition(Spherical(distance, Direction::right)); } -void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) { +void DifferentialDrive::SetWheelVelocity(float velocityLeft, + float velocityRight) { + // if (this->leftWheel != nullptr) + // this->leftWheel->SetAngularVelocity(Spherical(velocityLeft, + // Direction::left)); + // if (this->rightWheel != nullptr) + // this->rightWheel->SetAngularVelocity( + // Spherical(velocityRight, Direction::right)); if (this->leftWheel != nullptr) - this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left)); + this->leftWheel->SetTargetVelocity(velocityLeft); if (this->rightWheel != nullptr) - this->rightWheel->SetAngularVelocity( - Spherical(speedRight, Direction::right)); + this->rightWheel->SetTargetVelocity(velocityRight); } -void DifferentialDrive::Update(unsigned long currentMs, bool recursive) { - if (this->linearVelocityUpdated == false) - return; - // this assumes forward velocity only.... - float linearVelocity = this->GetLinearVelocity().distance; +void DifferentialDrive::Update(bool recursive) { + if (this->linearVelocityUpdated) { + // this assumes forward velocity only.... + float linearVelocity = this->GetLinearVelocity().distance; - Spherical angularVelocity = this->GetAngularVelocity(); - float angularSpeed = angularVelocity.distance * Deg2Rad; // in degrees/sec - // Determine the rotation direction - if (angularVelocity.direction.horizontal.InDegrees() < 0) - angularSpeed = -angularSpeed; + Spherical angularVelocity = this->GetAngularVelocity(); + float angularSpeed = angularVelocity.distance * Deg2Rad; // in degrees/sec + // Determine the rotation direction + if (angularVelocity.direction.horizontal.InDegrees() < 0) + angularSpeed = -angularSpeed; - // wheel separation can be replaced by this->leftwheel->position->distance - float speedLeft = - (linearVelocity + angularSpeed * this->wheelSeparation / 2) / - this->wheelRadius * Rad2Deg; - float speedRight = - (linearVelocity - angularSpeed * this->wheelSeparation / 2) / - this->wheelRadius * Rad2Deg; + // wheel separation can be replaced by this->leftwheel->position->distance + float speedLeft = + (linearVelocity + angularSpeed * this->wheelSeparation / 2) / + this->wheelRadius * Rad2Deg; + float speedRight = + (linearVelocity - angularSpeed * this->wheelSeparation / 2) / + this->wheelRadius * Rad2Deg; - this->SetWheelVelocity(speedLeft, speedRight); - Thing::Update(currentMs, recursive); - // std::cout << "lin. speed " << linearVelocity << " ang. speed " << - // angularVelocity.distance << " left wheel " - // << speedLeft << " right wheel " << speedRight << "\n"; + this->SetWheelVelocity(speedLeft, speedRight); + } + 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 \ No newline at end of file diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h index 03e6318..cb45977 100644 --- a/Things/DifferentialDrive.h +++ b/Things/DifferentialDrive.h @@ -1,6 +1,7 @@ #pragma once #include "Thing.h" +#include "Motor.h" namespace RoboidControl { @@ -9,11 +10,13 @@ namespace RoboidControl { /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink class DifferentialDrive : public Thing { public: - /// @brief Create a differential drive without networking support - DifferentialDrive(); - /// @brief Create a differential drive with networking support - /// @param participant The local participant - DifferentialDrive(Participant* participant); + /// @brief Create a new child differential drive + /// @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(Motor* leftMotor, Motor* rightMotor, Thing* parent = Thing::LocalRoot()); /// @brief Configures the dimensions of the drive /// @param wheelDiameter The diameter of the wheels in meters @@ -23,35 +26,41 @@ class DifferentialDrive : public Thing { /// linear and angular velocity. /// @sa SetLinearVelocity SetAngularVelocity void SetDriveDimensions(float wheelDiameter, float wheelSeparation); + + // 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 - void SetMotors(Thing* leftWheel, Thing* rightWheel); + void SetMotors(Motor& leftMotor, Motor& rightMotor); /// @brief Directly specify the speeds of the motors /// @param speedLeft The speed of the left wheel in degrees per second. /// Positive moves the robot in the forward direction. /// @param speedRight The speed of the right wheel in degrees per second. /// Positive moves the robot in the forward direction. - void SetWheelVelocity(float speedLeft, float speedRight); /// @copydoc RoboidControl::Thing::Update(unsigned long) - virtual void Update(unsigned long currentMs, bool recursive = true) override; + virtual void Update(bool recursive = true) override; + + 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 = 1.0f; + float wheelRadius = 0.0f; /// @brief The distance between the wheels in meters - float wheelSeparation = 1.0f; + float wheelSeparation = 0.0f; /// @brief Convert revolutions per second to meters per second float rpsToMs = 1.0f; - /// @brief The left wheel - Thing* leftWheel = nullptr; - /// @brief The right wheel - Thing* rightWheel = nullptr; }; } // namespace RoboidControl \ No newline at end of file diff --git a/Things/DigitalSensor.cpp b/Things/DigitalSensor.cpp index 3f460cb..35b9190 100644 --- a/Things/DigitalSensor.cpp +++ b/Things/DigitalSensor.cpp @@ -2,8 +2,17 @@ namespace RoboidControl { -//DigitalSensor::DigitalSensor() {} +DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent) { + this->type = Type::Switch; +} -DigitalSensor::DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId) : Thing(participant) {} +int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) { + bytes[(*ix)++] = state ? 1 : 0; + return 1; +} + +void DigitalSensor::ProcessBinary(char* bytes) { + this->state |= (bytes[0] == 1); +} } // namespace RoboidControl diff --git a/Things/DigitalSensor.h b/Things/DigitalSensor.h index 5e4bc8a..e31e87d 100644 --- a/Things/DigitalSensor.h +++ b/Things/DigitalSensor.h @@ -7,15 +7,31 @@ namespace RoboidControl { /// @brief A digital (on/off, 1/0, true/false) sensor class DigitalSensor : public Thing { public: + /// @brief Create a digital sensor without communication abilities + //DigitalSensor(); + /// @brief Create a digital sensor for a participant + /// @param owner The owning participant + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + DigitalSensor(Participant* owner = nullptr); //, unsigned char thingId = 0); + /// @brief Create a new child digital sensor + /// @param parent The parent 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()); + /// @brief The sigital state bool state = 0; - /// @brief The default constructor - //DigitalSensor(); - /// @brief Create a temperature sensor with the given ID - /// @param networkId The network ID of the sensor - /// @param thingId The ID of the thing - DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId); + /// @brief Function used to generate binary data for this digital sensor + /// @param buffer The byte array for thw binary data + /// @param ix The starting position for writing the binary data + int GenerateBinary(char* bytes, unsigned char* ix) override; + /// @brief Function used to process binary data received for this digital + /// sensor + /// @param bytes The binary data to process + virtual void ProcessBinary(char* bytes) override; }; } // namespace RoboidControl diff --git a/Things/DistanceSensor.cpp b/Things/DistanceSensor.cpp new file mode 100644 index 0000000..c33d64a --- /dev/null +++ b/Things/DistanceSensor.cpp @@ -0,0 +1,29 @@ +#include "DistanceSensor.h" + +#include "Messages/LowLevelMessages.h" + +namespace RoboidControl { + +DistanceSensor::DistanceSensor(Thing* parent) : Thing(parent) { + this->type = Type::DistanceSensor; + this->name = "Distance sensor"; +} + +float DistanceSensor::GetDistance() { + if (this->externalDistance < this->internalDistance) + return this->externalDistance; + else + return this->internalDistance; +} + +int DistanceSensor::GenerateBinary(char* bytes, unsigned char* ix) { + LowLevelMessages::SendFloat16(bytes, ix, this->internalDistance); + return *ix; +} + +void DistanceSensor::ProcessBinary(char* bytes) { + unsigned char ix = 0; + this->externalDistance = LowLevelMessages::ReceiveFloat16(bytes, &ix); +} + +} // namespace RoboidControl \ No newline at end of file diff --git a/Things/DistanceSensor.h b/Things/DistanceSensor.h new file mode 100644 index 0000000..08e324d --- /dev/null +++ b/Things/DistanceSensor.h @@ -0,0 +1,41 @@ +#pragma once + +#if !NO_STD +#include +#endif + +#include "Thing.h" + +namespace RoboidControl { + +/// @brief A sensor measuring distance +class DistanceSensor : public Thing { + public: + /// @brief Create a new child touch sensor + /// @param parent The parent thing + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + DistanceSensor(Thing* parent = Thing::LocalRoot()); + + /// @brief Get the current distance + float GetDistance(); + + /// @brief Function used to generate binary data for this sensor + /// @param buffer The byte array for thw binary data + /// @param ix The starting position for writing the binary data + int GenerateBinary(char* bytes, unsigned char* ix) override; + /// @brief Function used to process binary data received for this sensor + /// @param bytes The binary data to process + virtual void ProcessBinary(char* bytes) override; + + protected: +#if ARDUNIO + float internalDistance = INFINITY; + float externalDistance = INFINITY; +#else + float internalDistance = std::numeric_limits::infinity(); + float externalDistance = std::numeric_limits::infinity(); +#endif +}; + +} // namespace RoboidControl diff --git a/Things/Motor.cpp b/Things/Motor.cpp new file mode 100644 index 0000000..c80e82e --- /dev/null +++ b/Things/Motor.cpp @@ -0,0 +1,34 @@ +#include "Motor.h" + +#include "Messages/BinaryMsg.h" +#include "Participant.h" + +namespace RoboidControl { + +Motor::Motor(Thing* parent) : Thing(parent) { + this->type = Type::UncontrolledMotor; +} + +void Motor::SetTargetVelocity(float targetSpeed) { + if (targetSpeed != this->targetVelocity) { + this->targetVelocity = targetSpeed; + + if (this->owner->networkId != 0) { + // in other word: if we are connected... + BinaryMsg* binaryMsg = new BinaryMsg(this->owner->networkId, this); + this->owner->Send(binaryMsg); + delete binaryMsg; + } + } +} + +float Motor::GetTargetVelocity() { + return this->targetVelocity; +} + +int Motor::GenerateBinary(char* data, unsigned char* ix) { + data[(*ix)++] = this->targetVelocity * 127.0f; + return 1; +} + +} // namespace RoboidControl \ No newline at end of file diff --git a/Things/Motor.h b/Things/Motor.h new file mode 100644 index 0000000..0332289 --- /dev/null +++ b/Things/Motor.h @@ -0,0 +1,25 @@ +#pragma once + +#include "Thing.h" + +namespace RoboidControl { + +class Motor : public Thing { + public: + Motor(Thing* parent = Thing::LocalRoot()); + + /// @brief Motor turning direction + enum class Direction { Clockwise = 1, CounterClockwise = -1 }; + /// @brief The forward turning direction of the motor + Direction direction; + + virtual void SetTargetVelocity(float velocity); // -1..0..1 + virtual float GetTargetVelocity(); + + int GenerateBinary(char* bytes, unsigned char* ix) override; + + protected: + float targetVelocity = 0; +}; + +} // namespace RoboidControl \ No newline at end of file diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp new file mode 100644 index 0000000..4fbef8c --- /dev/null +++ b/Things/RelativeEncoder.cpp @@ -0,0 +1,22 @@ +#include "RelativeEncoder.h" + +namespace RoboidControl { + +RelativeEncoder::RelativeEncoder(Thing* parent) : Thing(parent) { + this->type = Type::RelativeEncoder; +} + +float RelativeEncoder::GetRotationSpeed() { + return rotationSpeed; +} + +int RelativeEncoder::GenerateBinary(char* data, unsigned char* ix) { + data[(*ix)++] = (unsigned char)(this->rotationSpeed * 127); + return 1; +} + +void RelativeEncoder::ProcessBinary(char* data) { + this->rotationSpeed = (float)data[0] / 127; +} + +} // namespace RoboidControl diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h new file mode 100644 index 0000000..350d8a5 --- /dev/null +++ b/Things/RelativeEncoder.h @@ -0,0 +1,39 @@ +#pragma once + +#include "Thing.h" + +namespace RoboidControl { + +/// @brief An Incremental Encoder measures the rotations of an axle using a +/// rotary sensor. Some encoders are able to detect direction, while others can +/// not. +class RelativeEncoder : public Thing { + public: + /// @brief Creates a sensor which measures distance from pulses + /// @param pulsesPerRevolution The number of pulse edges which make a + /// full rotation + /// @param distancePerRevolution The distance a wheel travels per full + /// rotation + //RelativeEncoder(Participant* owner); + // RelativeEncoder(Thing* parent); + RelativeEncoder(Thing* parent = Thing::LocalRoot()); + + /// @brief Get the rotation speed + /// @return The speed in revolutions per second + virtual float GetRotationSpeed(); + float rotationSpeed = 0; + + /// @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 + int GenerateBinary(char* bytes, unsigned char* ix) override; + /// @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: + /// @brief rotation speed in revolutions per second + //float _rotationSpeed; +}; + +} // namespace RoboidControl diff --git a/Things/TemperatureSensor.cpp b/Things/TemperatureSensor.cpp index 45718a5..39ecc41 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -4,9 +4,9 @@ namespace RoboidControl { -TemperatureSensor::TemperatureSensor(Participant* participant, - unsigned char thingId) - : Thing(participant, Type::TemperatureSensor, thingId) {} +TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent) { + this->type = Type::TemperatureSensor; +} void TemperatureSensor::SetTemperature(float temp) { this->temperature = temp; diff --git a/Things/TemperatureSensor.h b/Things/TemperatureSensor.h index 8d58d9c..0da4210 100644 --- a/Things/TemperatureSensor.h +++ b/Things/TemperatureSensor.h @@ -7,15 +7,17 @@ namespace RoboidControl { /// @brief A temperature sensor class TemperatureSensor : public Thing { public: - /// @brief The measured temperature - float temperature = 0; - /// @brief The default constructor //TemperatureSensor(); /// @brief Create a temperature sensor with the given ID /// @param networkId The network ID of the sensor /// @param thingId The ID of the thing - TemperatureSensor(Participant* participant, unsigned char thingId); + TemperatureSensor(Participant* participant); //, unsigned char thingId); + // TemperatureSensor(Thing* parent); + TemperatureSensor(Thing* parent = Thing::LocalRoot()); + + /// @brief The measured temperature + float temperature = 0; /// @brief Manually override the measured temperature /// @param temperature The new temperature diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index b5743f6..e5a593b 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -1,23 +1,36 @@ #include "TouchSensor.h" namespace RoboidControl { -TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {} -TouchSensor::TouchSensor(Participant* participant) - : Thing(participant, Thing::Type::TouchSensor) {} +TouchSensor::TouchSensor(Thing* parent) : Thing(parent) { + this->type = Type::TouchSensor; + this->name = "Touch sensor"; +} -TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) { - this->SetParent(parent); +bool TouchSensor::IsTouching() { + return this->internalTouch || this->externalTouch; +} + +// void TouchSensor::PrepareForUpdate() { +// //this->internalTouch = this->externalTouch; +// } + +void TouchSensor::Update(bool recursive) { + Thing::Update(recursive); } int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { - return 0; + std::cout << "BinaryMsg Touch " << this->internalTouch << std::endl; + bytes[(*ix)++] = this->internalTouch ? 1 : 0; + return 1; } void TouchSensor::ProcessBinary(char* bytes) { - // if (bytes[0] == 1) - // std::cout << this->name << " is Touching something!\n"; - 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 \ No newline at end of file diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index 39638e7..bdc5279 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -6,29 +6,31 @@ namespace RoboidControl { /// @brief A sensor which can detect touches class TouchSensor : public Thing { +// When finishing this release (0.3), I notice that this is equivalent to a digital sensor public: - /// @brief Create a touch sensor with isolated participant - TouchSensor(); - /// @brief Create a touch sensor - TouchSensor(Participant* participant); - /// @brief Create a temperature sensor with the given ID - /// @param networkId The network ID of the sensor - /// @param thingId The ID of the thing - TouchSensor(Thing* parent); - // TouchSensor(RemoteParticipant* participant, unsigned char networkId, - // unsigned char thingId); + /// @brief Create a new child touch sensor + /// @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()); /// @brief Value which is true when the sensor is touching something, false /// otherwise - bool touchedSomething = false; + bool IsTouching(); - /// @brief Function to create a binary message with the temperature + //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 int GenerateBinary(char* bytes, unsigned char* ix) override; - /// @brief Function to extract the temperature received in the binary message - /// @param bytes The binary data + /// @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; + bool internalTouch = false; }; } // namespace RoboidControl diff --git a/Windows/WindowsParticipant.cpp b/Windows/WindowsParticipant.cpp index d64eb1e..c6e99db 100644 --- a/Windows/WindowsParticipant.cpp +++ b/Windows/WindowsParticipant.cpp @@ -85,9 +85,9 @@ void ParticipantUDP::Receive() { unsigned int sender_port = ntohs(client_addr.sin_port); ReceiveData(packetSize, sender_ipAddress, sender_port); - // RoboidControl::ParticipantUDP* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port); + // RoboidControl::ParticipantUDP* remoteParticipant = this->Get(sender_ipAddress, sender_port); // if (remoteParticipant == nullptr) { - // remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port); + // remoteParticipant = this->Add(sender_ipAddress, sender_port); // // std::cout << "New sender " << sender_ipAddress << ":" // // << sender_port << "\n"; // // std::cout << "New remote participant " << diff --git a/Examples/BB2B.cpp b/examples/BB2A/main.cpp similarity index 68% rename from Examples/BB2B.cpp rename to examples/BB2A/main.cpp index 1dad561..ef8f269 100644 --- a/Examples/BB2B.cpp +++ b/examples/BB2A/main.cpp @@ -17,32 +17,32 @@ using namespace RoboidControl; int main() { // The robot's propulsion is a differential drive - DifferentialDrive* bb2b = new DifferentialDrive(); + DifferentialDrive bb2b = DifferentialDrive(); // Is has a touch sensor at the front left of the roboid - TouchSensor* touchLeft = new TouchSensor(bb2b); + TouchSensor touchLeft = TouchSensor(&bb2b); // and other one on the right - TouchSensor* touchRight = new TouchSensor(bb2b); + TouchSensor touchRight = TouchSensor(&bb2b); // Do forever: while (true) { // The left wheel turns forward when nothing is touched on the right side // and turn backward when the roboid hits something on the right - float leftWheelSpeed = (touchRight->touchedSomething) ? -600.0f : 600.0f; + float leftWheelSpeed = (touchRight.IsTouching()) ? -600.0f : 600.0f; // The right wheel does the same, but instead is controlled by // touches on the left side - float rightWheelSpeed = (touchLeft->touchedSomething) ? -600.0f : 600.0f; + float rightWheelSpeed = (touchLeft.IsTouching()) ? -600.0f : 600.0f; // When both sides are touching something, both wheels will turn backward // and the roboid will move backwards - bb2b->SetWheelVelocity(leftWheelSpeed, rightWheelSpeed); + bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed); // Update the roboid state - bb2b->Update(true); + bb2b.Update(true); // and sleep for 100ms #if defined(ARDUINO) - delay(100); + delay(10); #else - sleep_for(milliseconds(100)); + sleep_for(milliseconds(10)); #endif } diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 0000000..0cd9d2a --- /dev/null +++ b/examples/CMakeLists.txt @@ -0,0 +1,8 @@ +# examples/CMakeLists.txt + +# Check if the options are enabled and add the corresponding examples +if(BUILD_EXAMPLE_BB2A) + add_executable(BB2A BB2A/main.cpp) # Adjust the path as necessary + + target_link_libraries(BB2A RoboidControl) +endif() diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 0000000..bfba240 --- /dev/null +++ b/examples/README.md @@ -0,0 +1 @@ +Important: this folder has to be named 'examples' exactly to maintain compatibility with Arduino \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..99ee89c --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,30 @@ +# Unit test configuration + +add_compile_definitions(GTEST) +include(FetchContent) +FetchContent_Declare( + googletest + DOWNLOAD_EXTRACT_TIMESTAMP ON + URL https://github.com/google/googletest/archive/refs/heads/main.zip +) + +# For Windows: Prevent overriding the parent project's compiler/linker settings +set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) +FetchContent_MakeAvailable(googletest) + +file(GLOB_RECURSE test_srcs *.cc) +message(STATUS "Test sources: ${test_srcs}") +add_executable( + RoboidControlTest + ${test_srcs} +) +message(STATUS "RoboidControlTest target created") +target_link_libraries( + RoboidControlTest + gtest_main + RoboidControl + LinearAlgebra +) + +include(GoogleTest) +gtest_discover_tests(RoboidControlTest) \ No newline at end of file diff --git a/test/participant_test.cc b/test/participant_test.cc index c826863..13831bd 100644 --- a/test/participant_test.cc +++ b/test/participant_test.cc @@ -4,111 +4,75 @@ // not supported using Visual Studio 2022 compiler... #include -#include -#include -// #include - -#include "Participant.h" #include "Participants/SiteServer.h" #include "Thing.h" +#include +#include + using namespace RoboidControl; -// Function to get the current time in milliseconds as unsigned long -unsigned long get_time_ms() { - auto now = std::chrono::steady_clock::now(); - auto ms = std::chrono::duration_cast( - now.time_since_epoch()); - return static_cast(ms.count()); +TEST(Participant, Participant) { + ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7682); + + unsigned long milliseconds = Thing::GetTimeMs(); + unsigned long startTime = milliseconds; + while (milliseconds < startTime + 7000) { + participant->Update(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + milliseconds = Thing::GetTimeMs(); + } + SUCCEED(); } -// class RoboidControlSuite : public ::testing::test { -// TEST_F(RoboidControlSuite, HiddenParticipant) { -// Thing thing = Thing(); - -// unsigned long milliseconds = get_time_ms(); -// unsigned long startTime = milliseconds; -// while (milliseconds < startTime + 1000) { -// Thing.Update(milliseconds); - -// milliseconds = get_time_ms(); -// } -// ASSERT_EQ(1, 1); -// } -// } +TEST(Participant, SiteServer) { + SiteServer* siteServer = new SiteServer(); -class ParticipantSuite : public ::testing::Test { -protected: - // SetUp and TearDown can be used to set up and clean up before/after each - // test - void SetUp() override { - // Initialize test data here + unsigned long milliseconds = Thing::GetTimeMs(); + unsigned long startTime = milliseconds; + while (milliseconds < startTime + 7000) { + siteServer->Update(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + milliseconds = Thing::GetTimeMs(); } + SUCCEED(); +} - void TearDown() override { - // Clean up test data here +TEST(Participant, SiteParticipant) { + SiteServer* siteServer = new SiteServer(7681); + ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682); + + unsigned long milliseconds = Thing::GetTimeMs(); + unsigned long startTime = milliseconds; + while (milliseconds < startTime + 7000) { + siteServer->Update(); + participant->Update(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + milliseconds = Thing::GetTimeMs(); } -}; + SUCCEED(); +} +TEST(Participant, ThingMsg) { + SiteServer* siteServer = new SiteServer(7681); + ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682); + Thing* thing = new Thing(); + thing->SetName("First Thing"); + thing->SetModel("https://passer.life/extras/ant.jpg"); + + unsigned long milliseconds = Thing::GetTimeMs(); + unsigned long startTime = milliseconds; + while (milliseconds < startTime + 7000) { + siteServer->Update(); + participant->Update(); - -// TEST_F(ParticipantSuite, ParticipantUDP) { -// ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681); - -// unsigned long milliseconds = get_time_ms(); -// unsigned long startTime = milliseconds; -// while (milliseconds < startTime + 1000) { -// participant->Update(milliseconds); - -// milliseconds = get_time_ms(); -// } -// ASSERT_EQ(1, 1); -// } - -// TEST_F(ParticipantSuite, SiteServer) { -// SiteServer site = SiteServer(7681); - -// unsigned long milliseconds = get_time_ms(); -// unsigned long startTime = milliseconds; -// while (milliseconds < startTime + 1000) { -// site.Update(milliseconds); - -// milliseconds = get_time_ms(); -// } -// ASSERT_EQ(1, 1); -// } - -// TEST_F(ParticipantSuite, SiteParticipant) { -// SiteServer site = SiteServer(7681); -// ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681); - -// unsigned long milliseconds = get_time_ms(); -// unsigned long startTime = milliseconds; -// while (milliseconds < startTime + 1000) { -// site.Update(milliseconds); -// participant.Update(milliseconds); - -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); -// milliseconds = get_time_ms(); -// } -// ASSERT_EQ(1, 1); -// } - -// TEST_F(ParticipantSuite, Thing) { -// SiteServer site = SiteServer(7681); -// ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681); -// Thing thing = Thing(&participant); - -// unsigned long milliseconds = get_time_ms(); -// unsigned long startTime = milliseconds; -// while (milliseconds < startTime + 1000) { -// site.Update(milliseconds); -// participant.Update(milliseconds); - -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); -// milliseconds = get_time_ms(); -// } -// ASSERT_EQ(1, 1); -// } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + milliseconds = Thing::GetTimeMs(); + } + SUCCEED(); +} #endif diff --git a/test/thing_test.cc b/test/thing_test.cc index b8ceea1..9414754 100644 --- a/test/thing_test.cc +++ b/test/thing_test.cc @@ -15,7 +15,7 @@ TEST(RoboidControlSuite, HiddenParticipant) { unsigned long milliseconds = Thing::GetTimeMs(); unsigned long startTime = milliseconds; while (milliseconds < startTime + 1000) { - Thing::UpdateThings(milliseconds); + thing->Update(); milliseconds = Thing::GetTimeMs(); } @@ -23,13 +23,13 @@ TEST(RoboidControlSuite, HiddenParticipant) { } TEST(RoboidControlSuite, IsolatedParticipant) { - ParticipantUDP* participant = ParticipantUDP::Isolated(); - Thing* thing = new Thing(participant); + ParticipantUDP* participant = new ParticipantUDP(0); + Thing* thing = new Thing(); unsigned long milliseconds = Thing::GetTimeMs(); unsigned long startTime = milliseconds; while (milliseconds < startTime + 1000) { - participant->Update(milliseconds); + participant->Update(); milliseconds = Thing::GetTimeMs(); }