Compare commits

..

21 Commits
v0.3.0 ... main

Author SHA1 Message Date
c95d1cfc06 Merge branch 'main' of https://git.passer.life/RoboidControl/RoboidControl-cpp 2025-06-02 12:26:25 +02:00
627a79f56c Merge commit '90e89b2ecd49870e84af4b66fe41610e4855673c' 2025-06-02 12:26:07 +02:00
c3ba44d47a Merge commit 'e50c8eb9c89c7fc801ed2c543a1d3970011efb2c' 2025-05-26 15:29:28 +02:00
e50c8eb9c8 Squashed 'LinearAlgebra/' changes from bfa1123..8b6f628
8b6f628 Fix Matrix unit test
28a3064 Fix tests
91f4280 Reduced the use of raw pointers
83539df Fixes to make it work againg, but failing at udp.begin()
3cca327 Optimizations
361807c Performance improvements
7bd83fa Processomg Acc data * propagating
0eb2851 Linking works
06ff5e9 Added ESP-IDF Wifi/UDP support (but it still give linker errors)
7461a1f Implemented integrate function
b7f102a Fix matrix2 tests
7d2dd08 First steps implementation Matrix operations
f05b411 Cleanup
05a3b09 Update namespace, Windows compatibility
95a248a First udp communcation is working
fea15c3 removed namespace Passer
753de2d Fixes
9674f68 Disabled test failing on MacOS
5b89234 Made it work on MacOS
d0ba29e Updated LinearAlgebra
ebfb4fb Merge commit '0b63a044eb84175cc922f3f1fe0ce039d101bf7b'
0b63a04 Updated documentation, cleanup
a0082bd Add direction documentation
770ce32 Merge commit '2b5f5cd175b20fa01e04b696dfe0cfbe2c4ad9da'
2b5f5cd Removed shorthand Deg/Rad aliases... (see below)
d212db2 Trying to fix specialization before instantiation error
e445e5b Trying to fix specialization before instantiation error
f47e504 Trying to fix specialization before instantiation error
b877d4d Trying to fix specialization before instantiation error
06e8623 Trying to fix specialization before instantiation error
e97aee9 Trying to fix specialization before instantiation error
0e00e5d Merge branch 'main' of http://gitlab.passervr.com/passer/cpp/linear-algebra
52de55d Trying to fix specialization before instantiation error
47ba024 Trying to fix specialization before instantiation error
2402030 Trying to fix specialization before instantiation error
4e3f253 Fixed gitignore
06c70e2 Merge branch 'Experimental'
6f30334 Updated PolarOf, removed default Polar type
94a3105 Cleanup
89a5711 Fixed typo in documentation
64b7f11 Removed default Angle type
58beb36 Completed AngleOf documentation
5f58a2c Updated documentation
97d937e Fixed (copilot) unit tests
585a3b0 Removed default Spherical type
d3226fd Added copilot test documentation
a7aa679 Added a performance test
2c57c3f Fixed direction for spherical
28ee225 Normalize Spherical at creation
800eb75 improved rounding of discrete angles
536d6ce Add Binary support
18756ba Correct axis on quaternion from swingtwist
06e42c5 Normalized swing twist
edbb4b1 Removes Angle::deg90/180 because of issues
0a07b23 SwingTwist == and Angle
9406b57 SwingTwist::AngleAxis using Direction
06da9e5 Removed AngleAxis, Sperhical will be used instead
2e61e1b Fix unit test
54d0318 Extended unit tests (plus fixes)
8286c1c Minor improvements
95a6fb3 Improved Angle quality
9eca318 Fixed inverse
0ebfce4 Spherical direction
136e44e Fixed unit tests
fb5cee8 Proper angle implementation (unit tests still fail)
92d5ef0 Normalizing directions
1ea65d5 Made toQuestion const
f1c70c7 Fix unit tests
2ad6a5a Fix include
11259a9 Extend functionality
3363388 Extended AngleAxis & Direction
09e25a8 Add Asin and Atan
a646e93 Add ACos
407e771 Add WithDistance
69bad8f Added degrees, radians, cos, sin, and tan functions
5c3dfa6 Adde degrees/readians and poc ACos
05c61a3 Add distance between
6a1d043 Added Angle comparison
6b3bcfc Another anglebetween fix
b5a6330 Fix anglebetween
b975aed Added angle and rotate functions
f483439 Moved Polar/Spherical to template class completely
e0ef43a Merge branch 'Experimental' of http://gitlab.passervr.com/passer/cpp/linear-algebra into Experimental
b460bec Added scaling to SphericalOf<T>
e10e010 Added Direction to library sources
dedaa31 Fix unit tests
1da93ca Fix ToVector3
9c3503f Added SphericalOf
353cb1b Add conversion from Quaternion
cf86ba8 Cleanup template classes
ea6894e Fix generic template functions
1ce7234 Bug fixes
38ebb34 Added Direction to replace Axis
c72bba4 Change Vector3::Angle return type to AngleOf<float>
18cf653 Added scaling support
51772a1 Added add/subtract for discrete angles
49c6740 Make Angle -> float conversion explicit
608c45c Fix precision error
4591aef Added Spherical16
48828e2 Fix int/float issues
86b7c21 Added Degrees inline function
a4b0491 Fix unit tests
b81b77b Extend AngleOf support
c70c079 Add spherical subtract
2bad384 Added spherical addition
a25a8be Fixed namespace issues
f8009a7 Updated namespace
91c7b20 Fix gitlab test runner
78468e3 Renamed VectorAlgebra project to LinearAlgebra
47a6368 Fix Vector3
ee62cba XYZ deprecation
62d7439 Extend ulitity functions
95713c8 Cleanup, added utility functions
f7d9d97 Cleanup
66e47d7 Cleanup Vector2 and Polar
e922a01 fix == operator
5693097 Initial subtractop test
c411b43 Textual improvement
0c54276 equality support
b1e34b6 Improved unit tests
6ddb074 Improved unit tests
5489b3c Fix test
3d971c1 Improve namespace
791bd78 namespace improvement
9bea94f Use Angle type for Polar
8a2ce21 Swapped polar constructor param order
e5f8d73 namespace fix
8e7a8c6 Fix namespaces
64ca768 normalize toAngleAxis result
4385bef Add angle-axis
4b07328 Add conversion from Vector3
fe12c99 Fix Spherical reference
c274c3e Add conversion from Spherical
66ff721 Fixed wrong conversion to short
264bcbc Added 32 bit angle
f190dd7 Merge branch 'DiscreteAngle' into Experimental
ae55a24 Further  vector3 conversion fixes
ec0cc47 Fix spherical to vector
89b5da8 Fix ambiguity
e62fa96 Fix specialization again
430344a Fix specialization error
3e5ede0 Removed old file
e62bacc First Discrete Angle functions
87d2c11 Set strict warnings
5141766 Add spherical.toVector3 test (and fixes)
8e3b9e2 Fix Polar test
7b85556 Addedfirst Polar test
395f82d Fix Spherical Unit test
0cbef79 Add first Spherical Unit test
dc601a1 Removed Vector2.tofactor
159bdae Added spherical.tovector (but is is buggy)
273ebae Fixes
0468031 Bug fix
3714507 Implemented templated angles
69e7ee1 Added DiscreteAgle
2cbf259 Make negation virtual override
2b50bab Add negation for DiscreteAngle
de3a647 Updated negation
254bb27 Added negation
bd04285 Make superclass accessable
2bb0009 Initial implementation

