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

Compare commits

base: RoboidControl:main
Branches Tags
RoboidControl:main
RoboidControl:Braccio
RoboidControl:ESP-IDF
RoboidControl:ControlCore
RoboidControl:Experimental
RoboidControl:subtree
RoboidControl:edition/arduino
RoboidControl:lighthousedeck
RoboidControl:hierarchy-support
RoboidControl:Quadcopter2
RoboidControl:DiscreteAngle
RoboidControl:master
RoboidControl:v0.3.0
RoboidControl:v0.2.1
RoboidControl:v0.1
RoboidControl:v0.2
..
compare: RoboidControl:v0.3.0
Branches Tags
RoboidControl:main
RoboidControl:Braccio
RoboidControl:ESP-IDF
RoboidControl:ControlCore
RoboidControl:Experimental
RoboidControl:subtree
RoboidControl:edition/arduino
RoboidControl:lighthousedeck
RoboidControl:hierarchy-support
RoboidControl:Quadcopter2
RoboidControl:DiscreteAngle
RoboidControl:master
RoboidControl:v0.3.0
RoboidControl:v0.2.1
RoboidControl:v0.1
RoboidControl:v0.2

No commits in common. "main" and "v0.3.0" have entirely different histories.
main ... v0.3.0

49 changed files with 372 additions and 986 deletions
Show Stats Expand all files Collapse all files

32
Arduino/ArduinoParticipant.cpp
View File

@ -1,9 +1,5 @@
#include "ArduinoParticipant.h"
#if !defined(NO_STD)
#include <iostream>
#endif
#if defined(ARDUINO)
#if defined(ARDUINO_ARCH_ESP8266)
#include <ESP8266WiFi.h>
@ -33,9 +29,7 @@ void ParticipantUDP::Setup() {
#if defined(UNO_R4)
if (WiFi.status() == WL_NO_MODULE) {
#if !defined(NO_STD)
std::cout << "No network available!\n";
#endif
return;
}
#else
@ -48,13 +42,11 @@ void ParticipantUDP::Setup() {
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
}
void ParticipantUDP::GetBroadcastAddress() {
@ -65,10 +57,8 @@ 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() {
@ -82,6 +72,19 @@ void ParticipantUDP::Receive() {
senderAddress.toCharArray(sender_ipAddress, 16);
unsigned int sender_port = udp->remotePort();
// Participant* remoteParticipant = this->Get(sender_ipAddress,
// sender_port); if (remoteParticipant == nullptr) {
// remoteParticipant = this->Add(sender_ipAddress,
// sender_port);
// // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
// // << "\n";
// // std::cout << "New remote participant " <<
// remoteParticipant->ipAddress
// // << ":" << remoteParticipant->port << " "
// // << (int)remoteParticipant->networkId << "\n";
// }
// ReceiveData(packetSize, remoteParticipant);
ReceiveData(packetSize, sender_ipAddress, sender_port);
packetSize = udp->parsePacket();
}
@ -94,22 +97,15 @@ 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);
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);
} while (udp->endPacket() == 0 && n < 10);
#endif
return true;

104
Arduino/Things/DRV8833.cpp
View File

@ -5,39 +5,6 @@
namespace RoboidControl {
namespace Arduino {
#pragma region DRV8833
DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) {
this->pinStandby = config.standby;
if (pinStandby != 255)
pinMode(pinStandby, OUTPUT);
this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
this->motorA->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(this->drv8833.motorA,
this->drv8833.motorB,
parent),
drv8833(config, this) {}
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
@ -46,7 +13,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
unsigned char pinIn1,
unsigned char pinIn2,
bool reverse)
: Motor() {
: Thing(driver->owner) {
this->SetParent(driver);
this->pinIn1 = pinIn1;
@ -66,20 +33,33 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
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::SetTargetVelocity(float motorSpeed) {
Motor::SetTargetVelocity(motorSpeed);
void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
Thing::SetAngularVelocity(velocity);
// ignoring rotation axis for now.
// Spherical angularVelocity = this->GetAngularVelocity();
float angularSpeed = velocity.distance; // in degrees/sec
uint8_t motorSignal =
(uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);
float rpm = angularSpeed / 360 * 60;
float motorSpeed = rpm / this->maxRpm;
// std::cout << "moto speed " << this->name << " = " << motorSpeed
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
// << ", motor signal = " << (int)motorSignal << "\n";
#if (ESP32)
@ -129,7 +109,43 @@ void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
#endif
}
#pragma endregion Motor
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(this, pinAIn1, pinAIn2, reverseA);
this->motorA->SetName("Motor A");
this->motorB = new DRV8833Motor(this, pinBIn1, pinBIn2, reverseB);
this->motorB->SetName("Motor B");
}
DRV8833::DRV8833(Thing* parent,
unsigned char pinAIn1,
unsigned char pinAIn2,
unsigned char pinBIn1,
unsigned char pinBIn2,
unsigned char pinStandby,
bool reverseA,
bool reverseB)
: DRV8833(parent->owner,
pinAIn1,
pinAIn2,
pinBIn1,
pinBIn2,
pinStandby,
reverseA,
reverseB) {
this->SetParent(parent);
}
} // namespace Arduino
} // namespace RoboidControl

72
Arduino/Things/DRV8833.h
View File

