Updates for Quadcopter support

Activation.cpp

@@ -4,32 +4,33 @@ float Activation::HeavisideStep(float inputValue, float bias) {
   return (inputValue + bias > 0) ? 1.0F : 0.0F;
 }
 
-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));
+    return (exp(inputValue) - exp(-inputValue)) / (exp(inputValue) + exp(-inputValue));
 }
 
 float Activation::Sigmoid(float inputValue) {
-  return 1 / (1 + expf(-inputValue));
+    return 1 / (1 + expf(-inputValue));
 }
 
-float Activation::Quadratic(float minValue, float range, float inputValue) {
+float Activation::Linear(float inputValue, float minValue, float range) {
   if (inputValue > minValue + range)
     return 0;
   if (inputValue < minValue)
     return 1;
 
-  float f = inputValue * (1 / range);  // normalize to 1..0
+  float f = (inputValue - minValue) * (1 / range);  // normalize to 1..0
+  float influence = 1 - f;             // invert
+  return influence;
+}
+
+
+float Activation::Quadratic(float inputValue, float minValue, float range) {
+  if (inputValue > minValue + range)
+    return 0;
+  if (inputValue < minValue)
+    return 1;
+
+  float f = (inputValue - minValue) * (1 / range);  // normalize to 1..0
   float influence = 1 - (f * f);       // quadratic & invert
   return influence;
 }

Activation.h

@@ -13,7 +13,7 @@ class Activation {
 
   static float Linear(float inputValue, float bias = 0, float range = 0);
 
-  static float Quadratic(float minValue, float range, float inputValue);  // minValue = bias
+  static float Quadratic(float inputValue, float bias = 0, float range = 0);  // minValue = bias
 
   static float ParticleLife(float minValue, float maxValue, float attraction, float inputValue);  // minValue = bias
 };

Placement.cpp

@@ -9,7 +9,7 @@ Placement::Placement() {
 //   this->thing = thing;
 // }
 
-Placement::Placement(Vector2 direction, Sensor* thing) {
+Placement::Placement(Vector3 direction, Sensor* thing) {
   this->position = Vector3::zero;
   this->direction = direction;
   this->thing = thing;
@@ -23,7 +23,7 @@ Placement::Placement(Vector2 direction, Sensor* thing) {
 
 Placement::Placement(Vector3 position, Motor* thing) {
   this->position = position;
-  this->direction = Vector2::zero;
+  this->direction = Vector3::zero;
   this->thing = thing;
 }
 

Placement.h

@@ -10,7 +10,7 @@
 class Placement {
  public:
   Placement();
-  Placement(Vector2 direction, Sensor* sensor);
+  Placement(Vector3 direction, Sensor* sensor);
 
   //Placement(Vector3 position, Sensor* sensor);
   Placement(Vector3 position, Motor* motor);
@@ -22,6 +22,6 @@ class Placement {
   unsigned int childCount = 0;
 
   Vector3 position;
-  Vector2 direction;
+  Vector3 direction;
   Thing* thing;
 };

Propulsion.cpp

@@ -33,6 +33,10 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
 
 }
 
+void Propulsion::AddQuadcopter() {
+    this->quadcopter = new Quadcopter();
+}
+
 unsigned int Propulsion::GetMotorCount() {
     return this->motorCount;
 }
@@ -95,6 +99,10 @@ 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);
+
+  if (quadcopter != nullptr) {
+      quadcopter->SetTwistSpeed(forward, yaw, pitch);
+  }
 }
 
 void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) {
@@ -103,6 +111,11 @@ void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity)
   SetDiffDriveVelocities(leftVelocity, rightVelocity);
 }
 
+void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed, float rollSpeed) {
+    if (quadcopter != nullptr)
+        quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
+}
+
 Quadcopter* Propulsion::GetQuadcopter() {
     return quadcopter;
 }

Propulsion.h