git-subtree-dir: LinearAlgebra
git-subtree-split: 8b6f628d0c6b9e72260ce1d9c0437124c2d7b6d8
2025-05-26 15:26:44 +02:00
90e89b2ecd Squashed 'LinearAlgebra/' changes from bfa1123..72f1472
72f1472 Fix missing <chrono> include
54634f0 Backporting some style changes from Python
fea15c3 removed namespace Passer
753de2d Fixes
9674f68 Disabled test failing on MacOS
5b89234 Made it work on MacOS
d0ba29e Updated LinearAlgebra
ebfb4fb Merge commit '0b63a044eb84175cc922f3f1fe0ce039d101bf7b'
0b63a04 Updated documentation, cleanup
a0082bd Add direction documentation
770ce32 Merge commit '2b5f5cd175b20fa01e04b696dfe0cfbe2c4ad9da'
2b5f5cd Removed shorthand Deg/Rad aliases... (see below)
d212db2 Trying to fix specialization before instantiation error
e445e5b Trying to fix specialization before instantiation error
f47e504 Trying to fix specialization before instantiation error
b877d4d Trying to fix specialization before instantiation error
06e8623 Trying to fix specialization before instantiation error
e97aee9 Trying to fix specialization before instantiation error
0e00e5d Merge branch 'main' of http://gitlab.passervr.com/passer/cpp/linear-algebra
52de55d Trying to fix specialization before instantiation error
47ba024 Trying to fix specialization before instantiation error
2402030 Trying to fix specialization before instantiation error
4e3f253 Fixed gitignore
06c70e2 Merge branch 'Experimental'
6f30334 Updated PolarOf, removed default Polar type
94a3105 Cleanup
89a5711 Fixed typo in documentation
64b7f11 Removed default Angle type
58beb36 Completed AngleOf documentation
5f58a2c Updated documentation
97d937e Fixed (copilot) unit tests
585a3b0 Removed default Spherical type
d3226fd Added copilot test documentation
a7aa679 Added a performance test
2c57c3f Fixed direction for spherical
28ee225 Normalize Spherical at creation
800eb75 improved rounding of discrete angles
536d6ce Add Binary support
18756ba Correct axis on quaternion from swingtwist
06e42c5 Normalized swing twist
edbb4b1 Removes Angle::deg90/180 because of issues
0a07b23 SwingTwist == and Angle
9406b57 SwingTwist::AngleAxis using Direction
06da9e5 Removed AngleAxis, Sperhical will be used instead
2e61e1b Fix unit test
54d0318 Extended unit tests (plus fixes)
8286c1c Minor improvements
95a6fb3 Improved Angle quality
9eca318 Fixed inverse
0ebfce4 Spherical direction
136e44e Fixed unit tests
fb5cee8 Proper angle implementation (unit tests still fail)
92d5ef0 Normalizing directions
1ea65d5 Made toQuestion const
f1c70c7 Fix unit tests
2ad6a5a Fix include
11259a9 Extend functionality
3363388 Extended AngleAxis & Direction
09e25a8 Add Asin and Atan
a646e93 Add ACos
407e771 Add WithDistance
69bad8f Added degrees, radians, cos, sin, and tan functions
5c3dfa6 Adde degrees/readians and poc ACos
05c61a3 Add distance between
6a1d043 Added Angle comparison
6b3bcfc Another anglebetween fix
b5a6330 Fix anglebetween
b975aed Added angle and rotate functions
f483439 Moved Polar/Spherical to template class completely
e0ef43a Merge branch 'Experimental' of http://gitlab.passervr.com/passer/cpp/linear-algebra into Experimental
b460bec Added scaling to SphericalOf<T>
e10e010 Added Direction to library sources
dedaa31 Fix unit tests
1da93ca Fix ToVector3
9c3503f Added SphericalOf
353cb1b Add conversion from Quaternion
cf86ba8 Cleanup template classes
ea6894e Fix generic template functions
1ce7234 Bug fixes
38ebb34 Added Direction to replace Axis
c72bba4 Change Vector3::Angle return type to AngleOf<float>
18cf653 Added scaling support
51772a1 Added add/subtract for discrete angles
49c6740 Make Angle -> float conversion explicit
608c45c Fix precision error
4591aef Added Spherical16
48828e2 Fix int/float issues
86b7c21 Added Degrees inline function
a4b0491 Fix unit tests
b81b77b Extend AngleOf support
c70c079 Add spherical subtract
2bad384 Added spherical addition
a25a8be Fixed namespace issues
f8009a7 Updated namespace
91c7b20 Fix gitlab test runner
78468e3 Renamed VectorAlgebra project to LinearAlgebra
47a6368 Fix Vector3
ee62cba XYZ deprecation
62d7439 Extend ulitity functions
95713c8 Cleanup, added utility functions
f7d9d97 Cleanup
66e47d7 Cleanup Vector2 and Polar
e922a01 fix == operator
5693097 Initial subtractop test
c411b43 Textual improvement
0c54276 equality support
b1e34b6 Improved unit tests
6ddb074 Improved unit tests
5489b3c Fix test
3d971c1 Improve namespace
791bd78 namespace improvement
9bea94f Use Angle type for Polar
8a2ce21 Swapped polar constructor param order
e5f8d73 namespace fix
8e7a8c6 Fix namespaces
64ca768 normalize toAngleAxis result
4385bef Add angle-axis
4b07328 Add conversion from Vector3
fe12c99 Fix Spherical reference
c274c3e Add conversion from Spherical
66ff721 Fixed wrong conversion to short
264bcbc Added 32 bit angle
f190dd7 Merge branch 'DiscreteAngle' into Experimental
ae55a24 Further  vector3 conversion fixes
ec0cc47 Fix spherical to vector
89b5da8 Fix ambiguity
e62fa96 Fix specialization again
430344a Fix specialization error
3e5ede0 Removed old file
e62bacc First Discrete Angle functions
87d2c11 Set strict warnings
5141766 Add spherical.toVector3 test (and fixes)
8e3b9e2 Fix Polar test
7b85556 Addedfirst Polar test
395f82d Fix Spherical Unit test
0cbef79 Add first Spherical Unit test
dc601a1 Removed Vector2.tofactor
159bdae Added spherical.tovector (but is is buggy)
273ebae Fixes
0468031 Bug fix
3714507 Implemented templated angles
69e7ee1 Added DiscreteAgle
2cbf259 Make negation virtual override
2b50bab Add negation for DiscreteAngle
de3a647 Updated negation
254bb27 Added negation
bd04285 Make superclass accessable
2bb0009 Initial implementation