@ -1,10 +1,8 @@
#pragma once
#include <Arduino.h>
#include "Participants/IsolatedParticipant.h"
#include "Thing.h"
#include "Things/DifferentialDrive.h"
#include "Things/Motor.h"
namespace RoboidControl {
namespace Arduino {
@ -13,46 +11,44 @@ class DRV8833Motor;
class DRV8833 : public Thing {
public:
struct Configuration {
int AIn1;
int AIn2;
int BIn1;
int BIn2;
int standby;
};
/// @brief Setup a DRV8833 motor controller
DRV8833(Configuration config, Thing* parent = Thing::LocalRoot());
/// @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(Thing* parent,
unsigned char pinAIn1,
unsigned char pinAIn2,
unsigned char pinBIn1,
unsigned char pinBIn2,
unsigned char pinStandby = 255,
bool reverseA = false,
bool reverseB = false);
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;
};
#pragma endregion Differential drive
#pragma region Motor
/// @brief Support for a DRV8833 motor controller
class DRV8833Motor : public Motor {
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
@ -61,16 +57,16 @@ class DRV8833Motor : public Motor {
unsigned char pinIn1,
unsigned char pinIn2,
bool reverse = false);
// void SetMaxRPM(unsigned int rpm);
void SetMaxRPM(unsigned int rpm);
// virtual void SetAngularVelocity(Spherical velocity) override;
virtual void SetTargetVelocity(float targetSpeed) override;
// bool reverse = false;
virtual void SetAngularVelocity(Spherical velocity) override;
bool reverse = false;
protected:
unsigned char pinIn1 = 255;
unsigned char pinIn2 = 255;
// unsigned int maxRpm = 200;
unsigned int maxRpm = 200;
#if (ESP32)
uint8_t in1Ch;
@ -79,7 +75,5 @@ class DRV8833Motor : public Motor {
#endif
};
#pragma endregion Motor
} // namespace Arduino
} // namespace RoboidControl

118
Arduino/Things/DigitalInput.cpp
View File

@ -5,119 +5,19 @@
namespace RoboidControl {
namespace Arduino {
#pragma region Digital input
DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(Type::Undetermined, parent) {
DigitalInput::DigitalInput(Participant* participant, unsigned char pin)
: TouchSensor(participant) {
this->pin = pin;
pinMode(this->pin, INPUT);
std::cout << "digital input start\n";
pinMode(pin, INPUT);
}
void DigitalInput::Update(bool recursive) {
this->isHigh = digitalRead(this->pin);
this->isLow = !this->isHigh;
Thing::Update(recursive);
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);
}
#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->touchedSomething = 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)
attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0,
RISING);
else if (interruptCount == 1)
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;
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

74
Arduino/Things/DigitalInput.h
View File

@ -1,92 +1,26 @@
#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 Thing {
class DigitalInput : public TouchSensor {
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(Thing* parent, unsigned char pin);
DigitalInput(unsigned char pin, Thing* parent);
bool isHigh = false;
bool isLow = false;
DigitalInput(Participant* participant, unsigned char pin);
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(bool recursive = false) override;
virtual void Update(unsigned long currentTimeMs,
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

61
Arduino/Things/UltrasonicSensor.cpp
View File

@ -1,32 +1,39 @@
#include "UltrasonicSensor.h"
#include <Arduino.h>
#include <iostream>
namespace RoboidControl {
namespace Arduino {
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
: Thing(Type::Undetermined, parent) {
this->name = "Ultrasonic sensor";
this->pinTrigger = config.triggerPin;
this->pinEcho = config.echoPin;
UltrasonicSensor::UltrasonicSensor(Participant* participant,
unsigned char pinTrigger,
unsigned char pinEcho)
: TouchSensor(participant) {
this->pinTrigger = pinTrigger;
this->pinEcho = pinEcho;
pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode
pinMode(pinEcho, INPUT); // configure the echo pin to input mode
}
UltrasonicSensor::UltrasonicSensor(Thing* parent,
unsigned char pinTrigger,
unsigned char pinEcho)
: UltrasonicSensor(parent->owner, pinTrigger, pinEcho) {
this->SetParent(parent);
}
float UltrasonicSensor::GetDistance() {
// Start the ultrasonic 'ping'
digitalWrite(pinTrigger, LOW);
delayMicroseconds(2);
delayMicroseconds(5);
digitalWrite(pinTrigger, HIGH);
delayMicroseconds(10);
digitalWrite(pinTrigger, LOW);
// Measure the duration of the pulse on the echo pin
unsigned long duration_us =
pulseIn(pinEcho, HIGH, 10000); // the result is in microseconds
float duration_us =
pulseIn(pinEcho, HIGH, 100000); // the result is in microseconds
// Calculate the distance:
// * Duration should be divided by 2, because the ping goes to the object
@ -37,38 +44,32 @@ 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 = (float)duration_us / 2 / 1000000 * 340;
// Serial.println(this->distance);
// std::cout << "US distance " << this->distance << std::endl;
this->distance = duration_us / 2 / 1000000 * 340;
// Filter faulty measurements. The sensor can often give values > 30 m which
// are not correct
// if (distance > 30)
// distance = 0;
return this->distance;
this->touchedSomething |= (this->distance > 0 && this->distance <= this->touchDistance);
// std::cout << "Ultrasonic " << this->touchedSomething << " | " << (this->distance > 0) << " " <<
// (this->distance <= this->touchDistance) << "\n";
return distance;
}
void UltrasonicSensor::Update(bool recursive) {
void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
this->touchedSomething = false;
GetDistance();
Thing::Update(recursive);
Thing::Update(currentTimeMs, recursive);
}
#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->touchedSomething |= (this->ultrasonic.distance > 0 &&
this->ultrasonic.distance <= this->touchDistance);
}
#pragma region Touch sensor
// void UltrasonicSensor::ProcessBinary(char* bytes) {
// this->touchedSomething = (bytes[0] == 1);
// if (this->touchedSomething)
// std::cout << "Touching something!\n";
// }
} // namespace Arduino
} // namespace RoboidControl

45
Arduino/Things/UltrasonicSensor.h
View File

@ -6,19 +6,26 @@ namespace RoboidControl {
namespace Arduino {
/// @brief An HC-SR04 ultrasonic distance sensor
class UltrasonicSensor : Thing {
class UltrasonicSensor : public TouchSensor {
public:
struct Configuration {
int triggerPin;
int echoPin;
};
// Inherit all constructors
//using TouchSensor::TouchSensor;
UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());
/// @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);
UltrasonicSensor(Thing* parent,
unsigned char pinTrigger,
unsigned char pinEcho);
// parameters
/// @brief The distance at which the object is considered to be touched
// float touchDistance = 0.2f;
float touchDistance = 0.2f;
// state
@ -30,35 +37,15 @@ class UltrasonicSensor : Thing {
float GetDistance();
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(bool recursive = false) override;
virtual void Update(unsigned long currentTimeMs,
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 TouchSensor;
};
#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 region Touch sensor
} // namespace Arduino
} // namespace RoboidControl

6
CMakeLists.txt
View File

@ -19,13 +19,13 @@ if(ESP_PLATFORM)
REQUIRES esp_netif esp_wifi
)
else()
set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
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(

14
examples/BB2B.cpp → Examples/BB2B.cpp
View File

@ -17,26 +17,26 @@ using namespace RoboidControl;
int main() {
// The robot's propulsion is a differential drive
DifferentialDrive bb2b = DifferentialDrive();
DifferentialDrive* bb2b = new DifferentialDrive();
// Is has a touch sensor at the front left of the roboid
TouchSensor touchLeft = TouchSensor(bb2b);
TouchSensor* touchLeft = new TouchSensor(bb2b);
// and other one on the right
TouchSensor touchRight = TouchSensor(bb2b);
TouchSensor* touchRight = new 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->touchedSomething) ? -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->touchedSomething) ? -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)

0
examples/CMakeLists.txt → Examples/CMakeLists.txt
View File

6
LinearAlgebra/Direction.cpp
View File

@ -9,7 +9,6 @@
#include <math.h>
namespace LinearAlgebra {
template <typename T>
DirectionOf<T>::DirectionOf() {
this->horizontal = AngleOf<T>();
@ -99,6 +98,5 @@ void DirectionOf<T>::Normalize() {
}
}
template class LinearAlgebra::DirectionOf<float>;
template class LinearAlgebra::DirectionOf<signed short>;
}
template class DirectionOf<float>;
template class DirectionOf<signed short>;

2
LinearAlgebra/Direction.h
View File

@ -99,4 +99,6 @@ using Direction = DirectionSingle;
} // namespace LinearAlgebra
using namespace LinearAlgebra;
#endif

4
LinearAlgebra/Polar.cpp
View File

@ -175,5 +175,5 @@ PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
return r;
}
template class LinearAlgebra::PolarOf<float>;
template class LinearAlgebra::PolarOf<signed short>;
template class PolarOf<float>;
template class PolarOf<signed short>;

