commit 189ea6c68903ded461f0b24601ed50fabd2ec37e Author: Pascal Serrarens Date: Mon Nov 6 14:24:18 2023 +0100 Initial commit diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..c716151 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "VectorAlgebra"] + path = VectorAlgebra + url = http://gitlab.passervr.com/passer/cpp/vectoralgebra.git diff --git a/Activation.cpp b/Activation.cpp new file mode 100644 index 0000000..37002fa --- /dev/null +++ b/Activation.cpp @@ -0,0 +1,45 @@ +#include + +float Activation::HeavisideStep(float inputValue, float bias) { + return (inputValue + bias > 0) ? 1 : 0; +} + +float Activation::Linear(float inputValue, float bias, float range) { + if (inputValue > bias + range) + return 0; + if (inputValue < bias) + return 1; + + float f = inputValue * (1 / range); // normalize to 1..0 + float influence = 1 - f; // invert + return influence; +} + +float Activation::Tanh(float inputValue) { + return (exp(inputValue) - exp(-inputValue)) / (exp(inputValue) + exp(-inputValue)); +} + +float Activation::Sigmoid(float inputValue) { + return 1 / (1 + expf(-inputValue)); +} + +float Activation::Quadratic(float minValue, float range, float inputValue) { + if (inputValue > minValue + range) + return 0; + if (inputValue < minValue) + return 1; + + float f = inputValue * (1 / range); // normalize to 1..0 + float influence = 1 - (f * f); // quadratic & invert + return influence; +} + +float Activation::ParticleLife(float minValue, float maxValue, float attraction, float inputValue) { + if (inputValue < minValue) + return inputValue / minValue - 1; + + if (inputValue < maxValue) + return attraction * (1 - fabs(2 * inputValue - minValue - maxValue) / (maxValue - minValue)); + + return 0; +} \ No newline at end of file diff --git a/Activation.h b/Activation.h new file mode 100644 index 0000000..6595008 --- /dev/null +++ b/Activation.h @@ -0,0 +1,21 @@ +#ifndef RC_ACTIVATION_H +#define RC_ACTIVATION_H + +#include + +class Activation { + public: + static float HeavisideStep(float inputValue, float bias = 0); // Range: {0,1} + + static float Tanh(float inputValue); // Range: (-1, 1) + + static float Sigmoid(float inputValue); // Range: (0, 1) + + static float Linear(float inputValue, float bias = 0, float range = 0); + + static float Quadratic(float minValue, float range, float inputValue); // minValue = bias + + static float ParticleLife(float minValue, float maxValue, float attraction, float inputValue); // minValue = bias +}; + +#endif \ No newline at end of file diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp new file mode 100644 index 0000000..2b5625b --- /dev/null +++ b/ControlledMotor.cpp @@ -0,0 +1,20 @@ +#include + +ControlledMotor::ControlledMotor() {} + +ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) { + this->motor = motor; + this->encoder = encoder; +} + +void ControlledMotor::SetTargetVelocity(float velocity) { + this->targetVelocity = velocity; +} + +void ControlledMotor::Update(float timeStep) { + float velocity = GetActualVelocity(); + float error = targetVelocity - velocity; + + float acceleration = error * timeStep * pidP; // Just P is used at this moment + motor->SetSpeed(targetVelocity + acceleration); // or something like that +} \ No newline at end of file diff --git a/ControlledMotor.h b/ControlledMotor.h new file mode 100644 index 0000000..5d15ce1 --- /dev/null +++ b/ControlledMotor.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +class ControlledMotor : public Thing { + public: + ControlledMotor(); + ControlledMotor(Motor* motor, Encoder* encoder); + + float velocity; + + float pidP = 1; + float pidD = 0; + float pidI = 0; + + void Update(float timeStep); + + void SetTargetVelocity(float rotationsPerSecond); + float GetActualVelocity() { + return rotationDirection * encoder->GetRotationsPerSecond(); + } // in rotations per second + + protected: + float targetVelocity; + Motor* motor; + Encoder* encoder; + enum Direction { Forward = 1, + Reverse = -1 }; + + Direction rotationDirection; +}; \ No newline at end of file diff --git a/DistanceSensor.h b/DistanceSensor.h new file mode 100644 index 0000000..15e744a --- /dev/null +++ b/DistanceSensor.h @@ -0,0 +1,24 @@ +#pragma once + +#include "Sensor.h" + +/// @brief A sensor which can measure the distance the the nearest object +class DistanceSensor : public Sensor { + public: + /// @brief Determine the distance to the nearest object + /// @return the measured distance in meters to the nearest object + virtual float GetDistance() = 0; + + /// @brief The distance at which ObjectNearby triggers + float triggerDistance = 1; + + bool IsOn() { + bool isOn = GetDistance() <= triggerDistance; + return isOn; + } + + bool isOff() { + bool isOff = GetDistance() > triggerDistance; + return isOff; + } +}; diff --git a/Encoder.cpp b/Encoder.cpp new file mode 100644 index 0000000..709140d --- /dev/null +++ b/Encoder.cpp @@ -0,0 +1,44 @@ +#include + +volatile unsigned char Encoder::transitionCount = 0; + +Encoder::Encoder() { + rps = 0; + transitionsPerRotation = 1; // to prevent devide by zero + distance = 0; +} + +Encoder::Encoder(unsigned char pin, unsigned char transitionsPerRotation) + : Encoder::Encoder() { + /// Hmm. Arduino dependent code + // pinMode(pin, INPUT_PULLUP); + // attachInterrupt(digitalPinToInterrupt(pin), InterruptHandler, CHANGE); + this->transitionsPerRotation = transitionsPerRotation; +} + +void Encoder::InterruptHandler() { + transitionCount++; +} + +float Encoder::GetRotationsPerSecond() { + return rps; +} + +void Encoder::ResetDistance() { + distance = 0; +} + +float Encoder::GetRotationDistance() { + return distance; +} + +void Encoder::Update(float timeStep) { + // Hmmm. Arduino-dependent code... + // noInterrupts(); + float distanceThisUpdate = transitionCount / transitionsPerRotation; + transitionCount = 0; + // interrupts(); + + // float rps = distanceThisUpdate * timeStep; + distance += distanceThisUpdate; +} \ No newline at end of file diff --git a/Encoder.h b/Encoder.h new file mode 100644 index 0000000..e734139 --- /dev/null +++ b/Encoder.h @@ -0,0 +1,23 @@ +#pragma once + +class Encoder { + public: + Encoder(); + Encoder(unsigned char pin, unsigned char transitionsPerRotation); + + float GetRotationsPerSecond(); + + void ResetDistance(); + float GetRotationDistance(); + + void Update(float timeStep); + + protected: + static void InterruptHandler(); + static volatile unsigned char transitionCount; + + unsigned char transitionsPerRotation; + float rps; + + float distance; // this is direction agnostic +}; diff --git a/Motor.cpp b/Motor.cpp new file mode 100644 index 0000000..597b711 --- /dev/null +++ b/Motor.cpp @@ -0,0 +1,25 @@ +#include + +Motor::Motor() { + this->isSensor = false; +} + +// Motor::Motor(uint8_t pinIn1, uint8_t pinIn2) { +// this->pinIn1 = pinIn1; +// this->pinIn2 = pinIn2; + +// pinMode(pinIn1, OUTPUT); // configure the in1 pin to output mode +// pinMode(pinIn2, OUTPUT); // configure the in2 pin to output mode +// } + +// void Motor::SetDirection(Direction direction) { +// digitalWrite(pinIn1, direction); +// digitalWrite(pinIn2, !direction); // This is the opposite of pinIn1 +// } + +// void Motor::SetSpeed(float speed) { // 0..1 +// currentSpeed = speed; +// uint8_t motorSignal = (uint8_t)(speed * 255); +// analogWrite(pinIn1, speed); +// analogWrite(pinIn2, 255 - speed); +// } diff --git a/Motor.h b/Motor.h new file mode 100644 index 0000000..8eb277c --- /dev/null +++ b/Motor.h @@ -0,0 +1,20 @@ +#pragma once + +#include + +class Motor : public Thing { + public: + Motor(); + /// @brief Turning direction of the motor + enum Direction { Forward = 1, + Reverse = -1 }; + + /// @brief Set the turning direction of the motor + // void SetDirection(Direction direction); + + virtual void SetSpeed(float speed) = 0; + float GetSpeed(); + + protected: + float currentSpeed = 0; +}; diff --git a/Placement.cpp b/Placement.cpp new file mode 100644 index 0000000..de7f092 --- /dev/null +++ b/Placement.cpp @@ -0,0 +1,39 @@ +#include + +Placement::Placement() { + this->position = Vector3::zero; + this->thing = nullptr; +} +// Placement::Placement(Vector3 position, Thing* thing) { +// this->position = position; +// this->thing = thing; +// } + +Placement::Placement(Vector2 direction, Sensor* thing) { + this->position = Vector3::zero; + this->direction = direction; + this->thing = thing; +} + +Placement::Placement(Vector3 position, Sensor* thing) { + this->position = position; + this->direction = Vector2::zero; + this->thing = thing; +} + +Placement::Placement(Vector3 position, Motor* thing) { + this->position = position; + this->direction = Vector2::zero; + this->thing = thing; +} + +Placement::Placement(Vector3 position, ControlledMotor* thing) { + this->position = position; + this->direction = Vector2::zero; + this->thing = thing; +} + +Placement::Placement(Thing* thing, Vector3 position) { + this->thing = thing; + this->position = position; +} \ No newline at end of file diff --git a/Placement.h b/Placement.h new file mode 100644 index 0000000..81b3abe --- /dev/null +++ b/Placement.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "Sensor.h" + +class Placement { + public: + Placement(); + Placement(Vector2 direction, Sensor* sensor); + + Placement(Vector3 position, Sensor* sensor); + Placement(Vector3 position, Motor* motor); + Placement(Vector3 position, ControlledMotor* motor); + Placement(Thing* thing, Vector3 position); + + Placement* parent = nullptr; + Placement** children = nullptr; + unsigned int childCount = 0; + + Vector3 position; + Vector2 direction; + Thing* thing; +}; diff --git a/Propulsion.cpp b/Propulsion.cpp new file mode 100644 index 0000000..79c4e9f --- /dev/null +++ b/Propulsion.cpp @@ -0,0 +1,74 @@ +#include +#include + +#include + +Propulsion::Propulsion() { + this->motors = nullptr; + this->motorCount = 0; +} + +void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) { + this->motors = motors; + this->motorCount = motorCount; +} + +void Propulsion::AddMotors(Placement* motors, unsigned int motorCount) { + this->placement = motors; + this->motorCount = motorCount; +} + +void Propulsion::Update() { + // Hmmm. Arduino dependent code + // unsigned long curMillis = millis(); + // float timeStep = (float)(curMillis - lastMillis) / 1000; + // lastMillis = curMillis; + + for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { + MotorPlacement placement = motors[motorIx]; + // placement.controlledMotor->Update(timeStep); + } +} + +void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) { + for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { + Motor* motor = motors[motorIx].motor; + if (motor == nullptr) + continue; + + float xPosition = motors[motorIx].position.x; + if (xPosition < 0) + motor->SetSpeed(leftSpeed); + else if (xPosition > 0) + motor->SetSpeed(rightSpeed); + }; +} + +void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) { + for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { + MotorPlacement placement = motors[motorIx]; + if (placement.position.x < 0) + placement.controlledMotor->SetTargetVelocity(leftVelocity); + else if (placement.position.x > 0) + placement.controlledMotor->SetTargetVelocity(rightVelocity); + }; +} + +void Propulsion::SetTwistSpeed(float forward, float yaw) { + // This is configuration dependent, a drone will do something completely different... + float leftSpeed = Float::Clamp(forward - yaw, -1, 1); + float rightSpeed = Float::Clamp(forward + yaw, -1, 1); + SetDiffDriveSpeed(leftSpeed, rightSpeed); +} + +void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) { + float leftSpeed = Float::Clamp(forward - yaw, -1, 1); + float rightSpeed = Float::Clamp(forward + yaw, -1, 1); + SetDiffDriveSpeed(leftSpeed, rightSpeed); +} + +void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) { + float leftVelocity = Float::Clamp(forwardVelocity - turningVelocity, -1, 1); + float rightVelocity = Float::Clamp(forwardVelocity + turningVelocity, -1, 1); + SetDiffDriveVelocities(leftVelocity, rightVelocity); +} diff --git a/Propulsion.h b/Propulsion.h new file mode 100644 index 0000000..23da4f8 --- /dev/null +++ b/Propulsion.h @@ -0,0 +1,40 @@ +#pragma once + +#include +#include +#include + +#include + +struct MotorPlacement { + Motor* motor; + ControlledMotor* controlledMotor; + Vector2 position; +}; + +class Propulsion { + public: + /// @brief Setup sensing + Propulsion(); + + void Update(); + + void AddMotors(MotorPlacement* motors, unsigned int motorCount); + void AddMotors(Placement* motors, unsigned int motorCount); + + void SetDiffDriveSpeed(float leftSpeed, float rightSpeed); + void SetDiffDriveVelocities(float leftVelocity, float rightVelocity); + + void SetTwistSpeed(float forward, float yaw); + void SetTwistSpeed(float forward, float yaw, float pitch); + void SetTwistVelocity(float forward, float yaw); + + // Think: drones + void SetLinearSpeed(Vector3 direction); + + protected: + unsigned long lastMillis; + MotorPlacement* motors = nullptr; + Placement* placement = nullptr; + unsigned int motorCount = 0; +}; diff --git a/Roboid.cpp b/Roboid.cpp new file mode 100644 index 0000000..c3e355c --- /dev/null +++ b/Roboid.cpp @@ -0,0 +1,13 @@ +#include + +Roboid::Roboid() { + this->configuration = nullptr; + this->thingCount = 0; +} + +Roboid::Roboid(Placement configuration[], unsigned int thingCount) { + this->configuration = configuration; + this->thingCount = thingCount; + + sensing.AddSensors(configuration, thingCount); +} \ No newline at end of file diff --git a/Roboid.h b/Roboid.h new file mode 100644 index 0000000..f2f156d --- /dev/null +++ b/Roboid.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include +#include +#include + +class Roboid { + public: + Roboid(); + Roboid(Placement configuration[], unsigned int thingCount); + + Sensing sensing; + Propulsion propulsion; + + Placement* configuration; + unsigned int thingCount; +}; \ No newline at end of file diff --git a/Sensing.cpp b/Sensing.cpp new file mode 100644 index 0000000..f2fedbf --- /dev/null +++ b/Sensing.cpp @@ -0,0 +1,115 @@ +#include +#include +#include + +#include +#include + +SensorPlacement::SensorPlacement(DistanceSensor* distanceSensor, Vector2 direction) { + this->distanceSensor = distanceSensor; + this->switchSensor = nullptr; + this->direction = direction; +} +SensorPlacement::SensorPlacement(Switch* switchSensor, Vector2 direction) { + this->distanceSensor = nullptr; + this->switchSensor = switchSensor; + this->direction = direction; +} + +Sensing::Sensing() {} + +// void Sensing::AddSensors(SensorPlacement* sensors, unsigned int sensorCount) { +// this->sensors = sensors; +// this->sensorCount = sensorCount; +// } + +void Sensing::AddSensors(Placement* things, unsigned int thingCount) { + sensorCount = 0; + for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { + Thing* thing = things[thingIx].thing; + if (thing->isSensor) + sensorCount++; + } + + sensorPlacements = new Placement[sensorCount]; + + unsigned int sensorIx = 0; + for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { + Thing* thing = things[thingIx].thing; + if (thing->isSensor) + sensorPlacements[sensorIx++] = things[thingIx]; + } +} + +float Sensing::DistanceForward(float angle) { + float minDistance = INFINITY; + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement placement = sensorPlacements[sensorIx]; + Sensor* sensor = (Sensor*)placement.thing; + if (sensor->isDistanceSensor == false) + continue; + + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + float sensorAngle = placement.direction.x; + if (sensorAngle > -angle && sensorAngle < angle) { + minDistance = fmin(minDistance, distanceSensor->GetDistance()); + } + } + return minDistance; +} + +float Sensing::DistanceLeft(float angle) { + float minDistance = INFINITY; + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement placement = sensorPlacements[sensorIx]; + Sensor* sensor = (Sensor*)placement.thing; + if (sensor->isDistanceSensor == false) + continue; + + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + float sensorAngle = placement.direction.x; + // Serial.printf(" distance sensor: %f %f 0\n", -angle, sensorAngle); + if (sensorAngle < 0 && sensorAngle > -angle) { + minDistance = fmin(minDistance, distanceSensor->GetDistance()); + } + } + return minDistance; +} + +float Sensing::DistanceRight(float angle) { + float minDistance = INFINITY; + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement placement = sensorPlacements[sensorIx]; + Sensor* sensor = (Sensor*)placement.thing; + if (sensor->isDistanceSensor == false) + continue; + + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + float sensorAngle = placement.direction.x; + // Serial.printf(" distance sensor: 0 %f %f\n", sensorAngle, angle); + if (sensorAngle > 0 && sensorAngle < angle) { + minDistance = fmin(minDistance, distanceSensor->GetDistance()); + } + } + return minDistance; +} + +bool Sensing::SwitchOn(float fromAngle, float toAngle) { + if (toAngle < fromAngle) + return false; + + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement placement = sensorPlacements[sensorIx]; + float angle = placement.direction.x; + if (angle > fromAngle && angle < toAngle) { + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + + // if (placement.switchSensor != nullptr && placement.switchSensor->IsOn()) + // return true; + // else + if (distanceSensor != nullptr && distanceSensor->IsOn()) + return true; + } + } + return false; +} \ No newline at end of file diff --git a/Sensing.h b/Sensing.h new file mode 100644 index 0000000..849321f --- /dev/null +++ b/Sensing.h @@ -0,0 +1,66 @@ +#pragma once + +#include + +#include + +class DistanceSensor; +class Switch; + +class NewSensorPlacement : public Placement { +}; + +struct SensorPlacement { + DistanceSensor* distanceSensor; + Switch* switchSensor; + Vector2 direction; + + SensorPlacement(DistanceSensor* distanceSensor, Vector2 direction); + SensorPlacement(Switch* switchSensor, Vector2 direction); +}; + +class Sensing { + public: + /// @brief Setup sensing + Sensing(); + + // void AddSensors(SensorPlacement* sensors, unsigned int sensorCount); + void AddSensors(Placement* sensors, unsigned int sensorCount); + + float DistanceForward(float angle = 90); + + /// @brief Distance to the closest object on the left + /// @return distance in meters, INFINITY when no object is detected. + /// @note An object is on the left when the `angle` is between -180 and 0 degrees. + /// @note An object dead straight (0 degrees) is not reported. + float DistanceLeft() { return DistanceLeft(180); } + /// @brief Distance to the closest object on the left + /// @param angle the maximum angle on the left used for detection. + /// @return distance in meters, INFINITY when no object is detected. + /// @note An object is on the left when the `angle` is between -`angle` and 0 degrees. + /// @note An object dead straight (0 degrees) is not reported. + /// @note When an object is beyond `angle` meters, it is not reported. + float DistanceLeft(float angle); + + /// @brief Distance to the closest object on the right + /// @return distance in meters, INFINITY when no object is detected + /// @note An object is on the right when the `angle` is between 0 and 180 degrees + /// @note An object dead straight (0 degrees) is not reported + float DistanceRight() { return DistanceRight(180); } + /// @brief Distance to the closest object on the right + /// @param angle the maximum angle on the left used for detection. + /// @return distance in meters, INFINITY when no object is detected + /// @note An object is on the left when the `angle` is between 0 and `angle` degrees. + /// @note An object dead straight (0 degrees) is not reported. + /// @note When an object is beyond `angle` meters, it is not reported. + float DistanceRight(float angle); + + float Distance(float leftAngle, float rightAngle); + + bool SwitchOn(float fromAngle, float toAngle); + + protected: + // SensorPlacement* sensors = nullptr; + Placement* sensorPlacements = nullptr; + unsigned int sensorCount = 0; +}; diff --git a/Sensor.cpp b/Sensor.cpp new file mode 100644 index 0000000..c6511d3 --- /dev/null +++ b/Sensor.cpp @@ -0,0 +1,5 @@ +#include "Sensor.h" + +Sensor::Sensor() { + this->isSensor = true; +} \ No newline at end of file diff --git a/Sensor.h b/Sensor.h new file mode 100644 index 0000000..d452e0e --- /dev/null +++ b/Sensor.h @@ -0,0 +1,9 @@ +#pragma once + +#include + +class Sensor : public Thing { + public: + Sensor(); + bool isDistanceSensor = false; +}; \ No newline at end of file diff --git a/Servo.h b/Servo.h new file mode 100644 index 0000000..faadd09 --- /dev/null +++ b/Servo.h @@ -0,0 +1,8 @@ +#pragma once + +class Servo { + public: + Servo(); + + void SetTargetAngle(float angle) = 0; +}; \ No newline at end of file diff --git a/Thing.h b/Thing.h new file mode 100644 index 0000000..8ea7661 --- /dev/null +++ b/Thing.h @@ -0,0 +1,6 @@ +#pragma once + +class Thing { + public: + bool isSensor; +}; \ No newline at end of file diff --git a/VectorAlgebra b/VectorAlgebra new file mode 160000 index 0000000..bebd097 --- /dev/null +++ b/VectorAlgebra @@ -0,0 +1 @@ +Subproject commit bebd097db3c3f3f311b58257719c80ee10237632