git-subtree-dir: LinearAlgebra
git-subtree-split: 72f1472f2e1d2c9a1fe002460e47606b9aeed548
2025-05-26 14:04:37 +02:00
0920f99c1e Fix tests 2025-05-26 12:27:09 +02:00
6e860be913 Updated BB2B example 2025-05-26 09:22:59 +02:00
8620023277 Working controlled motor 2025-05-23 17:57:14 +02:00
140138977a AR mode works, but reaction time is slow 2025-05-21 15:59:11 +02:00
1b5fef15e6 More refactoring to get rid of pointers 2025-05-19 15:58:35 +02:00
da8831a968 Added controlledMotor, ptr to ref for hierarchy 2025-05-19 08:55:00 +02:00
Pascal Serrarens
5b5513971c Reduced the use of raw pointers 2025-05-18 17:34:52 +02:00
Pascal Serrarens
3f8eadf8cc Added Root & new Thing ref. constructor 2025-05-17 18:09:38 +02:00
17de9db0be Added rel. encoder, removed currentTime in Update() 2025-05-16 11:54:40 +02:00
Pascal Serrarens
26be4557c5 Added thing reconstruct 2025-05-15 19:52:24 +02:00
31c725b0ca BB2B with HW works 2025-05-15 15:59:43 +02:00
169d842143 Step2 of renaming examples map 2025-05-14 12:40:23 +02:00
ada8973964 step 1 of changing examples map 2025-05-14 12:39:54 +02:00
38fd8f51c8 Added readme about the examples folder name 2025-05-14 12:36:06 +02:00
23ea03fbaf Ultrasonic TouchSensor subclass 2025-05-14 12:04:29 +02:00
40aab4dc7a fix NO_STD issues 2025-05-13 09:38:30 +02:00
53 changed files with 1017 additions and 378 deletions