@@ -22,6 +22,7 @@ class Propulsion {
 
   //void AddMotors(MotorPlacement* motors, unsigned int motorCount);
   void AddMotors(Placement* motors, unsigned int motorCount);
+  void AddQuadcopter();
 
   unsigned int GetMotorCount();
   Motor* GetMotor(unsigned int motorIx);
@@ -35,7 +36,7 @@ class Propulsion {
 
   // Think: drones
   Quadcopter* GetQuadcopter();
-  void SetLinearSpeed(Vector3 direction);
+  void SetLinearSpeed(Vector3 direction, float yawSpeed = 0.0F, float rollSpeed = 0.0F);
 
  protected:
   //unsigned long lastMillis;

Quadcopter.cpp

@@ -3,7 +3,29 @@
 Quadcopter::Quadcopter() {
 }
 
-void Quadcopter::LinearMotion(Vector3 velocity, float yawRate) {
+void Quadcopter::LinearMotion(Vector3 velocity, float yawSpeed, float rollSpeed) {
 	this->velocity = velocity;
-	this->yawRate = yawRate;
-}
+	this->yawSpeed = yawSpeed;
+	this->rollSpeed = rollSpeed;
+}
+
+void Quadcopter::SetTwistSpeed(float forward, float yaw, float pitch) {
+	this->velocity = Vector3::forward * forward;
+	this->yawSpeed = yaw;
+	this->pitchSpeed = pitch;
+}
+
+Vector3 Quadcopter::GetTargetVelocity() {
+	return this->velocity;
+}
+
+float Quadcopter::GetPitchSpeed() {
+	return this->pitchSpeed;
+}
+
+float Quadcopter::GetYawSpeed() {
+	return this->yawSpeed;
+}
+float Quadcopter::GetRollSpeed() {
+	return this->rollSpeed;
+}

Quadcopter.h

@@ -7,8 +7,16 @@ class Quadcopter : public Thing {
 public:
 	Quadcopter();
 
-	void LinearMotion(Vector3 velocity, float yawRate = 0.0F);
+	void LinearMotion(Vector3 velocity, float yawSpeed = 0.0F, float rollSpeed = 0.0);
+	void SetTwistSpeed(float forward, float yaw, float pitch);
+
+	Vector3 GetTargetVelocity();
+	float GetYawSpeed();
+	float GetPitchSpeed();
+	float GetRollSpeed();
 protected:
 	Vector3 velocity = Vector3::zero;
-	float yawRate = 0.0F;
+	float pitchSpeed = 0.0F;
+	float yawSpeed = 0.0F;
+	float rollSpeed = 0.0F;
 };

Sensing.cpp

@@ -109,6 +109,40 @@ float Sensing::DistanceRight(float angle) {
   return minDistance;
 }
 
+float Sensing::DistanceUp(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.y; // not correct!
+        if (sensorAngle > 0 && sensorAngle < angle) {
+            minDistance = fmin(minDistance, distanceSensor->GetDistance());
+        }
+    }
+    return minDistance;
+}
+
+float Sensing::DistanceDown(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.y; // not correct!
+        if (sensorAngle < 0 && sensorAngle > -angle) {
+            minDistance = fmin(minDistance, distanceSensor->GetDistance());
+        }
+    }
+    return minDistance;
+}
+
 bool Sensing::SwitchOn(float fromAngle, float toAngle) {
   if (toAngle < fromAngle)
     return false;

Sensing.h

@@ -59,6 +59,11 @@ class Sensing {
   /// @note When an object is beyond `angle` meters, it is not reported.
   float DistanceRight(float angle);
 
+  float DistanceUp() { return DistanceUp(180); }
+  float DistanceUp(float angle);
+  float DistanceDown() { return DistanceDown(180); }
+  float DistanceDown(float angle);
+
   float Distance(float leftAngle, float rightAngle);
 
   bool SwitchOn(float fromAngle, float toAngle);