Initial commit

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "VectorAlgebra"]
+	path = VectorAlgebra
+	url = http://gitlab.passervr.com/passer/cpp/vectoralgebra.git

Activation.cpp

@@ -0,0 +1,45 @@
+#include <Activation.h>
+
+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;
+}

Activation.h

@@ -0,0 +1,21 @@
+#ifndef RC_ACTIVATION_H
+#define RC_ACTIVATION_H
+
+#include <math.h>
+
+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

ControlledMotor.cpp

@@ -0,0 +1,20 @@
+#include <ControlledMotor.h>
+
+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
+}

ControlledMotor.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <Encoder.h>
+#include <Motor.h>
+
+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;
+};

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;
+  }
+};

Encoder.cpp

@@ -0,0 +1,44 @@
+#include <Encoder.h>
+
+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;
+}

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
+};

Motor.cpp

@@ -0,0 +1,25 @@
+#include <Motor.h>
+
+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);
+// }

Motor.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <Thing.h>
+
+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;
+};

Placement.cpp

@@ -0,0 +1,39 @@
+#include <Placement.h>
+
+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;
+}

Placement.h

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <ControlledMotor.h>
+#include <Motor.h>
+#include <Thing.h>
+#include <Vector2.h>
+#include <Vector3.h>
+#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;
+};

Propulsion.cpp

@@ -0,0 +1,74 @@
+#include <ControlledMotor.h>
+#include <Propulsion.h>
+
+#include <FloatSingle.h>
+
+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);
+}

Propulsion.h

@@ -0,0 +1,40 @@
+#pragma once
+
+#include <ControlledMotor.h>
+#include <Placement.h>
+#include <Vector2.h>
+
+#include <list>
+
+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;
+};

Roboid.cpp

@@ -0,0 +1,13 @@
+#include <Roboid.h>
+
+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);
+}

Roboid.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <Activation.h>
+#include <Placement.h>
+#include <Propulsion.h>
+#include <Sensing.h>
+
+class Roboid {
+ public:
+  Roboid();
+  Roboid(Placement configuration[], unsigned int thingCount);
+
+  Sensing sensing;
+  Propulsion propulsion;
+
+  Placement* configuration;
+  unsigned int thingCount;
+};

Sensing.cpp

@@ -0,0 +1,115 @@
+#include <DistanceSensor.h>
+#include <Sensing.h>
+#include <Switch.h>
+
+#include <math.h>
+#include <algorithm>
+
+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;
+}

Sensing.h

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <Placement.h>
+
+#include <list>
+
+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;
+};

Sensor.cpp

@@ -0,0 +1,5 @@
+#include "Sensor.h"
+
+Sensor::Sensor() {
+  this->isSensor = true;
+}

Sensor.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#include <Thing.h>
+
+class Sensor : public Thing {
+ public:
+  Sensor();
+  bool isDistanceSensor = false;
+};

Servo.h

@@ -0,0 +1,8 @@
+#pragma once
+
+class Servo {
+ public:
+  Servo();
+
+  void SetTargetAngle(float angle) = 0;
+};

Thing.h

@@ -0,0 +1,6 @@
+#pragma once
+
+class Thing {
+ public:
+  bool isSensor;
+};

VectorAlgebra

@@ -0,0 +1 @@
+Subproject commit bebd097db3c3f3f311b58257719c80ee10237632