View File

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

View File

@ -5,6 +5,39 @@
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
@ -13,7 +46,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
unsigned char pinIn1,
unsigned char pinIn2,
bool reverse)
: Thing(driver->owner) {
: Motor() {
this->SetParent(driver);
this->pinIn1 = pinIn1;
@ -33,33 +66,20 @@ 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::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)
@ -109,43 +129,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(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);
}
#pragma endregion Motor
} // namespace Arduino
} // namespace RoboidControl

View File

@ -1,8 +1,10 @@
#pragma once
#include <Arduino.h>
#include "Participants/IsolatedParticipant.h"
#include "Thing.h"
#include "Things/DifferentialDrive.h"
#include "Things/Motor.h"
namespace RoboidControl {
namespace Arduino {
@ -11,44 +13,46 @@ class DRV8833Motor;
class DRV8833 : public Thing {
public:
struct Configuration {
int AIn1;
int AIn2;
int BIn1;
int BIn2;
int standby = 255;
};
/// @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(Thing* parent,
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;
};
/// @brief Support for a DRV8833 motor controller
class DRV8833Motor : public Thing {
public:
/// @brief Motor turning direction
enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
#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 {
public:
/// @brief Setup the DC motor
/// @param pinIn1 the pin number for the in1 signal
/// @param pinIn2 the pin number for the in2 signal
@ -57,16 +61,16 @@ class DRV8833Motor : public Thing {
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;
bool reverse = false;
// 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;
// unsigned int maxRpm = 200;
#if (ESP32)
uint8_t in1Ch;
@ -75,5 +79,7 @@ class DRV8833Motor : public Thing {
#endif
};
#pragma endregion Motor
} // namespace Arduino
} // namespace RoboidControl

View File

@ -5,19 +5,125 @@
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(Type::Undetermined, parent) {
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->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) {
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

View File

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

View File

@ -1,39 +1,32 @@
#include "UltrasonicSensor.h"
#include <Arduino.h>
#include <iostream>
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(Type::Undetermined, parent) {
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
}
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(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
@ -44,32 +37,38 @@ 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 > 0 && this->distance <= this->touchDistance);
// std::cout << "Ultrasonic " << this->touchedSomething << " | " << (this->distance > 0) << " " <<
// (this->distance <= this->touchDistance) << "\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 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
} // namespace Arduino
} // namespace RoboidControl

View File

@ -6,26 +6,19 @@ namespace RoboidControl {
namespace Arduino {
/// @brief An HC-SR04 ultrasonic distance sensor
class UltrasonicSensor : public TouchSensor {
class UltrasonicSensor : Thing {
public:
// Inherit all constructors
//using TouchSensor::TouchSensor;
struct Configuration {
int trigger;
int echo;
};
/// @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);
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
@ -37,15 +30,35 @@ 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 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

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(

View File

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

View File

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

View File

@ -139,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++) {
@ -206,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;

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -2,6 +2,7 @@
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include <chrono>
#include "Polar.h"
#include "Spherical.h"

View File

@ -2,6 +2,7 @@
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include <chrono>
#include "Spherical.h"
#include "Vector3.h"

View File

@ -2,6 +2,7 @@
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include <chrono>
#include "Spherical.h"

View File

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

View File

@ -1,5 +1,9 @@
#include "ParticipantMsg.h"
#if !defined(NO_STD)
#include <iostream>
#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

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->GetParent() != nullptr)) {
if (thing->positionUpdated || (force && thing->IsRoot())) {
this->position = thing->GetPosition();
this->poseType |= Pose_Position;
}
if (thing->orientationUpdated || (force && thing->GetParent() != nullptr)) {
if (thing->orientationUpdated || (force && thing->IsRoot())) {
this->orientation = thing->GetOrientation();
this->poseType |= Pose_Orientation;
}

View File

@ -1,5 +1,9 @@
#include "TextMsg.h"
#if !defined(NO_STD)
#include <iostream>
#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;

View File

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

View File

@ -8,7 +8,26 @@ 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;
@ -26,14 +45,14 @@ Participant::Participant(const char* ipAddress, int port) {
}
Participant::~Participant() {
registry.Remove(this);
// 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);
}
}
@ -112,23 +131,26 @@ void Participant::Remove(Thing* thing) {
#pragma region ParticipantRegistry
Participant* ParticipantRegistry::Get(const char* ipAddress,
unsigned int port) {
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;
// 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;
@ -136,11 +158,12 @@ 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;
@ -152,19 +175,22 @@ 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";
}
}
@ -172,9 +198,15 @@ 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

View File

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

View File

@ -1,30 +1,49 @@
#include "ParticipantUDP.h"
#include "Participant.h"
#include "Thing.h"
#include "Arduino/ArduinoParticipant.h"
#include "EspIdf/EspIdfParticipant.h"
#include "Windows/WindowsParticipant.h"
#include "Posix/PosixParticipant.h"
#include "Windows/WindowsParticipant.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;
@ -64,11 +83,18 @@ void ParticipantUDP::SetupUDP(int localPort,
this->connected = true;
}
#pragma endregion Init
#pragma region Update
void ParticipantUDP::Update(unsigned long currentTimeMs) {
if (currentTimeMs == 0)
currentTimeMs = Thing::GetTimeMs();
// The update order
// 1. receive external messages
// 2. update the state
// 3. send out the updated messages
void ParticipantUDP::Update() {
unsigned long currentTimeMs = Thing::GetTimeMs();
PrepMyThings();
if (this->isIsolated == false) {
if (this->connected == false)
@ -88,15 +114,26 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
this->ReceiveUDP();
}
UpdateMyThings(currentTimeMs);
UpdateOtherThings(currentTimeMs);
UpdateMyThings();
UpdateOtherThings();
}
void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) {
void ParticipantUDP::PrepMyThings() {
for (Thing* thing : this->things) {
if (thing == nullptr)
continue;
thing->PrepareForUpdate();
}
}
void ParticipantUDP::UpdateMyThings() {
// std::cout << this->things.size() << std::endl;
for (Thing* thing : this->things) {
if (thing == nullptr) // || thing->GetParent() != nullptr)
continue;
// std::cout << thing->name << "\n";
if (thing->hierarchyChanged) {
if (!(this->isIsolated || this->networkId == 0)) {
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
@ -111,12 +148,14 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) {
}
}
// 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(currentTimeMs, false);
thing->Update(false);
// std::cout << "C\n";
if (!(this->isIsolated || this->networkId == 0)) {
if (thing->terminate) {
DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
@ -137,20 +176,28 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) {
delete binaryMsg;
}
}
// std::cout << "D\n";
if (thing->terminate)
this->Remove(thing);
// std::cout << "E\n";
}
}
void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) {
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
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(currentTimeMs);
participant->Participant::Update();
if (this->isIsolated)
continue;
@ -195,8 +242,8 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
if (bufferSize <= 0)
return true;
// std::cout << "send msg " << (int)this->buffer[0] << " to "
// << remoteParticipant->ipAddress << std::endl;
// std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
// << " to " << remoteParticipant->ipAddress << std::endl;
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
@ -373,14 +420,14 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process SiteMsg " << (int)this->networkId
std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
<< " -> " << (int)msg->networkId << "\n";
#endif
if (this->networkId != msg->networkId) {
this->networkId = msg->networkId;
// std::cout << this->things.size() << " things\n";
std::cout << this->things.size() << " things\n";
for (Thing* thing : this->things)
this->SendThingInfo(sender, thing);
}
@ -441,7 +488,7 @@ 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 << "] " << (int)msg->poseType << "\n";
#endif

View File

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

View File

@ -22,16 +22,22 @@ SiteServer::SiteServer(int port) : ParticipantUDP(port) {
#pragma region Update
void SiteServer::UpdateMyThings(unsigned long currentTimeMs) {
void SiteServer::UpdateMyThings() {
for (Thing* thing : this->things) {
if (thing == nullptr)
continue;
thing->Update(currentTimeMs, true);
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;
@ -66,15 +72,24 @@ 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);
// 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->GetParent() != nullptr)
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(nullptr);
thing->SetParent(Thing::LocalRoot());
}
#pragma endregion Receive

View File

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

View File

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

View File

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

View File

@ -5,6 +5,7 @@
#include "Participants/IsolatedParticipant.h"
#include <string.h>
// #include <iostream>
#if defined(ARDUINO)
#include "Arduino.h"
@ -19,14 +20,31 @@ namespace RoboidControl {
#pragma region Init
Thing::Thing(unsigned char thingType)
: Thing(IsolatedParticipant::Isolated(), thingType) {}
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(Participant* owner,
unsigned char thingType,
unsigned char thingId) {
this->owner = owner;
this->id = thingId;
//this->owner->Add(this, true);
std::cout << this->owner->name << ": New root thing " << std::endl;
}
Thing::Thing(unsigned char thingType, Thing* parent) {
this->type = thingType;
this->position = Spherical::zero;
@ -38,17 +56,25 @@ Thing::Thing(Participant* owner,
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 = parent->owner;
this->owner->Add(this, true);
this->SetParent(parent);
std::cout << this->owner->name << ": New thing for " << parent->name
<< std::endl;
}
Thing::Thing(Thing* parent, unsigned char thingType, unsigned char thingId)
: Thing(parent->owner, thingType, thingId) {
this->SetParent(parent);
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) {
@ -77,6 +103,19 @@ 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)
@ -130,7 +169,7 @@ Thing* Thing::RemoveChild(Thing* child) {
}
}
child->parent = nullptr;
child->parent = Thing::LocalRoot();
delete[] this->children;
this->children = newChildren;
@ -221,7 +260,8 @@ Spherical Thing::GetAngularVelocity() {
unsigned long Thing::GetTimeMs() {
#if defined(ARDUINO)
return millis();
unsigned long ms = millis();
return ms;
#else
auto now = std::chrono::steady_clock::now();
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
@ -230,11 +270,13 @@ unsigned long Thing::GetTimeMs() {
#endif
}
void Thing::Update(bool recursive) {
Update(GetTimeMs(), recursive);
}
// void Thing::Update(bool recursive) {
// Update(GetTimeMs(), recursive);
// }
void Thing::Update(unsigned long currentTimeMs, bool recursive) {
void Thing::PrepareForUpdate() {}
void Thing::Update(bool recursive) {
// if (this->positionUpdated || this->orientationUpdated)
// OnPoseChanged callback
this->positionUpdated = false;
@ -245,17 +287,18 @@ void Thing::Update(unsigned long currentTimeMs, 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(currentTimeMs, recursive);
child->Update(recursive);
}
}
}
void Thing::UpdateThings(unsigned long currentTimeMs) {
IsolatedParticipant::Isolated()->Update(currentTimeMs);
void Thing::UpdateThings() {
IsolatedParticipant::Isolated()->Update();
}
#pragma endregion Update

60
Thing.h
View File

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

View File

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

40
Things/ControlledMotor.h Normal file
View File

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

View File

@ -1,13 +1,28 @@
#include "DifferentialDrive.h"
#include "Messages/LowLevelMessages.h"
namespace RoboidControl {
DifferentialDrive::DifferentialDrive() : Thing(Thing::Type::DifferentialDrive) {}
DifferentialDrive::DifferentialDrive(Thing* parent)
: Thing(Type::DifferentialDrive, parent) {
this->name = "Differential drive";
DifferentialDrive::DifferentialDrive(Participant* participant, unsigned char thingId)
: Thing(participant, Thing::Type::DifferentialDrive, thingId) {}
this->leftWheel = new Motor(this);
this->leftWheel->name = "Left motor";
DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) : Thing(parent, 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;
}
void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
float wheelSeparation) {
@ -24,27 +39,38 @@ 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) {
void DifferentialDrive::Update(bool recursive) {
if (this->linearVelocityUpdated) {
// this assumes forward velocity only....
float linearVelocity = this->GetLinearVelocity().distance;
@ -65,7 +91,14 @@ void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
this->SetWheelVelocity(speedLeft, speedRight);
}
Thing::Update(currentMs, recursive);
Thing::Update(recursive);
}
int DifferentialDrive::GenerateBinary(char* data, unsigned char* ix) {
data[(*ix)++] = this->leftWheel->id;
data[(*ix)++] = this->rightWheel->id;
LowLevelMessages::SendFloat16(data, ix, this->wheelRadius);
return 4;
}
} // namespace RoboidControl

View File

@ -1,6 +1,7 @@
#pragma once
#include "Thing.h"
#include "Motor.h"
namespace RoboidControl {
@ -9,18 +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
/// @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, unsigned char thingId = 0);
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
@ -30,10 +26,13 @@ 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.
@ -43,21 +42,25 @@ class DifferentialDrive : public Thing {
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

View File

@ -2,13 +2,18 @@
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(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) {}
int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
bytes[(*ix)++] = state ? 1 : 0;

View File

@ -8,17 +8,18 @@ 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, unsigned char thingId = 0);
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); //, unsigned char thingId = 0);
DigitalSensor(Thing* parent = Thing::LocalRoot());
/// @brief The sigital state
bool state = 0;

16
Things/Motor.cpp Normal file
View File

@ -0,0 +1,16 @@
#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 Normal file
View File

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

View File

@ -0,0 +1,21 @@
#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 Normal file
View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -17,26 +17,26 @@ 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.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)

1
examples/README.md Normal file
View File

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

View File

@ -7,6 +7,9 @@
#include "Participants/SiteServer.h"
#include "Thing.h"
#include <chrono>
#include <thread>
using namespace RoboidControl;
TEST(Participant, Participant) {
@ -15,7 +18,7 @@ TEST(Participant, Participant) {
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
participant->Update(milliseconds);
participant->Update();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();
@ -29,7 +32,7 @@ TEST(Participant, SiteServer) {
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
siteServer->Update(milliseconds);
siteServer->Update();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();
@ -44,8 +47,8 @@ TEST(Participant, SiteParticipant) {
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
siteServer->Update(milliseconds);
participant->Update(milliseconds);
siteServer->Update();
participant->Update();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();
@ -56,15 +59,15 @@ TEST(Participant, SiteParticipant) {
TEST(Participant, ThingMsg) {
SiteServer* siteServer = new SiteServer(7681);
ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682);
Thing* thing = new Thing(participant);
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(milliseconds);
participant->Update(milliseconds);
siteServer->Update();
participant->Update();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();

View File

@ -15,7 +15,7 @@ TEST(RoboidControlSuite, HiddenParticipant) {
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 1000) {
Thing::UpdateThings(milliseconds);
Thing::UpdateThings();
milliseconds = Thing::GetTimeMs();
}
@ -24,12 +24,12 @@ TEST(RoboidControlSuite, HiddenParticipant) {
TEST(RoboidControlSuite, IsolatedParticipant) {
ParticipantUDP* participant = ParticipantUDP::Isolated();
Thing* thing = new Thing(participant);
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();
}