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

Merge commit 'a6a91798b24552522373414d43981b082025cfdd' into ControlCore

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-07 11:59:48 +01:00
commit 8ce608ae2a
38 changed files with 677 additions and 496 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Arduino/Things/DigitalInput.cpp
View File

@ -5,16 +5,18 @@
namespace RoboidControl {
namespace Arduino {
DigitalInput::DigitalInput(Participant* participant, unsigned char pin) : TouchSensor(participant) {
DigitalInput::DigitalInput(Participant* participant, unsigned char pin)
: TouchSensor(participant) {
this->pin = pin;
pinMode(pin, INPUT);
}
void DigitalInput::Update(unsigned long currentTimeMs) {
void DigitalInput::Update(unsigned long currentTimeMs, bool recursive) {
this->touchedSomething = digitalRead(pin) == LOW;
// std::cout << "DigitalINput pin " << (int)this->pin << ": " << this->touchedSomething << "\n";
// std::cout << "DigitalINput pin " << (int)this->pin << ": " <<
// this->touchedSomething << "\n";
Thing::Update(currentTimeMs, recursive);
}
} // namespace Arduino

3
Arduino/Things/DigitalInput.h
View File

@ -14,7 +14,8 @@ class DigitalInput : public TouchSensor {
DigitalInput(Participant* participant, unsigned char pin);
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(unsigned long currentTimeMs) override;
virtual void Update(unsigned long currentTimeMs,
bool recursive = false) override;
protected:
/// @brief The pin used for digital input

13
Arduino/Things/UltrasonicSensor.cpp
View File

@ -5,7 +5,9 @@
namespace RoboidControl {
namespace Arduino {
UltrasonicSensor::UltrasonicSensor(Participant* participant, unsigned char pinTrigger, unsigned char pinEcho)
UltrasonicSensor::UltrasonicSensor(Participant* participant,
unsigned char pinTrigger,
unsigned char pinEcho)
: TouchSensor(participant) {
this->pinTrigger = pinTrigger;
this->pinEcho = pinEcho;
@ -23,7 +25,8 @@ float UltrasonicSensor::GetDistance() {
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
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
@ -43,14 +46,16 @@ float UltrasonicSensor::GetDistance() {
this->touchedSomething |= (this->distance <= this->touchDistance);
// std::cout << "Ultrasonic " << this->distance << " " << this->touchedSomething << "\n";
// std::cout << "Ultrasonic " << this->distance << " " <<
// this->touchedSomething << "\n";
return distance;
}
void UltrasonicSensor::Update(unsigned long currentTimeMs) {
void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
this->touchedSomething = false;
GetDistance();
Thing::Update(currentTimeMs, recursive);
}
// void UltrasonicSensor::ProcessBinary(char* bytes) {

10
Arduino/Things/UltrasonicSensor.h
View File

@ -12,7 +12,9 @@ class UltrasonicSensor : public TouchSensor {
/// @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(Participant* participant,
unsigned char pinTrigger,
unsigned char pinEcho);
// parameters
@ -23,12 +25,14 @@ class UltrasonicSensor : public TouchSensor {
/// @brief The last read distance
float distance = 0;
/// @brief erform an ultrasonic 'ping' to determine the distance to the nearest object
/// @brief erform an ultrasonic 'ping' to determine the distance to the
/// nearest object
/// @return the measured distance in meters to the nearest object
float GetDistance();
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(unsigned long currentTimeMs) override;
virtual void Update(unsigned long currentTimeMs,
bool recursive = false) override;
protected:
/// @brief The pin number of the trigger signal

10
CMakeLists.txt
View File

@ -5,8 +5,9 @@ if(ESP_PLATFORM)
INCLUDE_DIRS "."
)
else()
project(RoboidCOntrol)
project(RoboidControl)
add_subdirectory(LinearAlgebra)
add_subdirectory(Examples)
set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
@ -51,13 +52,6 @@ else()
LinearAlgebra
)
# if(MSVC)
# target_compile_options(RoboidControlTest PRIVATE /W4 /WX)
# # else()
# # target_compile_options(RoboidControlTest PRIVATE -Wall -Wextra -Wpedantic -Werror)
# endif()
include(GoogleTest)
gtest_discover_tests(RoboidControlTest)
endif()

50
Examples/BB2B.cpp Normal file
View File

@ -0,0 +1,50 @@
#include "Thing.h"
#include "Things/DifferentialDrive.h"
#include "Things/TouchSensor.h"
#if defined(ARDUINO)
#include "Arduino.h"
#else
#include <chrono>
#include <thread>
using namespace std::this_thread;
using namespace std::chrono;
#endif
using namespace RoboidControl;
int main() {
// The robot's propulsion is a differential drive
DifferentialDrive* bb2b = new DifferentialDrive();
// Is has a touch sensor at the front left of the roboid
TouchSensor* touchLeft = new TouchSensor(bb2b);
// and other one on the right
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;
// The right wheel does the same, but instead is controlled by
// touches on the left side
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);
// Update the roboid state
bb2b->Update(true);
// and sleep for 100ms
#if defined(ARDUINO)
delay(100);
#else
sleep_for(milliseconds(100));
#endif
}
return 0;
}

25
Examples/CMakeLists.txt Normal file
View File

@ -0,0 +1,25 @@
# examples/CMakeLists.txt
# Specify the minimum CMake version
cmake_minimum_required(VERSION 3.10)
# Specify the path to the main project directory
set(MAIN_PROJECT_DIR "${CMAKE_SOURCE_DIR}/..")
# Set the project name
project(Examples)
include_directories(..)
# Add the executable for the main project
#add_executable(MainExecutable ${SOURCES})
# Find the main project library (assuming it's defined in the root CMakeLists.txt)
#find_package(RoboidControl REQUIRED) # Replace MyLibrary with your actual library name
# Add example executables
add_executable(BB2B BB2B.cpp)
target_link_libraries(
BB2B
RoboidControl
LinearAlgebra
)

113
LinearAlgebra/Angle.cpp
View File

@ -3,15 +3,15 @@
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "Angle.h"
#include "FloatSingle.h"
#include <math.h>
#include "FloatSingle.h"
const float Rad2Deg = 57.29578F;
const float Deg2Rad = 0.0174532924F;
namespace LinearAlgebra {
//===== AngleSingle, AngleOf<float>
template <> AngleOf<float> Passer::LinearAlgebra::AngleOf<float>::Degrees(float degrees) {
template <>
AngleOf<float> AngleOf<float>::Degrees(float degrees) {
if (isfinite(degrees)) {
while (degrees < -180)
degrees += 360;
@ -22,7 +22,8 @@ template <> AngleOf<float> Passer::LinearAlgebra::AngleOf<float>::Degrees(float
return AngleOf<float>(degrees);
}
template <> AngleOf<float> AngleOf<float>::Radians(float radians) {
template <>
AngleOf<float> AngleOf<float>::Radians(float radians) {
if (isfinite(radians)) {
while (radians <= -pi)
radians += 2 * pi;
@ -33,9 +34,13 @@ template <> AngleOf<float> AngleOf<float>::Radians(float radians) {
return Binary(radians * Rad2Deg);
}
template <> float AngleOf<float>::InDegrees() const { return this->value; }
template <>
float AngleOf<float>::InDegrees() const {
return this->value;
}
template <> float AngleOf<float>::InRadians() const {
template <>
float AngleOf<float>::InRadians() const {
return this->value * Deg2Rad;
}
@ -58,25 +63,29 @@ AngleOf<signed short> AngleOf<signed short>::Radians(float radians) {
return Binary(value);
}
template <> float AngleOf<signed short>::InDegrees() const {
template <>
float AngleOf<signed short>::InDegrees() const {
float degrees = this->value / 65536.0f * 360.0f;
return degrees;
}
template <> float AngleOf<signed short>::InRadians() const {
template <>
float AngleOf<signed short>::InRadians() const {
float radians = this->value / 65536.0f * (2 * pi);
return radians;
}
//===== Angle8, AngleOf<signed char>
template <> AngleOf<signed char> AngleOf<signed char>::Degrees(float degrees) {
template <>
AngleOf<signed char> AngleOf<signed char>::Degrees(float degrees) {
// map float [-180..180) to integer [-128..127)
signed char value = (signed char)roundf(degrees / 360.0F * 256.0F);
return Binary(value);
}
template <> AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
template <>
AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
if (!isfinite(radians))
return AngleOf<signed char>::zero;
@ -85,32 +94,42 @@ template <> AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
return Binary(value);
}
template <> float AngleOf<signed char>::InDegrees() const {
template <>
float AngleOf<signed char>::InDegrees() const {
float degrees = this->value / 256.0f * 360.0f;
return degrees;
}
template <> float AngleOf<signed char>::InRadians() const {
template <>
float AngleOf<signed char>::InRadians() const {
float radians = this->value / 128.0f * pi;
return radians;
}
//===== Generic
template <typename T> AngleOf<T>::AngleOf() : value(0) {}
template <typename T>
AngleOf<T>::AngleOf() : value(0) {}
template <typename T> AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}
template <typename T>
AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}
template <typename T> const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
template <typename T>
const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
template <typename T> AngleOf<T> AngleOf<T>::Binary(T rawValue) {
template <typename T>
AngleOf<T> AngleOf<T>::Binary(T rawValue) {
AngleOf<T> angle = AngleOf<T>();
angle.SetBinary(rawValue);
return angle;
}
template <typename T> T AngleOf<T>::GetBinary() const { return this->value; }
template <typename T> void AngleOf<T>::SetBinary(T rawValue) {
template <typename T>
T AngleOf<T>::GetBinary() const {
return this->value;
}
template <typename T>
void AngleOf<T>::SetBinary(T rawValue) {
this->value = rawValue;
}
@ -119,24 +138,28 @@ bool AngleOf<T>::operator==(const AngleOf<T> angle) const {
return this->value == angle.value;
}
template <typename T> bool AngleOf<T>::operator>(AngleOf<T> angle) const {
template <typename T>
bool AngleOf<T>::operator>(AngleOf<T> angle) const {
return this->value > angle.value;
}
template <typename T> bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
template <typename T>
bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
return this->value >= angle.value;
}
template <typename T> bool AngleOf<T>::operator<(AngleOf<T> angle) const {
template <typename T>
bool AngleOf<T>::operator<(AngleOf<T> angle) const {
return this->value < angle.value;
}
template <typename T> bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
template <typename T>
bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
return this->value <= angle.value;
}
template <typename T>
signed int Passer::LinearAlgebra::AngleOf<T>::Sign(AngleOf<T> angle) {
signed int AngleOf<T>::Sign(AngleOf<T> angle) {
if (angle.value < 0)
return -1;
if (angle.value > 0)
@ -145,14 +168,15 @@ signed int Passer::LinearAlgebra::AngleOf<T>::Sign(AngleOf<T> angle) {
}
template <typename T>
AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Abs(AngleOf<T> angle) {
AngleOf<T> AngleOf<T>::Abs(AngleOf<T> angle) {
if (Sign(angle) < 0)
return -angle;
else
return angle;
}
template <typename T> AngleOf<T> AngleOf<T>::operator-() const {
template <typename T>
AngleOf<T> AngleOf<T>::operator-() const {
AngleOf<T> angle = Binary(-this->value);
return angle;
}
@ -206,7 +230,8 @@ AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T> &angle) {
// return AngleOf::Degrees((float)factor * angle.InDegrees());
// }
template <typename T> void AngleOf<T>::Normalize() {
template <typename T>
void AngleOf<T>::Normalize() {
float angleValue = this->InDegrees();
if (!isfinite(angleValue))
return;
@ -218,7 +243,8 @@ template <typename T> void AngleOf<T>::Normalize() {
*this = AngleOf::Degrees(angleValue);
}
template <typename T> AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
template <typename T>
AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
float angleValue = angle.InDegrees();
if (!isfinite(angleValue))
return angle;
@ -237,7 +263,8 @@ AngleOf<T> AngleOf<T>::Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max) {
}
template <typename T>
AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
AngleOf<T> toAngle,
float maxDegrees) {
maxDegrees = fmaxf(0, maxDegrees); // filter out negative distances
AngleOf<T> d = toAngle - fromAngle;
@ -249,28 +276,34 @@ AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
return fromAngle + d;
}
template <typename T> float AngleOf<T>::Cos(AngleOf<T> angle) {
template <typename T>
float AngleOf<T>::Cos(AngleOf<T> angle) {
return cosf(angle.InRadians());
}
template <typename T> float AngleOf<T>::Sin(AngleOf<T> angle) {
template <typename T>
float AngleOf<T>::Sin(AngleOf<T> angle) {
return sinf(angle.InRadians());
}
template <typename T> float AngleOf<T>::Tan(AngleOf<T> angle) {
template <typename T>
float AngleOf<T>::Tan(AngleOf<T> angle) {
return tanf(angle.InRadians());
}
template <typename T> AngleOf<T> AngleOf<T>::Acos(float f) {
template <typename T>
AngleOf<T> AngleOf<T>::Acos(float f) {
return AngleOf<T>::Radians(acosf(f));
}
template <typename T> AngleOf<T> AngleOf<T>::Asin(float f) {
template <typename T>
AngleOf<T> AngleOf<T>::Asin(float f) {
return AngleOf<T>::Radians(asinf(f));
}
template <typename T> AngleOf<T> AngleOf<T>::Atan(float f) {
template <typename T>
AngleOf<T> AngleOf<T>::Atan(float f) {
return AngleOf<T>::Radians(atanf(f));
}
template <typename T>
AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Atan2(float y, float x) {
AngleOf<T> AngleOf<T>::Atan2(float y, float x) {
return AngleOf<T>::Radians(atan2f(y, x));
}
@ -354,6 +387,8 @@ AngleOf<T> AngleOf<T>::SineRuleAngle(float a, AngleOf<T> beta, float b) {
return alpha;
}
template class Passer::LinearAlgebra::AngleOf<float>;
template class Passer::LinearAlgebra::AngleOf<signed char>;
template class Passer::LinearAlgebra::AngleOf<signed short>;
template class AngleOf<float>;
template class AngleOf<signed char>;
template class AngleOf<signed short>;
} // namespace LinearAlgebra

15
LinearAlgebra/Angle.h
View File

@ -5,7 +5,6 @@
#ifndef ANGLE_H
#define ANGLE_H
namespace Passer {
namespace LinearAlgebra {
static float pi = 3.1415927410125732421875F;
@ -18,7 +17,8 @@ static float Deg2Rad = (pi * 2) / 360.0f;
/// The angle is internally limited to (-180..180] degrees or (-PI...PI]
/// radians. When an angle exceeds this range, it is normalized to a value
/// within the range.
template <typename T> class AngleOf {
template <typename T>
class AngleOf {
public:
/// @brief Create a new angle with a zero value
AngleOf<T>();
@ -150,7 +150,8 @@ public:
/// @param toAngle The angle to rotate towards
/// @param maxAngle The maximum angle to rotate
/// @return The rotated angle
static AngleOf<T> MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
static AngleOf<T> MoveTowards(AngleOf<T> fromAngle,
AngleOf<T> toAngle,
float maxAngle);
/// @brief Calculates the cosine of an angle
@ -215,8 +216,12 @@ using AngleSingle = AngleOf<float>;
using Angle16 = AngleOf<signed short>;
using Angle8 = AngleOf<signed char>;
#if defined(ARDUINO)
using Angle = Angle16;
#else
using Angle = AngleSingle;
#endif
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

29
LinearAlgebra/Direction.cpp
View File

@ -9,7 +9,8 @@
#include <math.h>
template <typename T> DirectionOf<T>::DirectionOf() {
template <typename T>
DirectionOf<T>::DirectionOf() {
this->horizontal = AngleOf<T>();
this->vertical = AngleOf<T>();
}
@ -41,7 +42,7 @@ const DirectionOf<T> DirectionOf<T>::right =
DirectionOf<T>(AngleOf<T>::Degrees(90), AngleOf<T>());
template <typename T>
Vector3 Passer::LinearAlgebra::DirectionOf<T>::ToVector3() const {
Vector3 DirectionOf<T>::ToVector3() const {
Quaternion q = Quaternion::Euler(-this->vertical.InDegrees(),
this->horizontal.InDegrees(), 0);
Vector3 v = q * Vector3::forward;
@ -49,12 +50,12 @@ Vector3 Passer::LinearAlgebra::DirectionOf<T>::ToVector3() const {
}
template <typename T>
DirectionOf<T>
Passer::LinearAlgebra::DirectionOf<T>::FromVector3(Vector3 vector) {
DirectionOf<T> DirectionOf<T>::FromVector3(Vector3 vector) {
DirectionOf<T> d;
d.horizontal = AngleOf<T>::Atan2(
vector.Right(),
vector.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
vector
.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
d.vertical =
AngleOf<T>::Degrees(-90) -
AngleOf<T>::Acos(
@ -64,34 +65,32 @@ Passer::LinearAlgebra::DirectionOf<T>::FromVector3(Vector3 vector) {
}
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Degrees(float horizontal,
float vertical) {
DirectionOf<T> DirectionOf<T>::Degrees(float horizontal, float vertical) {
return DirectionOf<T>(AngleOf<T>::Degrees(horizontal),
AngleOf<T>::Degrees(vertical));
}
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Radians(float horizontal,
float vertical) {
DirectionOf<T> DirectionOf<T>::Radians(float horizontal, float vertical) {
return DirectionOf<T>(AngleOf<T>::Radians(horizontal),
AngleOf<T>::Radians(vertical));
}
template <typename T>
bool Passer::LinearAlgebra::DirectionOf<T>::operator==(
const DirectionOf<T> direction) const {
bool DirectionOf<T>::operator==(const DirectionOf<T> direction) const {
return (this->horizontal == direction.horizontal) &&
(this->vertical == direction.vertical);
}
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator-() const {
DirectionOf<T> DirectionOf<T>::operator-() const {
DirectionOf<T> r = DirectionOf<T>(this->horizontal + AngleOf<T>::Degrees(180),
-this->vertical);
return r;
}
template <typename T> void DirectionOf<T>::Normalize() {
template <typename T>
void DirectionOf<T>::Normalize() {
if (this->vertical > AngleOf<T>::Degrees(90) ||
this->vertical < AngleOf<T>::Degrees(-90)) {
this->horizontal += AngleOf<T>::Degrees(180);
@ -99,5 +98,5 @@ template <typename T> void DirectionOf<T>::Normalize() {
}
}
template class Passer::LinearAlgebra::DirectionOf<float>;
template class Passer::LinearAlgebra::DirectionOf<signed short>;
template class DirectionOf<float>;
template class DirectionOf<signed short>;

8
LinearAlgebra/Direction.h
View File

@ -7,7 +7,6 @@
#include "Angle.h"
namespace Passer {
namespace LinearAlgebra {
struct Vector3;
@ -22,7 +21,8 @@ struct Vector3;
/// rotation has been applied.
/// The angles are automatically normalized to stay within the abovenmentioned
/// ranges.
template <typename T> class DirectionOf {
template <typename T>
class DirectionOf {
public:
/// @brief horizontal angle, range= (-180..180]
AngleOf<T> horizontal;
@ -98,7 +98,7 @@ using Direction = DirectionSingle;
#endif
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
using namespace LinearAlgebra;
#endif

5
LinearAlgebra/FloatSingle.h
View File

@ -5,7 +5,6 @@
#ifndef FLOAT_H
#define FLOAT_H
namespace Passer {
namespace LinearAlgebra {
class Float {
@ -17,7 +16,7 @@ public:
};
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
using namespace LinearAlgebra;
#endif

10
LinearAlgebra/Matrix.h
View File

@ -3,11 +3,11 @@
#include "Vector3.h"
namespace Passer {
namespace LinearAlgebra {
/// @brief Single precision float matrix
template <typename T> class MatrixOf {
template <typename T>
class MatrixOf {
public:
MatrixOf(unsigned int rows, unsigned int cols);
MatrixOf(unsigned int rows, unsigned int cols, const T* source)
@ -54,7 +54,8 @@ public:
}
}
static void Multiply(const MatrixOf<T> *m1, const MatrixOf<T> *m2,
static void Multiply(const MatrixOf<T>* m1,
const MatrixOf<T>* m2,
MatrixOf<T>* r);
void Multiply(const MatrixOf<T>* m, MatrixOf<T>* r) const {
Multiply(this, m, r);
@ -115,7 +116,6 @@ private:
};
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
using namespace LinearAlgebra;
#endif

50
LinearAlgebra/Polar.cpp
View File

@ -3,11 +3,13 @@
#include "Polar.h"
#include "Vector2.h"
template <typename T> PolarOf<T>::PolarOf() {
template <typename T>
PolarOf<T>::PolarOf() {
this->distance = 0.0f;
this->angle = AngleOf<T>();
}
template <typename T> PolarOf<T>::PolarOf(float distance, AngleOf<T> angle) {
template <typename T>
PolarOf<T>::PolarOf(float distance, AngleOf<T> angle) {
// distance should always be 0 or greater
if (distance < 0.0f) {
this->distance = -distance;
@ -34,16 +36,18 @@ PolarOf<T> PolarOf<T>::Radians(float distance, float radians) {
return PolarOf<T>(distance, AngleOf<T>::Radians(radians));
}
template <typename T> PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
template <typename T>
PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
float distance = v.magnitude();
AngleOf<T> angle =
AngleOf<T>::Degrees(Vector2::SignedAngle(Vector2::forward, v));
PolarOf<T> p = PolarOf(distance, angle);
return p;
}
template <typename T> PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
float distance = v.distance * cosf(v.direction.vertical.InDegrees() *
Passer::LinearAlgebra::Deg2Rad);
template <typename T>
PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
float distance =
v.distance * cosf(v.direction.vertical.InDegrees() * Deg2Rad);
AngleOf<T> angle = v.direction.horizontal;
PolarOf<T> p = PolarOf(distance, angle);
return p;
@ -60,31 +64,37 @@ const PolarOf<T> PolarOf<T>::right = PolarOf(1.0, AngleOf<T>::Degrees(90));
template <typename T>
const PolarOf<T> PolarOf<T>::left = PolarOf(1.0, AngleOf<T>::Degrees(-90));
template <typename T> bool PolarOf<T>::operator==(const PolarOf &v) const {
template <typename T>
bool PolarOf<T>::operator==(const PolarOf& v) const {
return (this->distance == v.distance &&
this->angle.InDegrees() == v.angle.InDegrees());
}
template <typename T> PolarOf<T> PolarOf<T>::Normalize(const PolarOf &v) {
template <typename T>
PolarOf<T> PolarOf<T>::Normalize(const PolarOf& v) {
PolarOf<T> r = PolarOf(1, v.angle);
return r;
}
template <typename T> PolarOf<T> PolarOf<T>::normalized() const {
template <typename T>
PolarOf<T> PolarOf<T>::normalized() const {
PolarOf<T> r = PolarOf(1, this->angle);
return r;
}
template <typename T> PolarOf<T> PolarOf<T>::operator-() const {
template <typename T>
PolarOf<T> PolarOf<T>::operator-() const {
PolarOf<T> v =
PolarOf(this->distance, this->angle + AngleOf<T>::Degrees(180));
return v;
}
template <typename T> PolarOf<T> PolarOf<T>::operator-(const PolarOf &v) const {
template <typename T>
PolarOf<T> PolarOf<T>::operator-(const PolarOf& v) const {
PolarOf<T> r = -v;
return *this + r;
}
template <typename T> PolarOf<T> PolarOf<T>::operator-=(const PolarOf &v) {
template <typename T>
PolarOf<T> PolarOf<T>::operator-=(const PolarOf& v) {
*this = *this - v;
return *this;
// angle = AngleOf<T>::Normalize(newAngle);
@ -105,7 +115,8 @@ template <typename T> PolarOf<T> PolarOf<T>::operator-=(const PolarOf &v) {
// return d;
// }
template <typename T> PolarOf<T> PolarOf<T>::operator+(const PolarOf &v) const {
template <typename T>
PolarOf<T> PolarOf<T>::operator+(const PolarOf& v) const {
if (v.distance == 0)
return PolarOf(this->distance, this->angle);
if (this->distance == 0.0f)
@ -133,16 +144,19 @@ template <typename T> PolarOf<T> PolarOf<T>::operator+(const PolarOf &v) const {
PolarOf vector = PolarOf(newDistance, newAngleA);
return vector;
}
template <typename T> PolarOf<T> PolarOf<T>::operator+=(const PolarOf &v) {
template <typename T>
PolarOf<T> PolarOf<T>::operator+=(const PolarOf& v) {
*this = *this + v;
return *this;
}
template <typename T> PolarOf<T> PolarOf<T>::operator*=(float f) {
template <typename T>
PolarOf<T> PolarOf<T>::operator*=(float f) {
this->distance *= f;
return *this;
}
template <typename T> PolarOf<T> PolarOf<T>::operator/=(float f) {
template <typename T>
PolarOf<T> PolarOf<T>::operator/=(float f) {
this->distance /= f;
return *this;
}
@ -161,5 +175,5 @@ PolarOf<T> PolarOf<T>::Rotate(const PolarOf &v, AngleOf<T> angle) {
return r;
}
template class Passer::LinearAlgebra::PolarOf<float>;
template class Passer::LinearAlgebra::PolarOf<signed short>;
template class PolarOf<float>;
template class PolarOf<signed short>;

10
LinearAlgebra/Polar.h
View File

@ -7,15 +7,16 @@
#include "Angle.h"
namespace Passer {
namespace LinearAlgebra {
struct Vector2;
template <typename T> class SphericalOf;
template <typename T>
class SphericalOf;
/// @brief A polar vector using an angle in various representations
/// @tparam T The implementation type used for the representation of the angle
template <typename T> class PolarOf {
template <typename T>
class PolarOf {
public:
/// @brief The distance in meters
/// @remark The distance shall never be negative
@ -153,8 +154,7 @@ using Polar16 = PolarOf<signed short>;
// using Polar = PolarSingle;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
using namespace LinearAlgebra;
#include "Spherical.h"
#include "Vector2.h"

19
LinearAlgebra/Quaternion.h
View File

@ -32,7 +32,6 @@ typedef struct Quat {
} Quat;
}
namespace Passer {
namespace LinearAlgebra {
/// <summary>
@ -157,7 +156,8 @@ public:
/// <param name="to">The destination rotation</param>
/// <param name="maxDegreesDelta">The maximum amount of degrees to
/// rotate</param> <returns>The possibly limited rotation</returns>
static Quaternion RotateTowards(const Quaternion &from, const Quaternion &to,
static Quaternion RotateTowards(const Quaternion& from,
const Quaternion& to,
float maxDegreesDelta);
/// <summary>
@ -191,7 +191,8 @@ public:
/// <returns>The resulting rotation</returns>
/// A factor 0 returns rotation1, factor1 returns rotation2.
static Quaternion Slerp(const Quaternion& rotation1,
const Quaternion &rotation2, float factor);
const Quaternion& rotation2,
float factor);
/// <summary>
/// Unclamped sherical lerp between two rotations
/// </summary>
@ -202,7 +203,8 @@ public:
/// A factor 0 returns rotation1, factor1 returns rotation2.
/// Values outside the 0..1 range will result in extrapolated rotations
static Quaternion SlerpUnclamped(const Quaternion& rotation1,
const Quaternion &rotation2, float factor);
const Quaternion& rotation2,
float factor);
/// <summary>
/// Create a rotation from euler angles
@ -260,8 +262,10 @@ public:
/// <param name="swing">A pointer to the quaternion for the swing
/// result</param> <param name="twist">A pointer to the quaternion for the
/// twist result</param>
static void GetSwingTwist(Vector3 axis, Quaternion rotation,
Quaternion *swing, Quaternion *twist);
static void GetSwingTwist(Vector3 axis,
Quaternion rotation,
Quaternion* swing,
Quaternion* twist);
/// <summary>
/// Calculate the dot product of two quaternions
@ -284,7 +288,6 @@ private:
};
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
using namespace LinearAlgebra;
#endif

35
LinearAlgebra/Spherical.cpp
View File

@ -5,13 +5,15 @@
#include <math.h>
template <typename T> SphericalOf<T>::SphericalOf() {
template <typename T>
SphericalOf<T>::SphericalOf() {
this->distance = 0.0f;
this->direction = DirectionOf<T>();
}
template <typename T>
SphericalOf<T>::SphericalOf(float distance, AngleOf<T> horizontal,
SphericalOf<T>::SphericalOf(float distance,
AngleOf<T> horizontal,
AngleOf<T> vertical) {
if (distance < 0) {
this->distance = -distance;
@ -34,7 +36,8 @@ SphericalOf<T>::SphericalOf(float distance, DirectionOf<T> direction) {
}
template <typename T>
SphericalOf<T> SphericalOf<T>::Degrees(float distance, float horizontal,
SphericalOf<T> SphericalOf<T>::Degrees(float distance,
float horizontal,
float vertical) {
AngleOf<T> horizontalAngle = AngleOf<T>::Degrees(horizontal);
AngleOf<T> verticalAngle = AngleOf<T>::Degrees(vertical);
@ -43,7 +46,8 @@ SphericalOf<T> SphericalOf<T>::Degrees(float distance, float horizontal,
}
template <typename T>
SphericalOf<T> SphericalOf<T>::Radians(float distance, float horizontal,
SphericalOf<T> SphericalOf<T>::Radians(float distance,
float horizontal,
float vertical) {
return SphericalOf<T>(distance, AngleOf<T>::Radians(horizontal),
AngleOf<T>::Radians(vertical));
@ -57,7 +61,8 @@ SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
return r;
}
template <typename T> SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
template <typename T>
SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
float distance = v.magnitude();
if (distance == 0.0f) {
return SphericalOf(distance, AngleOf<T>(), AngleOf<T>());
@ -81,7 +86,8 @@ template <typename T> SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
* @tparam T The type of the distance and direction values.
* @return Vector3 The 3D vector representation of the spherical coordinates.
*/
template <typename T> Vector3 SphericalOf<T>::ToVector3() const {
template <typename T>
Vector3 SphericalOf<T>::ToVector3() const {
float verticalRad = (pi / 2) - this->direction.vertical.InRadians();
float horizontalRad = this->direction.horizontal.InRadians();
@ -126,7 +132,8 @@ SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
return v;
}
template <typename T> SphericalOf<T> SphericalOf<T>::operator-() const {
template <typename T>
SphericalOf<T> SphericalOf<T>::operator-() const {
SphericalOf<T> v = SphericalOf<T>(
this->distance, this->direction.horizontal + AngleOf<T>::Degrees(180),
this->direction.vertical + AngleOf<T>::Degrees(180));
@ -209,12 +216,14 @@ SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T> &v) {
return *this;
}
template <typename T> SphericalOf<T> SphericalOf<T>::operator*=(float f) {
template <typename T>
SphericalOf<T> SphericalOf<T>::operator*=(float f) {
this->distance *= f;
return *this;
}
template <typename T> SphericalOf<T> SphericalOf<T>::operator/=(float f) {
template <typename T>
SphericalOf<T> SphericalOf<T>::operator/=(float f) {
this->distance /= f;
return *this;
}
@ -256,8 +265,8 @@ AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf &v1,
}
template <typename T>
AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
const SphericalOf<T> &v1, const SphericalOf<T> &v2,
AngleOf<T> SphericalOf<T>::SignedAngleBetween(const SphericalOf<T>& v1,
const SphericalOf<T>& v2,
const SphericalOf<T>& axis) {
Vector3 v1_vector = v1.ToVector3();
Vector3 v2_vector = v2.ToVector3();
@ -290,5 +299,5 @@ SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T> &v,
return r;
}
template class Passer::LinearAlgebra::SphericalOf<float>;
template class Passer::LinearAlgebra::SphericalOf<signed short>;
template class SphericalOf<float>;
template class SphericalOf<signed short>;

19
LinearAlgebra/Spherical.h
View File

@ -7,15 +7,16 @@
#include "Direction.h"
namespace Passer {
namespace LinearAlgebra {
struct Vector3;
template <typename T> class PolarOf;
template <typename T>
class PolarOf;
/// @brief A spherical vector using angles in various representations
/// @tparam T The implementation type used for the representations of the agles
template <typename T> class SphericalOf {
template <typename T>
class SphericalOf {
public:
/// @brief The distance in meters
/// @remark The distance should never be negative
@ -38,7 +39,8 @@ public:
/// @param horizontal The horizontal angle in degrees
/// @param vertical The vertical angle in degrees
/// @return The spherical vector
static SphericalOf<T> Degrees(float distance, float horizontal,
static SphericalOf<T> Degrees(float distance,
float horizontal,
float vertical);
/// @brief Short-hand Deg alias for the Degrees function
constexpr static auto Deg = Degrees;
@ -48,7 +50,8 @@ public:
/// @param horizontal The horizontal angle in radians
/// @param vertical The vertical angle in radians
/// @return The spherical vectpr
static SphericalOf<T> Radians(float distance, float horizontal,
static SphericalOf<T> Radians(float distance,
float horizontal,
float vertical);
// Short-hand Rad alias for the Radians function
constexpr static auto Rad = Radians;
@ -154,7 +157,8 @@ public:
/// @param horizontalAngle The horizontal rotation angle in local space
/// @param verticalAngle The vertical rotation angle in local space
/// @return The rotated vector
static SphericalOf<T> Rotate(const SphericalOf &v, AngleOf<T> horizontalAngle,
static SphericalOf<T> Rotate(const SphericalOf& v,
AngleOf<T> horizontalAngle,
AngleOf<T> verticalAngle);
/// @brief Rotate a spherical vector horizontally
/// @param v The vector to rotate
@ -187,8 +191,7 @@ using Spherical = SphericalSingle;
#endif
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
using namespace LinearAlgebra;
#include "Polar.h"
#include "Vector3.h"

4
LinearAlgebra/SwingTwist.cpp
View File

@ -164,5 +164,5 @@ void SwingTwistOf<T>::Normalize() {
}
}
template class Passer::LinearAlgebra::SwingTwistOf<float>;
template class Passer::LinearAlgebra::SwingTwistOf<signed short>;
template class SwingTwistOf<float>;
template class SwingTwistOf<signed short>;

16
LinearAlgebra/SwingTwist.h
View File

@ -10,13 +10,13 @@
#include "Quaternion.h"
#include "Spherical.h"
namespace Passer {
namespace LinearAlgebra {
/// @brief An orientation using swing and twist angles in various
/// representations
/// @tparam T The implmentation type used for the representation of the angles
template <typename T> class SwingTwistOf {
template <typename T>
class SwingTwistOf {
public:
DirectionOf<T> swing;
AngleOf<T> twist;
@ -25,7 +25,8 @@ public:
SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
SwingTwistOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
static SwingTwistOf<T> Degrees(float horizontal, float vertical = 0,
static SwingTwistOf<T> Degrees(float horizontal,
float vertical = 0,
float twist = 0);
Quaternion ToQuaternion() const;
@ -72,8 +73,13 @@ public:
using SwingTwistSingle = SwingTwistOf<float>;
using SwingTwist16 = SwingTwistOf<signed short>;
#if defined(ARDUINO)
using SwingTwist = SwingTwist16;
#else
using SwingTwist = SwingTwistSingle;
#endif
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
using namespace LinearAlgebra;
#endif

25
LinearAlgebra/Vector2.cpp
View File

@ -30,7 +30,7 @@ Vector2::Vector2(Vector3 v) {
y = v.Forward(); // z;
}
Vector2::Vector2(PolarSingle p) {
float horizontalRad = p.angle.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
float horizontalRad = p.angle.InDegrees() * Deg2Rad;
float cosHorizontal = cosf(horizontalRad);
float sinHorizontal = sinf(horizontalRad);
@ -56,9 +56,15 @@ bool Vector2::operator==(const Vector2 &v) {
float Vector2::Magnitude(const Vector2& v) {
return sqrtf(v.x * v.x + v.y * v.y);
}
float Vector2::magnitude() const { return (float)sqrtf(x * x + y * y); }
float Vector2::SqrMagnitude(const Vector2 &v) { return v.x * v.x + v.y * v.y; }
float Vector2::sqrMagnitude() const { return (x * x + y * y); }
float Vector2::magnitude() const {
return (float)sqrtf(x * x + y * y);
}
float Vector2::SqrMagnitude(const Vector2& v) {
return v.x * v.x + v.y * v.y;
}
float Vector2::sqrMagnitude() const {
return (x * x + y * y);
}
Vector2 Vector2::Normalize(const Vector2& v) {
float num = Vector2::Magnitude(v);
@ -77,7 +83,9 @@ Vector2 Vector2::normalized() const {
return result;
}
Vector2 Vector2::operator-() { return Vector2(-this->x, -this->y); }
Vector2 Vector2::operator-() {
return Vector2(-this->x, -this->y);
}
Vector2 Vector2::operator-(const Vector2& v) const {
return Vector2(this->x - v.x, this->y - v.y);
@ -148,12 +156,11 @@ float Vector2::SignedAngle(const Vector2 &v1, const Vector2 &v2) {
float angleFrom = atan2f(v1.y, v1.x);
float angleTo = atan2f(v2.y, v2.x);
return -(angleTo - angleFrom) * Passer::LinearAlgebra::Rad2Deg;
return -(angleTo - angleFrom) * Rad2Deg;
}
Vector2 Vector2::Rotate(const Vector2 &v,
Passer::LinearAlgebra::AngleSingle a) {
float angleRad = a.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
Vector2 Vector2::Rotate(const Vector2& v, AngleSingle a) {
float angleRad = a.InDegrees() * Deg2Rad;
#if defined(AVR)
float sinValue = sin(angleRad);
float cosValue = cos(angleRad); // * Angle::Deg2Rad);

9
LinearAlgebra/Vector2.h
View File

@ -26,11 +26,11 @@ typedef struct Vec2 {
} Vec2;
}
namespace Passer {
namespace LinearAlgebra {
struct Vector3;
template <typename T> class PolarOf;
template <typename T>
class PolarOf;
/// @brief A 2-dimensional vector
/// @remark This uses the right=handed carthesian coordinate system.
@ -188,7 +188,7 @@ public:
/// @param v The vector to rotate
/// @param a The angle in degrees to rotate
/// @return The rotated vector
static Vector2 Rotate(const Vector2 &v, Passer::LinearAlgebra::AngleSingle a);
static Vector2 Rotate(const Vector2& v, AngleSingle a);
/// @brief Lerp (linear interpolation) between two vectors
/// @param v1 The starting vector
@ -202,8 +202,7 @@ public:
};
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
using namespace LinearAlgebra;
#include "Polar.h"

17
LinearAlgebra/Vector3.cpp
View File

@ -31,10 +31,8 @@ Vector3::Vector3(Vector2 v) {
}
Vector3::Vector3(SphericalOf<float> s) {
float verticalRad = (90.0f - s.direction.vertical.InDegrees()) *
Passer::LinearAlgebra::Deg2Rad;
float horizontalRad =
s.direction.horizontal.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
float verticalRad = (90.0f - s.direction.vertical.InDegrees()) * Deg2Rad;
float horizontalRad = s.direction.horizontal.InDegrees() * Deg2Rad;
float cosVertical = cosf(verticalRad);
float sinVertical = sinf(verticalRad);
float cosHorizontal = cosf(horizontalRad);
@ -70,12 +68,16 @@ const Vector3 Vector3::back = Vector3(0, 0, -1);
float Vector3::Magnitude(const Vector3& v) {
return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
}
float Vector3::magnitude() const { return (float)sqrtf(x * x + y * y + z * z); }
float Vector3::magnitude() const {
return (float)sqrtf(x * x + y * y + z * z);
}
float Vector3::SqrMagnitude(const Vector3& v) {
return v.x * v.x + v.y * v.y + v.z * v.z;
}
float Vector3::sqrMagnitude() const { return (x * x + y * y + z * z); }
float Vector3::sqrMagnitude() const {
return (x * x + y * y + z * z);
}
Vector3 Vector3::Normalize(const Vector3& v) {
float num = Vector3::Magnitude(v);
@ -200,7 +202,8 @@ AngleOf<float> Vector3::Angle(const Vector3 &v1, const Vector3 &v2) {
return AngleOf<float>::Radians(r);
}
AngleOf<float> Vector3::SignedAngle(const Vector3 &v1, const Vector3 &v2,
AngleOf<float> Vector3::SignedAngle(const Vector3& v1,
const Vector3& v2,
const Vector3& axis) {
// angle in [0,180]
AngleOf<float> angle = Vector3::Angle(v1, v2);

10
LinearAlgebra/Vector3.h
View File

@ -31,10 +31,10 @@ protected:
} Vec3;
}
namespace Passer {
namespace LinearAlgebra {
template <typename T> class SphericalOf;
template <typename T>
class SphericalOf;
/// @brief A 3-dimensional vector
/// @remark This uses a right-handed carthesian coordinate system.
@ -210,7 +210,8 @@ public:
/// @param v2 The ending vector
/// @param axis The axis to rotate around
/// @return The signed angle between the two vectors
static AngleOf<float> SignedAngle(const Vector3 &v1, const Vector3 &v2,
static AngleOf<float> SignedAngle(const Vector3& v1,
const Vector3& v2,
const Vector3& axis);
/// @brief Lerp (linear interpolation) between two vectors
@ -225,8 +226,7 @@ public:
};
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
using namespace LinearAlgebra;
#include "Spherical.h"

4
LinearAlgebra/test/Angle16_test.cc
View File

@ -1,11 +1,13 @@
#if GTEST
#include "gtest/gtest.h"
#include <limits>
#include <math.h>
#include <limits>
#include "Angle.h"
using namespace LinearAlgebra;
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle16, Construct) {

4
LinearAlgebra/test/Angle8_test.cc
View File

@ -1,11 +1,13 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include <limits>
#include "Angle.h"
using namespace LinearAlgebra;
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle8, Construct) {

4
LinearAlgebra/test/AngleSingle_test.cc
View File

@ -1,11 +1,13 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include <limits>
#include "Angle.h"
using namespace LinearAlgebra;
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(AngleSingle, Construct) {

20
Messages/LowLevelMessages.cpp
View File

@ -1,7 +1,7 @@
#include "LowLevelMessages.h"
#include "LinearAlgebra/float16.h"
// #include <iostream>
#include "LinearAlgebra/float16.h"
namespace RoboidControl {
@ -42,30 +42,30 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
return (float)f.toFloat();
}
void LowLevelMessages::SendSpherical16(char* buffer,
void LowLevelMessages::SendSpherical(char* buffer,
unsigned char* ix,
Spherical16 s) {
Spherical s) {
SendFloat16(buffer, ix, s.distance);
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
}
Spherical16 LowLevelMessages::ReceiveSpherical16(const char* buffer,
Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
unsigned char* startIndex) {
float distance = ReceiveFloat16(buffer, startIndex);
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 256);
Angle horizontal = Angle::Radians(horizontal8.InRadians());
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 256);
Angle vertical = Angle::Radians(vertical8.InRadians());
Spherical16 s = Spherical16(distance, horizontal, vertical);
Spherical s = Spherical(distance, horizontal, vertical);
return s;
}
void LowLevelMessages::SendQuat32(char* buffer,
unsigned char* ix,
SwingTwist16 rotation) {
SwingTwist rotation) {
Quaternion q = rotation.ToQuaternion();
unsigned char qx = (char)(q.x * 127 + 128);
unsigned char qy = (char)(q.y * 127 + 128);
@ -85,14 +85,14 @@ void LowLevelMessages::SendQuat32(char* buffer,
buffer[(*ix)++] = qw;
}
SwingTwist16 LowLevelMessages::ReceiveQuat32(const char* buffer,
SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
unsigned char* ix) {
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qw = buffer[(*ix)++] / 255.0F;
Quaternion q = Quaternion(qx, qy, qz, qw);
SwingTwist16 s = SwingTwist16::FromQuaternion(q);
SwingTwist s = SwingTwist::FromQuaternion(q);
return s;
}

9
Messages/LowLevelMessages.h
View File

@ -11,11 +11,12 @@ class LowLevelMessages {
static void SendFloat16(char* buffer, unsigned char* ix, float value);
static float ReceiveFloat16(const char* buffer, unsigned char* startIndex);
static void SendSpherical16(char* buffer, unsigned char* ix, Spherical16 s);
static Spherical16 ReceiveSpherical16(const char* buffer, unsigned char* startIndex);
static void SendSpherical(char* buffer, unsigned char* ix, Spherical s);
static Spherical ReceiveSpherical(const char* buffer,
unsigned char* startIndex);
static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist16 q);
static SwingTwist16 ReceiveQuat32(const char* buffer, unsigned char* ix);
static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
};
} // namespace RoboidControl

24
Messages/PoseMsg.cpp
View File

@ -3,22 +3,6 @@
namespace RoboidControl {
// PoseMsg::PoseMsg(unsigned char networkId,
// unsigned char thingId,
// unsigned char poseType,
// Spherical16 position,
// SwingTwist16 orientation,
// Spherical16 linearVelocity,
// Spherical16 angularVelocity) {
// this->networkId = networkId;
// this->thingId = thingId;
// this->poseType = poseType;
// this->position = position;
// this->orientation = orientation;
// this->linearVelocity = linearVelocity;
// this->angularVelocity = angularVelocity;
// }
PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
this->networkId = networkId;
this->thingId = thing->id;
@ -51,7 +35,7 @@ PoseMsg::PoseMsg(const char* buffer) {
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
this->poseType = buffer[ix++];
this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
// linearVelocity
// angularVelocity
@ -69,13 +53,13 @@ unsigned char PoseMsg::Serialize(char* buffer) {
buffer[ix++] = this->thingId;
buffer[ix++] = this->poseType;
if ((this->poseType & Pose_Position) != 0)
LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
LowLevelMessages::SendSpherical(buffer, &ix, this->position);
if ((this->poseType & Pose_Orientation) != 0)
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
if ((this->poseType & Pose_LinearVelocity) != 0)
LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
if ((this->poseType & Pose_AngularVelocity) != 0)
LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
LowLevelMessages::SendSpherical(buffer, &ix, this->angularVelocity);
return ix;
}

31
Messages/PoseMsg.h
View File

@ -3,8 +3,8 @@
namespace RoboidControl {
/// @brief Message to communicate the pose of the thing
/// The pose is in local space relative to the parent. If there is not parent (the thing is a root thing), the pose will
/// be in world space.
/// The pose is in local space relative to the parent. If there is not parent
/// (the thing is a root thing), the pose will be in world space.
class PoseMsg : public IMessage {
public:
/// @brief The message ID
@ -29,29 +29,14 @@ class PoseMsg : public IMessage {
static const unsigned char Pose_AngularVelocity = 0x08;
/// @brief The position of the thing in local space in meters
Spherical16 position;
Spherical position;
/// @brief The orientation of the thing in local space
SwingTwist16 orientation;
/// @brief The linear velocity of the thing in local space in meters per second
Spherical16 linearVelocity;
SwingTwist orientation;
/// @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
Spherical16 angularVelocity;
/// @brief Create a new message for sending
/// @param networkId The network ID of the thing
/// @param thingId The ID of the thing
/// @param poseType Bit pattern stating which pose components are available
/// @param position The position of the thing in local space in meters
/// @param orientation The orientation of the thing in local space
/// @param linearVelocity The linear velocity of the thing in local space in meters per second
/// @param angularVelocity The angular velocity of the thing in local space
// PoseMsg(unsigned char networkId,
// unsigned char thingId,
// unsigned char poseType,
// Spherical16 position,
// SwingTwist16 orientation,
// Spherical16 linearVelocity = Spherical16(),
// Spherical16 angularVelocity = Spherical16());
Spherical angularVelocity;
/// @brief Create a new message for sending
/// @param networkId he network ID of the thing

2
Participant.cpp
View File

@ -8,7 +8,7 @@ Participant::Participant() {}
Participant::Participant(const char* ipAddress, int port) {
// make a copy of the ip address string
int addressLength = strlen(ipAddress);
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
char* addressString = new char[stringLength];
#if defined(_WIN32) || defined(_WIN64)

46
Thing.cpp
View File

@ -31,11 +31,11 @@ Thing::Thing(Participant* owner, int thingType) {
this->type = thingType;
this->networkId = 0;
this->position = Spherical16::zero;
this->orientation = SwingTwist16::identity;
this->position = Spherical::zero;
this->orientation = SwingTwist::identity;
this->linearVelocity = Spherical16::zero;
this->angularVelocity = Spherical16::zero;
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
// std::cout << "add thing to participant\n";
owner->Add(this);
@ -51,8 +51,8 @@ Thing::Thing(Participant* owner,
this->id = thingId;
this->type = (unsigned char)thingType;
this->linearVelocity = Spherical16::zero;
this->angularVelocity = Spherical16::zero;
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
// std::cout << "Created thing " << (int)this->networkId << "/" <<
// (int)this->id
// << "\n";
@ -183,16 +183,20 @@ unsigned long Thing::GetTimeMs() {
#endif
}
void Thing::Update() {
Update(GetTimeMs());
void Thing::Update(bool recursive) {
Update(GetTimeMs(), recursive);
}
void Thing::Update(unsigned long currentTimeMs) {
void Thing::Update(unsigned long currentTimeMs, bool recursive) {
(void)currentTimeMs;
// PoseMsg* poseMsg = new PoseMsg(this->networkId, this);
// participant->Send(remoteParticipant, poseMsg);
// delete poseMsg;
if (recursive) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing* child = this->children[childIx];
if (child == nullptr)
continue;
child->Update(currentTimeMs, recursive);
}
}
}
void Thing::UpdateThings(unsigned long currentTimeMs) {
@ -207,38 +211,38 @@ void Thing::ProcessBinary(char* bytes) {
(void)bytes;
};
void Thing::SetPosition(Spherical16 position) {
void Thing::SetPosition(Spherical position) {
this->position = position;
this->positionUpdated = true;
}
Spherical16 Thing::GetPosition() {
Spherical Thing::GetPosition() {
return this->position;
}
void Thing::SetOrientation(SwingTwist16 orientation) {
void Thing::SetOrientation(SwingTwist orientation) {
this->orientation = orientation;
this->orientationUpdated = true;
}
SwingTwist16 Thing::GetOrientation() {
SwingTwist Thing::GetOrientation() {
return this->orientation;
}
void Thing::SetLinearVelocity(Spherical16 linearVelocity) {
void Thing::SetLinearVelocity(Spherical linearVelocity) {
this->linearVelocity = linearVelocity;
this->linearVelocityUpdated = true;
}
Spherical16 Thing::GetLinearVelocity() {
Spherical Thing::GetLinearVelocity() {
return this->linearVelocity;
}
void Thing::SetAngularVelocity(Spherical16 angularVelocity) {
void Thing::SetAngularVelocity(Spherical angularVelocity) {
this->angularVelocity = angularVelocity;
this->angularVelocityUpdated = true;
}
Spherical16 Thing::GetAngularVelocity() {
Spherical Thing::GetAngularVelocity() {
return this->angularVelocity;
}

29
Thing.h
View File

@ -119,16 +119,16 @@ class Thing {
/// @brief Set the position of the thing
/// @param position The new position in local space, in meters
void SetPosition(Spherical16 position);
void SetPosition(Spherical position);
/// @brief Get the position of the thing
/// @return The position in local space, in meters
Spherical16 GetPosition();
Spherical GetPosition();
/// @brief Set the orientation of the thing
/// @param orientation The new orientation in local space
void SetOrientation(SwingTwist16 orientation);
void SetOrientation(SwingTwist orientation);
/// @brief Get the orientation of the thing
/// @return The orienation in local space
SwingTwist16 GetOrientation();
SwingTwist GetOrientation();
/// @brief The scale of the thing (deprecated I think)
// float scale = 1; // assuming uniform scale
@ -140,16 +140,16 @@ class Thing {
/// @brief Set the linear velocity of the thing
/// @param linearVelocity The new linear velocity in local space, in meters
/// per second
void SetLinearVelocity(Spherical16 linearVelocity);
void SetLinearVelocity(Spherical linearVelocity);
/// @brief Get the linear velocity of the thing
/// @return The linear velocity in local space, in meters per second
virtual Spherical16 GetLinearVelocity();
virtual Spherical GetLinearVelocity();
/// @brief Set the angular velocity of the thing
/// @param angularVelocity the new angular velocity in local space
virtual void SetAngularVelocity(Spherical16 angularVelocity);
virtual void SetAngularVelocity(Spherical angularVelocity);
/// @brief Get the angular velocity of the thing
/// @return The angular velocity in local space
virtual Spherical16 GetAngularVelocity();
virtual Spherical GetAngularVelocity();
bool linearVelocityUpdated = false;
bool angularVelocityUpdated = false;
@ -157,16 +157,16 @@ class Thing {
/// @brief The position in local space
/// @remark When this Thing has a parent, the position is relative to the
/// parent's position and orientation
Spherical16 position;
Spherical position;
/// @brief The orientation in local space
/// @remark When this Thing has a parent, the orientation is relative to the
/// parent's orientation
SwingTwist16 orientation;
SwingTwist orientation;
/// @brief The linear velocity in local space
Spherical16 linearVelocity;
Spherical linearVelocity;
/// @brief The angular velocity in local spze
Spherical16 angularVelocity;
Spherical angularVelocity;
public:
/// @brief Terminated things are no longer updated
@ -181,12 +181,11 @@ class Thing {
static unsigned long GetTimeMs();
void Update();
void Update(bool recursive = false);
/// @brief Updates the state of the thing
/// @param currentTimeMs The current clock time in milliseconds
virtual void Update(
unsigned long currentTimeMs); // { (void)currentTimeMs; };
virtual void Update(unsigned long currentTimeMs, bool recursive = false);
static void UpdateThings(unsigned long currentTimeMs);

30
Things/DifferentialDrive.cpp
View File

@ -1,20 +1,18 @@
#include "DifferentialDrive.h"
namespace RoboidControl {
DifferentialDrive::DifferentialDrive() : Thing() {}
RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant)
: Thing(participant) {
this->leftWheel = new Thing(participant);
this->rightWheel = new Thing(participant);
}
: Thing(participant) {}
void DifferentialDrive::SetDimensions(float wheelDiameter,
void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
float wheelSeparation) {
this->wheelRadius =
wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
this->wheelSeparation =
wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
this->rpsToMs = wheelDiameter * LinearAlgebra::pi;
float distance = this->wheelSeparation / 2;
if (leftWheel != nullptr)
@ -35,7 +33,17 @@ void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
}
void DifferentialDrive::Update(unsigned long currentMs) {
void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
if (this->leftWheel != nullptr)
this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
if (this->rightWheel != nullptr)
this->rightWheel->SetAngularVelocity(
Spherical(speedRight, Direction::right));
}
void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
if (this->linearVelocityUpdated == false)
return;
// this assumes forward velocity only....
float linearVelocity = this->GetLinearVelocity().distance;
@ -49,16 +57,12 @@ void DifferentialDrive::Update(unsigned long currentMs) {
float speedLeft =
(linearVelocity + angularSpeed * this->wheelSeparation / 2) /
this->wheelRadius * Rad2Deg;
if (this->leftWheel != nullptr)
this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
float speedRight =
(linearVelocity - angularSpeed * this->wheelSeparation / 2) /
this->wheelRadius * Rad2Deg;
if (this->rightWheel != nullptr)
this->rightWheel->SetAngularVelocity(
Spherical(speedRight, Direction::right));
this->SetWheelVelocity(speedLeft, speedRight);
Thing::Update(currentMs, recursive);
// std::cout << "lin. speed " << linearVelocity << " ang. speed " <<
// angularVelocity.distance << " left wheel "
// << speedLeft << " right wheel " << speedRight << "\n";

31
Things/DifferentialDrive.h
View File

@ -5,14 +5,39 @@
namespace RoboidControl {
/// @brief A thing which can move itself using a differential drive system
///
/// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
class DifferentialDrive : public Thing {
public:
/// @brief Create a differential drive without networking support
DifferentialDrive();
/// @brief Create a differential drive with networking support
/// @param participant The local participant
DifferentialDrive(Participant* participant);
void SetDimensions(float wheelDiameter, float wheelSeparation);
/// @brief Configures the dimensions of the drive
/// @param wheelDiameter The diameter of the wheels in meters
/// @param wheelSeparation The distance between the wheels in meters
///
/// These values are used to compute the desired wheel speed from the set
/// linear and angular velocity.
/// @sa SetLinearVelocity SetAngularVelocity
void SetDriveDimensions(float wheelDiameter, float wheelSeparation);
/// @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);
virtual void Update(unsigned long currentMs) override;
/// @brief Directly specify the speeds of the motors
/// @param speedLeft The speed of the left wheel in degrees per second.
/// Positive moves the robot in the forward direction.
/// @param speedRight The speed of the right wheel in degrees per second.
/// Positive moves the robot in the forward direction.
void SetWheelVelocity(float speedLeft, float speedRight);
/// @copydoc RoboidControl::Thing::Update(unsigned long)
virtual void Update(unsigned long currentMs, bool recursive = true) override;
protected:
/// @brief The radius of a wheel in meters
@ -23,7 +48,9 @@ class DifferentialDrive : public Thing {
/// @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;
};

9
Things/TouchSensor.cpp
View File

@ -1,10 +1,13 @@
#include "TouchSensor.h"
namespace RoboidControl {
TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {}
TouchSensor::TouchSensor(Participant* participant) : Thing(participant) {
this->touchedSomething = false;
this->type = (unsigned char)Thing::Type::TouchSensor;
TouchSensor::TouchSensor(Participant* participant)
: Thing(participant, Thing::Type::TouchSensor) {}
TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) {
this->SetParent(parent);
}
void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}

13
Things/TouchSensor.h
View File

@ -7,15 +7,20 @@ namespace RoboidControl {
/// @brief A sensor which can detect touches
class TouchSensor : public Thing {
public:
/// @brief Value which is true when the sensor is touching something, false otherwise
bool touchedSomething = false;
/// @brief Create a touch sensor with isolated participant
TouchSensor();
/// @brief Create a touch sensor
TouchSensor(Participant* participant);
/// @brief Create a temperature sensor with the given ID
/// @param networkId The network ID of the sensor
/// @param thingId The ID of the thing
// TouchSensor(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId);
TouchSensor(Thing* parent);
// TouchSensor(RemoteParticipant* participant, unsigned char networkId,
// unsigned char thingId);
/// @brief Value which is true when the sensor is touching something, false
/// otherwise
bool touchedSomething = false;
/// @brief Function to create a binary message with the temperature
/// @param buffer The byte array for thw binary data