4
LinearAlgebra/Spherical.cpp
View File

@ -5,8 +5,6 @@
#include <math.h>
namespace LinearAlgebra {
template <typename T>
SphericalOf<T>::SphericalOf() {
this->distance = 0.0f;
@ -303,5 +301,3 @@ SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
template class SphericalOf<float>;
template class SphericalOf<signed short>;
} // namespace LinearAlgebra

1
LinearAlgebra/Spherical.h
View File

@ -186,6 +186,7 @@ using Spherical = SphericalSingle;
#endif
} // namespace LinearAlgebra
using namespace LinearAlgebra;
#include "Polar.h"
#include "Vector3.h"

4
LinearAlgebra/SwingTwist.cpp
View File

@ -4,8 +4,6 @@
#include "SwingTwist.h"
namespace LinearAlgebra {
template <typename T>
SwingTwistOf<T>::SwingTwistOf() {
this->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
@ -168,5 +166,3 @@ void SwingTwistOf<T>::Normalize() {
template class SwingTwistOf<float>;
template class SwingTwistOf<signed short>;
}

2
LinearAlgebra/test/Direction_test.cc
View File

@ -6,8 +6,6 @@
#include "Direction.h"
using namespace LinearAlgebra;
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Direction16, Compare) {

2
Messages/NetworkIdMsg.cpp
View File

@ -1,7 +1,5 @@
#include "NetworkIdMsg.h"
#include <iostream>
namespace RoboidControl {
NetworkIdMsg::NetworkIdMsg(const char* buffer) {

6
Messages/ParticipantMsg.cpp
View File

@ -1,9 +1,5 @@
#include "ParticipantMsg.h"
#if !defined(NO_STD)
#include <iostream>
#endif
namespace RoboidControl {
ParticipantMsg::ParticipantMsg(char networkId) {
@ -17,7 +13,7 @@ ParticipantMsg::ParticipantMsg(const char* buffer) {
ParticipantMsg::~ParticipantMsg() {}
unsigned char ParticipantMsg::Serialize(char* buffer) {
#if defined(DEBUG) && !defined(NO_STD)
#if defined(DEBUG)
std::cout << "Send ParticipantMsg [" << (int)this->networkId << "] "
<< std::endl;
#endif

4
Messages/PoseMsg.cpp
View File

@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
this->thingId = thing->id;
this->poseType = 0;
if (thing->positionUpdated || (force && thing->IsRoot())) {
if (thing->positionUpdated || (force && thing->GetParent() != nullptr)) {
this->position = thing->GetPosition();
this->poseType |= Pose_Position;
}
if (thing->orientationUpdated || (force && thing->IsRoot())) {
if (thing->orientationUpdated || (force && thing->GetParent() != nullptr)) {
this->orientation = thing->GetOrientation();
this->poseType |= Pose_Orientation;
}

6
Messages/TextMsg.cpp
View File

@ -1,9 +1,5 @@
#include "TextMsg.h"
#if !defined(NO_STD)
#include <iostream>
#endif
namespace RoboidControl {
TextMsg::TextMsg(const char* text, unsigned char textLength) {
@ -28,7 +24,7 @@ unsigned char TextMsg::Serialize(char* buffer) {
if (this->textLength == 0 || this->text == nullptr)
return 0;
#if defined(DEBUG) && !defined(NO_STD)
#if defined(DEBUG)
std::cout << "Send TextMsg " << (int)this->textLength << " " << this->text << std::endl;
#endif
unsigned char ix = 0;

22
Messages/ThingMsg.cpp
View File

@ -14,21 +14,27 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) {
this->networkId = networkId;
this->thingId = thing->id;
this->thingType = thing->type;
if (thing->IsRoot())
this->parentId = 0;
else {
Thing* parent = thing->GetParent();
Thing* parent = thing->GetParent();
if (parent != nullptr)
this->parentId = parent->id;
}
else
this->parentId = 0;
}
// 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;

66
Participant.cpp
View File

@ -8,26 +8,7 @@ namespace RoboidControl {
ParticipantRegistry Participant::registry;
Participant* Participant::LocalParticipant = new Participant();
void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
LocalParticipant = &newParticipant;
std::cout << "Replaced local participant" << std::endl;
}
Participant::Participant() {
std::cout << "P\n";
//this->root.name = "Isolated";
this->root = new Thing(this);
this->root->name = "Root";
this->Add(this->root);
}
Participant::Participant(const char* ipAddress, int port) {
// Add the root thing to the list of things, because we could not do that
// earlier (I think)
this->Add(this->root);
// make a copy of the ip address string
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
@ -45,14 +26,14 @@ Participant::Participant(const char* ipAddress, int port) {
}
Participant::~Participant() {
// registry.Remove(this);
registry.Remove(this);
delete[] this->ipAddress;
}
void Participant::Update() {
void Participant::Update(unsigned long currentTimeMs) {
for (Thing* thing : this->things) {
if (thing != nullptr)
thing->Update(true);
thing->Update(currentTimeMs, true);
}
}
@ -131,26 +112,23 @@ void Participant::Remove(Thing* thing) {
#pragma region ParticipantRegistry
Participant* ParticipantRegistry::Get(const char* ipAddress,
unsigned int port) {
#if !defined(NO_STD)
unsigned int port) {
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;
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;
@ -158,12 +136,11 @@ Participant* ParticipantRegistry::Get(unsigned char 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) {
unsigned int port) {
Participant* participant = new Participant(ipAddress, port);
Add(participant);
return participant;
@ -175,22 +152,19 @@ void ParticipantRegistry::Add(Participant* participant) {
if (foundParticipant == nullptr) {
#if defined(NO_STD)
// this->things[this->thingCount++] = thing;
this->things[this->thingCount++] = thing;
#else
ParticipantRegistry::participants.push_back(participant);
#endif
// std::cout << "Add participant " << participant->ipAddress << ":"
// << participant->port << "[" << (int)participant->networkId
// << "]\n";
// std::cout << "participants " <<
// ParticipantRegistry::participants.size()
// << "\n";
// } else {
// std::cout << "Did not add, existing participant " <<
// participant->ipAddress
// << ":" << participant->port << "[" <<
// (int)participant->networkId
// << "]\n";
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";
}
}
@ -198,15 +172,9 @@ void ParticipantRegistry::Remove(Participant* participant) {
// participants.remove(participant);
}
#if defined(NO_STD)
Participant** ParticipantRegistry::GetAll() const {
return ParticipantRegistry::participants;
}
#else
const std::list<Participant*>& ParticipantRegistry::GetAll() const {
return ParticipantRegistry::participants;
}
#endif
#pragma endregion ParticipantRegistry

24
Participant.h
View File

@ -1,5 +1,4 @@
#pragma once
#include "Thing.h"
namespace RoboidControl {
@ -32,21 +31,13 @@ class ParticipantRegistry {
/// @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<Participant*>& GetAll() const;
private:
#if defined(NO_STD)
#else
/// @brief The list of known participants
std::list<Participant*> participants;
#endif
@ -60,9 +51,6 @@ class ParticipantRegistry {
/// reference to remote participants.
class Participant {
public:
/// @brief The name of the participant
const char* name = "Participant";
/// @brief The Ip Address of a participant.
const char* ipAddress = "0.0.0.0";
/// @brief The port number for UDP communication with the participant.
@ -71,7 +59,6 @@ class Participant {
/// @brief The network Id to identify the participant
unsigned char networkId = 0;
Participant();
/// @brief Create a new participant with the given communcation info
/// @param ipAddress The IP address of the participant
/// @param port The UDP port of the participant
@ -79,11 +66,6 @@ class Participant {
/// @brief Destructor for the participant
~Participant();
static Participant* LocalParticipant;
static void ReplaceLocalParticipant(Participant& newParticipant);
Thing* root = new Thing(this);
public:
#if defined(NO_STD)
unsigned char thingCount = 0;
@ -106,7 +88,7 @@ class Participant {
/// @brief Update all things for this participant
/// @param currentTimeMs The current time in milliseconds (optional)
virtual void Update();
virtual void Update(unsigned long currentTimeMs = 0);
public:
static ParticipantRegistry registry;

77
Participants/ParticipantUDP.cpp
View File

@ -1,49 +1,30 @@
#include "ParticipantUDP.h"
#include "Participant.h"
#include "Thing.h"
#include "Arduino/ArduinoParticipant.h"
#include "EspIdf/EspIdfParticipant.h"
#include "Posix/PosixParticipant.h"
#include "Windows/WindowsParticipant.h"
#include "Posix/PosixParticipant.h"
#include <string.h>
namespace RoboidControl {
#pragma region Init
ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) {
this->name = "ParticipantUDP";
this->remoteSite = nullptr;
if (this->port == 0)
this->isIsolated = true;
Participant::registry.Add(this);
this->root = Thing::LocalRoot(); //::LocalParticipant->root;
this->root->owner = this;
this->root->name = "UDP Root";
this->Add(this->root);
Participant::ReplaceLocalParticipant(*this);
}
ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
: Participant("127.0.0.1", localPort) {
this->name = "ParticipantUDP";
if (this->port == 0)
this->isIsolated = true;
else
this->remoteSite = new Participant(ipAddress, port);
Participant::registry.Add(this);
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
this->root->owner = this;
this->root->name = "UDP Root";
this->Add(this->root);
Participant::ReplaceLocalParticipant(*this);
}
static ParticipantUDP* isolatedParticipant = nullptr;
@ -83,18 +64,11 @@ void ParticipantUDP::SetupUDP(int localPort,
this->connected = true;
}
#pragma endregion Init
#pragma region Update
// The update order
// 1. receive external messages
// 2. update the state
// 3. send out the updated messages
void ParticipantUDP::Update() {
unsigned long currentTimeMs = Thing::GetTimeMs();
PrepMyThings();
void ParticipantUDP::Update(unsigned long currentTimeMs) {
if (currentTimeMs == 0)
currentTimeMs = Thing::GetTimeMs();
if (this->isIsolated == false) {
if (this->connected == false)
@ -114,26 +88,15 @@ void ParticipantUDP::Update() {
this->ReceiveUDP();
}
UpdateMyThings();
UpdateOtherThings();
UpdateMyThings(currentTimeMs);
UpdateOtherThings(currentTimeMs);
}
void ParticipantUDP::PrepMyThings() {
for (Thing* thing : this->things) {
if (thing == nullptr)
continue;
thing->PrepareForUpdate();
}
}
void ParticipantUDP::UpdateMyThings() {
// std::cout << this->things.size() << std::endl;
void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) {
for (Thing* thing : this->things) {
if (thing == nullptr) // || thing->GetParent() != nullptr)
continue;
// std::cout << thing->name << "\n";
if (thing->hierarchyChanged) {
if (!(this->isIsolated || this->networkId == 0)) {
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
@ -148,14 +111,12 @@ void ParticipantUDP::UpdateMyThings() {
}
}
// std::cout << "B\n";
// Why don't we do recursive?
// Because when a thing creates a thing in the update,
// that new thing is not sent out (because of hierarchyChanged)
// before it is updated itself: it is immediatedly updated!
thing->Update(false);
thing->Update(currentTimeMs, false);
// std::cout << "C\n";
if (!(this->isIsolated || this->networkId == 0)) {
if (thing->terminate) {
DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
@ -176,28 +137,20 @@ void ParticipantUDP::UpdateMyThings() {
delete binaryMsg;
}
}
// std::cout << "D\n";
if (thing->terminate)
this->Remove(thing);
// std::cout << "E\n";
}
}
void ParticipantUDP::UpdateOtherThings() {
#if defined(NO_STD)
Participant** participants = Participant::registry.GetAll();
for (int ix = 0; ix < Participant::registry.count; ix++) {
Participant* participant = participants[ix];
#else
void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) {
for (Participant* participant : Participant::registry.GetAll()) {
#endif
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();
participant->Participant::Update(currentTimeMs);
if (this->isIsolated)
continue;
@ -242,8 +195,8 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
if (bufferSize <= 0)
return true;
// std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
// << " to " << remoteParticipant->ipAddress << std::endl;
// std::cout << "send msg " << (int)this->buffer[0] << " to "
// << remoteParticipant->ipAddress << std::endl;
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
@ -420,14 +373,14 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
std::cout << this->name << ": process SiteMsg " << (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);
}
@ -488,7 +441,7 @@ void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
}
void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
#if !defined(DEBUG) && !defined(NO_STD)
#if !defined(DEBUG)
std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
<< "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
#endif

11
Participants/ParticipantUDP.h
View File

@ -78,6 +78,9 @@ class ParticipantUDP : public Participant {
/// local network
long publishInterval = 3000; // 3 seconds
/// @brief The name of the participant
const char* name = "ParticipantUDP";
protected:
char buffer[1024];
@ -97,15 +100,13 @@ class ParticipantUDP : public Participant {
#pragma region Update
public:
virtual void Update() override;
virtual void Update(unsigned long currentTimeMs = 0) override;
protected:
unsigned long nextPublishMe = 0;
/// @brief Prepare the local things for the next update
virtual void PrepMyThings();
virtual void UpdateMyThings();
virtual void UpdateOtherThings();
virtual void UpdateMyThings(unsigned long currentTimeMs);
virtual void UpdateOtherThings(unsigned long currentTimeMs);
#pragma endregion Update

25
Participants/SiteServer.cpp
View File

@ -22,22 +22,16 @@ SiteServer::SiteServer(int port) : ParticipantUDP(port) {
#pragma region Update
void SiteServer::UpdateMyThings() {
void SiteServer::UpdateMyThings(unsigned long currentTimeMs) {
for (Thing* thing : this->things) {
if (thing == nullptr)
continue;
thing->Update(true);
thing->Update(currentTimeMs, 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;
@ -72,24 +66,15 @@ void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
void SiteServer::Process(Participant* sender, ThingMsg* msg) {
Thing* thing = sender->Get(msg->thingId);
if (thing == nullptr)
// new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
// Thing::Reconstruct(sender, msg->thingType, msg->thingId);
//thing = new Thing(msg->thingType, sender->root);
;
thing->id = msg->thingId;
new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
if (msg->parentId != 0) {
thing->SetParent(Get(msg->parentId));
if (thing->IsRoot())
// if (thing->GetParent() != nullptr)
#if defined(NO_STD)
;
#else
if (thing->GetParent() != nullptr)
std::cout << "Could not find parent [" << (int)msg->networkId << "/"
<< (int)msg->parentId << "]\n";
#endif
} else
thing->SetParent(Thing::LocalRoot());
thing->SetParent(nullptr);
}
#pragma endregion Receive

2
Participants/SiteServer.h
View File

@ -24,7 +24,7 @@ class SiteServer : public ParticipantUDP {
#pragma region Update
virtual void UpdateMyThings() override;
virtual void UpdateMyThings(unsigned long currentTimeMs) override;
#pragma endregion Update

8
Posix/PosixParticipant.cpp
View File

@ -11,7 +11,7 @@
namespace RoboidControl {
namespace Posix {
void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
void Setup(int localPort, const char* remoteIpAddress, int remotePort) {
#if defined(__unix__) || defined(__APPLE__)
// Create a UDP socket
@ -63,7 +63,7 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remot
#endif
}
void ParticipantUDP::Receive() {
void Receive() {
#if defined(__unix__) || defined(__APPLE__)
sockaddr_in client_addr;
socklen_t len = sizeof(client_addr);
@ -90,7 +90,7 @@ void ParticipantUDP::Receive() {
#endif
}
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
bool 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 ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
return true;
}
bool ParticipantUDP::Publish(IMessage* msg) {
bool Publish(IMessage* msg) {
#if defined(__unix__) || defined(__APPLE__)
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)

7
Posix/PosixParticipant.h
View File

@ -11,13 +11,6 @@ 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

87
Thing.cpp
View File

@ -5,7 +5,6 @@
#include "Participants/IsolatedParticipant.h"
#include <string.h>
// #include <iostream>
#if defined(ARDUINO)
#include "Arduino.h"
@ -20,31 +19,14 @@ namespace RoboidControl {
#pragma region Init
Thing* Thing::LocalRoot() {
Participant* p = Participant::LocalParticipant;
Thing* localRoot = p->root;
return localRoot;
}
// Only use this for root things
Thing::Thing(Participant* owner) {
this->type = Type::Roboid; // should become root
this->position = Spherical::zero;
this->positionUpdated = true;
this->orientation = SwingTwist::identity;
this->orientationUpdated = true;
this->hierarchyChanged = true;
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
Thing::Thing(unsigned char thingType)
: Thing(IsolatedParticipant::Isolated(), thingType) {}
Thing::Thing(Participant* owner,
unsigned char thingType,
unsigned char thingId) {
this->owner = owner;
//this->owner->Add(this, true);
std::cout << this->owner->name << ": New root thing " << std::endl;
}
Thing::Thing(unsigned char thingType, Thing* parent) {
this->id = thingId;
this->type = thingType;
this->position = Spherical::zero;
@ -56,25 +38,17 @@ Thing::Thing(unsigned char thingType, Thing* parent) {
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
this->owner = parent->owner;
// std::cout << "add thing [" << (int)this->id << "] to owner "
// << this->owner->ipAddress << ":" << this->owner->port <<
// std::endl;
this->owner->Add(this, true);
}
Thing::Thing(Thing* parent, unsigned char thingType, unsigned char thingId)
: Thing(parent->owner, thingType, thingId) {
this->SetParent(parent);
std::cout << this->owner->name << ": New thing for " << parent->name
<< std::endl;
}
Thing::~Thing() {
std::cout << "Destroy thing " << this->name << std::endl;
}
// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType,
// unsigned char thingId) {
// Thing thing = Thing(owner, thingType);
// thing.id = thingId;
// return thing;
// }
#pragma endregion Init
void Thing::SetName(const char* name) {
@ -103,19 +77,6 @@ void Thing::SetParent(Thing* parent) {
this->hierarchyChanged = true;
}
// void Thing::SetParent(Thing* parent) {
// parent->AddChild(this);
// this->hierarchyChanged = true;
// }
// const Thing& Thing::GetParent() {
// return *this->parent;
// }
bool Thing::IsRoot() const {
return this == LocalRoot() || this->parent == nullptr; //&Thing::Root;
}
// void Thing::SetParent(Thing* root, const char* name) {
// Thing* thing = root->FindChild(name);
// if (thing != nullptr)
@ -169,7 +130,7 @@ Thing* Thing::RemoveChild(Thing* child) {
}
}
child->parent = Thing::LocalRoot();
child->parent = nullptr;
delete[] this->children;
this->children = newChildren;
@ -260,8 +221,7 @@ Spherical Thing::GetAngularVelocity() {
unsigned long Thing::GetTimeMs() {
#if defined(ARDUINO)
unsigned long ms = millis();
return ms;
return millis();
#else
auto now = std::chrono::steady_clock::now();
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
@ -270,13 +230,11 @@ unsigned long Thing::GetTimeMs() {
#endif
}
// void Thing::Update(bool recursive) {
// Update(GetTimeMs(), recursive);
// }
void Thing::PrepareForUpdate() {}
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;
@ -287,18 +245,17 @@ void Thing::Update(bool recursive) {
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);
child->Update(currentTimeMs, recursive);
}
}
}
void Thing::UpdateThings() {
IsolatedParticipant::Isolated()->Update();
void Thing::UpdateThings(unsigned long currentTimeMs) {
IsolatedParticipant::Isolated()->Update(currentTimeMs);
}
#pragma endregion Update

62
Thing.h
View File

@ -32,7 +32,6 @@ class Thing {
ControlledMotor,
UncontrolledMotor,
Servo,
IncrementalEncoder,
// Other
Roboid,
Humanoid,
@ -41,23 +40,19 @@ class Thing {
};
#pragma region Init
static Thing* LocalRoot();
private:
// Special constructor to create a root thing
Thing(Participant* parent);
// Which can only be used by the Participant
friend class Participant;
public:
/// @brief Create a new thing
/// @brief Create a new thing without communication abilities
/// @param thingType The type of thing (can use Thing::Type)
/// @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 a root thing.
Thing(unsigned char thingType = Thing::Type::Undetermined,
Thing* parent = LocalRoot());
Thing(unsigned char thingType = Type::Undetermined);
/// @brief Create a new thing for a participant
/// @param owner The owning participant
/// @param thingType The type of thing (can use Thing::Type)
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
Thing(Participant* owner,
unsigned char thingType = Type::Undetermined,
unsigned char thingId = 0);
/// @brief Create a new child thing
/// @param parent The parent thing
@ -65,12 +60,7 @@ class Thing {
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
/// @note The owner will be the same as the owner of the parent thing
~Thing();
static Thing Reconstruct(Participant* owner,
unsigned char thingType,
unsigned char thingId);
Thing(Thing* parent, unsigned char thingType = 0, unsigned char thingId = 0);
#pragma endregion Init
@ -82,7 +72,6 @@ class Thing {
/// @brief The participant managing this thing
Participant* owner = nullptr;
/// @brief The ID of the thing
unsigned char id = 0;
@ -117,15 +106,11 @@ class Thing {
/// @brief Sets the parent of this Thing
/// @param parent The Thing which should become the parent
// virtual void SetParent(Thing* parent);
void SetParent(Thing* parent);
virtual void SetParent(Thing* parent);
/// @brief Gets the parent of this Thing
/// @return The parent Thing
// Thing* GetParent();
Thing* GetParent();
bool IsRoot() const;
/// @brief The number of children
unsigned char childCount = 0;
/// @brief Get a child by index
@ -213,11 +198,9 @@ class Thing {
/// parent's orientation
SwingTwist orientation;
/// @brief The linear velocity of the thing in local space, in meters per
/// second
/// @brief The linear velocity of the thing in local space, in meters per second
Spherical linearVelocity;
/// @brief The angular velocity of the thing in local space, in degrees per
/// second
/// @brief The angular velocity of the thing in local space, in degrees per second
Spherical angularVelocity;
#pragma endregion Pose
@ -225,19 +208,20 @@ class Thing {
#pragma region Update
public:
virtual void PrepareForUpdate();
/// @brief Get the current time in milliseconds
/// @return The current time in milliseconds
static unsigned long GetTimeMs();
/// @brief Updates the state of the thing
void Update(bool recursive = false);
/// @brief Updates the state of the thing
/// @param currentTimeMs The current clock time in milliseconds; if this is
/// zero, the current time is retrieved automatically
/// @param recurse When true, this will Update the descendants recursively
virtual void Update(bool recurse = false);
virtual void Update(unsigned long currentTimeMs, bool recurse = false);
static void UpdateThings();
/// @brief Get the current time in milliseconds
/// @return The current time in milliseconds
static unsigned long GetTimeMs();
static void UpdateThings(unsigned long currentTimeMs);
#pragma endregion Update

54
Things/ControlledMotor.cpp
View File

@ -1,54 +0,0 @@
#include "ControlledMotor.h"
namespace RoboidControl {
ControlledMotor::ControlledMotor(Motor* motor,
RelativeEncoder* encoder,
Thing* parent)
: Motor(parent), motor(motor), encoder(encoder) {
this->type = Type::ControlledMotor;
// encoder.SetParent(*this);
// Thing parent = motor.GetParent();
// this->SetParent(parent);
}
void ControlledMotor::SetTargetVelocity(float velocity) {
this->targetVelocity = velocity;
this->rotationDirection =
(targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
}
void ControlledMotor::Update(bool recurse) {
encoder->Update(false);
this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
unsigned long currentTimeMs = GetTimeMs();
float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
this->lastUpdateTime = currentTimeMs;
float error = this->targetVelocity - this->actualVelocity;
float acceleration =
error * timeStep * pidP; // Just P is used at this moment
std::cout << "motor acc. " << acceleration << std::endl;
motor->SetTargetVelocity(targetVelocity +
acceleration); // 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

38
Things/ControlledMotor.h
View File

@ -1,38 +0,0 @@
#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 = 1;
float pidD = 0;
float pidI = 0;
/// @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 lastUpdateTime;
};
} // namespace RoboidControl

69
Things/DifferentialDrive.cpp
View File

@ -1,28 +1,13 @@
#include "DifferentialDrive.h"
#include "Messages/LowLevelMessages.h"
namespace RoboidControl {
DifferentialDrive::DifferentialDrive(Thing* parent)
: Thing(Type::DifferentialDrive, parent) {
this->name = "Differential drive";
DifferentialDrive::DifferentialDrive() : Thing(Thing::Type::DifferentialDrive) {}
this->leftWheel = new Motor(this);
this->leftWheel->name = "Left motor";
DifferentialDrive::DifferentialDrive(Participant* participant, unsigned char thingId)
: Thing(participant, Thing::Type::DifferentialDrive, thingId) {}
this->rightWheel = new Motor(this);
this->rightWheel->name = "Right motor";
}
DifferentialDrive::DifferentialDrive(Motor* leftMotor,
Motor* rightMotor,
Thing* parent)
: Thing(Type::DifferentialDrive, parent) {
this->name = "Differential drive";
this->leftWheel = leftMotor;
this->rightWheel = rightMotor;
}
DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) : Thing(parent, Thing::Type::DifferentialDrive, thingId) {}
void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
float wheelSeparation) {
@ -39,38 +24,27 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
}
// Motor& DifferentialDrive::GetMotorLeft() {
// return *this->leftWheel;
// }
// Motor& DifferentialDrive::GetMotorRight() {
// return *this->rightWheel;
// }
void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) {
void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
float distance = this->wheelSeparation / 2;
this->leftWheel = &leftMotor;
this->leftWheel->SetPosition(Spherical(distance, Direction::left));
this->leftWheel = leftWheel;
;
if (leftWheel != nullptr)
this->leftWheel->SetPosition(Spherical(distance, Direction::left));
this->rightWheel = &rightMotor;
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
this->rightWheel = rightWheel;
if (rightWheel != nullptr)
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
}
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));
void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
if (this->leftWheel != nullptr)
this->leftWheel->SetTargetVelocity(velocityLeft);
this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
if (this->rightWheel != nullptr)
this->rightWheel->SetTargetVelocity(velocityRight);
this->rightWheel->SetAngularVelocity(
Spherical(speedRight, Direction::right));
}
void DifferentialDrive::Update(bool recursive) {
void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
if (this->linearVelocityUpdated) {
// this assumes forward velocity only....
float linearVelocity = this->GetLinearVelocity().distance;
@ -91,14 +65,7 @@ void DifferentialDrive::Update(bool recursive) {
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;
Thing::Update(currentMs, recursive);
}
} // namespace RoboidControl

35
Things/DifferentialDrive.h
View File

@ -1,7 +1,6 @@
#pragma once
#include "Thing.h"
#include "Motor.h"
namespace RoboidControl {
@ -10,13 +9,18 @@ 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
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
DifferentialDrive(Participant* participant, unsigned char thingId = 0);
/// @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());
DifferentialDrive(Thing* parent, unsigned char thingId = 0);
/// @brief Configures the dimensions of the drive
/// @param wheelDiameter The diameter of the wheels in meters
@ -26,13 +30,10 @@ 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(Motor& leftMotor, Motor& rightMotor);
void SetMotors(Thing* leftWheel, Thing* rightWheel);
/// @brief Directly specify the speeds of the motors
/// @param speedLeft The speed of the left wheel in degrees per second.
@ -42,25 +43,21 @@ class DifferentialDrive : public Thing {
void SetWheelVelocity(float speedLeft, float speedRight);
/// @copydoc RoboidControl::Thing::Update(unsigned long)
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;
virtual void Update(unsigned long currentMs, bool recursive = true) override;
protected:
/// @brief The radius of a wheel in meters
float wheelRadius = 0.0f;
float wheelRadius = 1.0f;
/// @brief The distance between the wheels in meters
float wheelSeparation = 0.0f;
float wheelSeparation = 1.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

15
Things/DigitalSensor.cpp
View File

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

7
Things/DigitalSensor.h
View File

@ -8,18 +8,17 @@ namespace RoboidControl {
class DigitalSensor : public Thing {
public:
/// @brief Create a digital sensor without communication abilities
//DigitalSensor();
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);
DigitalSensor(Participant* owner, 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());
DigitalSensor(Thing* parent, unsigned char thingId = 0);
/// @brief The sigital state
bool state = 0;

16
Things/Motor.cpp
View File

@ -1,16 +0,0 @@
#include "Motor.h"
namespace RoboidControl {
Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {}
void Motor::SetTargetVelocity(float targetSpeed) {
this->targetVelocity = targetSpeed;
}
int Motor::GenerateBinary(char* data, unsigned char* ix) {
data[(*ix)++] = this->targetVelocity * 127.0f;
return 1;
}
} // namespace RoboidControl

25
Things/Motor.h
View File

@ -1,25 +0,0 @@
#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
int GenerateBinary(char* bytes, unsigned char* ix) override;
// virtual void ProcessBinary(char* bytes) override;
//protected:
float targetVelocity = 0;
};
} // namespace RoboidControl

21
Things/RelativeEncoder.cpp
View File

@ -1,21 +0,0 @@
#include "RelativeEncoder.h"
namespace RoboidControl {
RelativeEncoder::RelativeEncoder(Thing* parent)
: Thing(Type::IncrementalEncoder, parent) {}
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

39
Things/RelativeEncoder.h
View File

@ -1,39 +0,0 @@
#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;
/// @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

12
Things/TemperatureSensor.cpp
View File

@ -4,15 +4,9 @@
namespace RoboidControl {
// TemperatureSensor::TemperatureSensor(Participant* participant,
// unsigned char thingId)
// : Thing(participant, Type::TemperatureSensor, thingId) {}
// TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {}
TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(Type::TemperatureSensor, parent) {}
// TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {}
TemperatureSensor::TemperatureSensor(Participant* participant,
unsigned char thingId)
: Thing(participant, Type::TemperatureSensor, thingId) {}
void TemperatureSensor::SetTemperature(float temp) {
this->temperature = temp;

4
Things/TemperatureSensor.h
View File

@ -12,9 +12,7 @@ class TemperatureSensor : public Thing {
/// @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(Thing* parent);
TemperatureSensor(Thing* parent = Thing::LocalRoot());
TemperatureSensor(Participant* participant, unsigned char thingId);
/// @brief The measured temperature
float temperature = 0;

21
Things/TouchSensor.cpp
View File

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

14
Things/TouchSensor.h
View File

@ -8,19 +8,23 @@ namespace RoboidControl {
class TouchSensor : public Thing {
// Why finishing this release (0.3), I notice that this is equivalent to a digital sensor
public:
/// @brief Create a touch sensor without communication abilities
TouchSensor();
/// @brief Create a touch 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
TouchSensor(Participant* owner, unsigned char thingId = 0);
/// @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());
TouchSensor(Thing* parent, unsigned char thingId = 0);
/// @brief Value which is true when the sensor is touching something, false
/// otherwise
bool touchedSomething = false;
virtual void PrepareForUpdate() override;
virtual void Update(bool recursive) override;
/// @brief Function used to generate binary data for this touch sensor
/// @param buffer The byte array for thw binary data
/// @param ix The starting position for writing the binary data
@ -28,8 +32,6 @@ class TouchSensor : public Thing {
/// @brief Function used to process binary data received for this touch sensor
/// @param bytes The binary data to process
virtual void ProcessBinary(char* bytes) override;
protected:
bool externalTouch = false;
};
} // namespace RoboidControl

1
examples/README.md
View File

@ -1 +0,0 @@
Important: this folder has to be names 'examples' exactly to maintain compatibility with Arduino

15
test/participant_test.cc
View File

@ -7,9 +7,6 @@
#include "Participants/SiteServer.h"
#include "Thing.h"
#include <chrono>
#include <thread>
using namespace RoboidControl;
TEST(Participant, Participant) {
@ -18,7 +15,7 @@ TEST(Participant, Participant) {
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
participant->Update();
participant->Update(milliseconds);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();
@ -32,7 +29,7 @@ TEST(Participant, SiteServer) {
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
siteServer->Update();
siteServer->Update(milliseconds);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();
@ -47,8 +44,8 @@ TEST(Participant, SiteParticipant) {
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
siteServer->Update();
participant->Update();
siteServer->Update(milliseconds);
participant->Update(milliseconds);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();
@ -66,8 +63,8 @@ TEST(Participant, ThingMsg) {
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
siteServer->Update();
participant->Update();
siteServer->Update(milliseconds);
participant->Update(milliseconds);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();

4
test/thing_test.cc
View File

@ -15,7 +15,7 @@ TEST(RoboidControlSuite, HiddenParticipant) {
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 1000) {
Thing::UpdateThings();
Thing::UpdateThings(milliseconds);
milliseconds = Thing::GetTimeMs();
}
@ -29,7 +29,7 @@ TEST(RoboidControlSuite, IsolatedParticipant) {
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 1000) {
participant->Update();
participant->Update(milliseconds);
milliseconds = Thing::GetTimeMs();
}