Simulator compatibility

Activation.cpp

@@ -1,7 +1,7 @@
-#include <Activation.h>
+#include "Activation.h"
 
 float Activation::HeavisideStep(float inputValue, float bias) {
-  return (inputValue + bias > 0) ? 1 : 0;
+  return (inputValue + bias > 0) ? 1.0F : 0.0F;
 }
 
 float Activation::Linear(float inputValue, float bias, float range) {

ControlledMotor.cpp

@@ -1,4 +1,4 @@
-#include <ControlledMotor.h>
+#include "ControlledMotor.h"
 
 ControlledMotor::ControlledMotor() {}
 

ControlledMotor.h

@@ -1,7 +1,7 @@
 #pragma once
 
-#include <Encoder.h>
-#include <Motor.h>
+#include "Encoder.h"
+#include "Motor.h"
 
 class ControlledMotor : public Thing {
  public:
@@ -18,7 +18,7 @@ class ControlledMotor : public Thing {
 
   void SetTargetVelocity(float rotationsPerSecond);
   float GetActualVelocity() {
-    return rotationDirection * encoder->GetRotationsPerSecond();
+    return (int)rotationDirection * encoder->GetRotationsPerSecond();
   }  // in rotations per second
 
  protected:

DistanceSensor.h

@@ -5,9 +5,12 @@
 /// @brief A sensor which can measure the distance the the nearest object
 class DistanceSensor : public Sensor {
  public:
+     DistanceSensor() { isDistanceSensor = true; }
+     DistanceSensor(float triggerDistance) { isDistanceSensor = true; this->triggerDistance = triggerDistance; }
   /// @brief Determine the distance to the nearest object
   /// @return the measured distance in meters to the nearest object
-  virtual float GetDistance() = 0;
+     virtual float GetDistance() { return distance; };
+     virtual void SetDistance(float distance) { this->distance = distance; }; // for simulation purposes
 
   /// @brief The distance at which ObjectNearby triggers
   float triggerDistance = 1;
@@ -21,4 +24,6 @@ class DistanceSensor : public Sensor {
     bool isOff = GetDistance() > triggerDistance;
     return isOff;
   }
+protected:
+    float distance = 0;
 };

Encoder.cpp

@@ -1,4 +1,4 @@
-#include <Encoder.h>
+#include "Encoder.h"
 
 volatile unsigned char Encoder::transitionCount = 0;
 

Motor.cpp

@@ -1,7 +1,7 @@
-#include <Motor.h>
+#include "Motor.h"
 
 Motor::Motor() {
-  this->isSensor = false;
+  this->isMotor = true;
 }
 
 // Motor::Motor(uint8_t pinIn1, uint8_t pinIn2) {
@@ -23,3 +23,11 @@ Motor::Motor() {
 //   analogWrite(pinIn1, speed);
 //   analogWrite(pinIn2, 255 - speed);
 // }
+
+float Motor::GetSpeed() {
+	return this->currentSpeed;
+}
+
+void Motor::SetSpeed(float speed) {
+	this->currentSpeed = speed;
+}

Motor.h

@@ -12,7 +12,7 @@ class Motor : public Thing {
   /// @brief Set the turning direction of the motor
   // void SetDirection(Direction direction);
 
-  virtual void SetSpeed(float speed) = 0;
+  virtual void SetSpeed(float speed);
   float GetSpeed();
 
  protected:

Placement.cpp

@@ -1,4 +1,4 @@
-#include <Placement.h>
+#include "Placement.h"
 
 Placement::Placement() {
   this->position = Vector3::zero;
@@ -15,11 +15,11 @@ Placement::Placement(Vector2 direction, Sensor* thing) {
   this->thing = thing;
 }
 
-Placement::Placement(Vector3 position, Sensor* thing) {
-  this->position = position;
-  this->direction = Vector2::zero;
-  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;
@@ -27,13 +27,13 @@ Placement::Placement(Vector3 position, Motor* thing) {
   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::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

@@ -1,21 +1,21 @@
 #pragma once
 
-#include <ControlledMotor.h>
-#include <Motor.h>
-#include <Thing.h>
-#include <Vector2.h>
-#include <Vector3.h>
-#include "Sensor.h"
+#include "ControlledMotor.h"
+#include "Motor.h"
+#include "Thing.h"
+#include "Vector2.h"
+#include "Vector3.h"
+#include "DistanceSensor.h"
 
 class Placement {
  public:
   Placement();
   Placement(Vector2 direction, Sensor* sensor);
 
-  Placement(Vector3 position, Sensor* sensor);
+  //Placement(Vector3 position, Sensor* sensor);
   Placement(Vector3 position, Motor* motor);
-  Placement(Vector3 position, ControlledMotor* motor);
-  Placement(Thing* thing, Vector3 position);
+  /*Placement(Vector3 position, ControlledMotor* motor);
+  Placement(Thing* thing, Vector3 position);*/
 
   Placement* parent = nullptr;
   Placement** children = nullptr;

Propulsion.cpp

@@ -1,21 +1,51 @@
-#include <ControlledMotor.h>
-#include <Propulsion.h>
+#include "ControlledMotor.h"
+#include "Propulsion.h"
 
-#include <FloatSingle.h>
+#include "FloatSingle.h"
 
 Propulsion::Propulsion() {
-  this->motors = nullptr;
+  this->placement = nullptr;
   this->motorCount = 0;
 }
 
-void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) {
-  this->motors = motors;
-  this->motorCount = motorCount;
+//void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) {
+//  this->palce = motors;
+//  this->motorCount = motorCount;
+//}
+
+void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
+  //this->placement = motors;
+  //this->motorCount = motorCount;
+    this->motorCount = 0;
+    for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
+        Thing* thing = things[thingIx].thing;
+        if (thing->isMotor)
+            motorCount++;
+    }
+    this->placement = new Placement[motorCount];
+
+    unsigned int motorIx = 0;
+    for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
+        Thing* thing = things[thingIx].thing;
+        if (thing->isMotor)
+            this->placement[motorIx++] = things[thingIx];
+    }
+
 }
 
-void Propulsion::AddMotors(Placement* motors, unsigned int motorCount) {
-  this->placement = motors;
-  this->motorCount = motorCount;
+unsigned int Propulsion::GetMotorCount() {
+    return this->motorCount;
+}
+
+Motor* Propulsion::GetMotor(unsigned int motorId) {
+    if (motorId >= this->motorCount)
+        return nullptr;
+
+    Thing* thing = this->placement[motorId].thing;
+    if (thing->isMotor)
+        return (Motor*)thing;
+
+    return nullptr;
 }
 
 void Propulsion::Update() {
@@ -25,18 +55,18 @@ void Propulsion::Update() {
   // lastMillis = curMillis;
 
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    MotorPlacement placement = motors[motorIx];
+    //Placement placement = placement[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;
+    Motor* motor = (Motor*) placement[motorIx].thing;
     if (motor == nullptr)
       continue;
 
-    float xPosition = motors[motorIx].position.x;
+    float xPosition = placement[motorIx].position.x;
     if (xPosition < 0)
       motor->SetSpeed(leftSpeed);
     else if (xPosition > 0)
@@ -46,11 +76,11 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float 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);
+    //Placement placement = placement[motorIx];
+    //if (placement.position.x < 0)
+    //  placement.controlledMotor->SetTargetVelocity(leftVelocity);
+    //else if (placement.position.x > 0)
+    //  placement.controlledMotor->SetTargetVelocity(rightVelocity);
   };
 }
 

Propulsion.h

@@ -1,16 +1,16 @@
 #pragma once
 
-#include <ControlledMotor.h>
-#include <Placement.h>
-#include <Vector2.h>
+#include "ControlledMotor.h"
+#include "Placement.h"
+#include "Vector2.h"
 
 #include <list>
 
-struct MotorPlacement {
-  Motor* motor;
-  ControlledMotor* controlledMotor;
-  Vector2 position;
-};
+//struct MotorPlacement {
+//  Motor* motor;
+//  ControlledMotor* controlledMotor;
+//  Vector2 position;
+//};
 
 class Propulsion {
  public:
@@ -19,9 +19,12 @@ class Propulsion {
 
   void Update();
 
-  void AddMotors(MotorPlacement* motors, unsigned int motorCount);
+  //void AddMotors(MotorPlacement* motors, unsigned int motorCount);
   void AddMotors(Placement* motors, unsigned int motorCount);
 
+  unsigned int GetMotorCount();
+  Motor* GetMotor(unsigned int motorIx);
+
   void SetDiffDriveSpeed(float leftSpeed, float rightSpeed);
   void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);
 
@@ -33,8 +36,8 @@ class Propulsion {
   void SetLinearSpeed(Vector3 direction);
 
  protected:
-  unsigned long lastMillis;
-  MotorPlacement* motors = nullptr;
+  //unsigned long lastMillis;
+  //MotorPlacement* motors = nullptr;
   Placement* placement = nullptr;
   unsigned int motorCount = 0;
 };

Roboid.cpp

@@ -1,4 +1,4 @@
-#include <Roboid.h>
+#include "Roboid.h"
 
 Roboid::Roboid() {
   this->configuration = nullptr;
@@ -10,4 +10,5 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
   this->thingCount = thingCount;
 
   sensing.AddSensors(configuration, thingCount);
+  propulsion.AddMotors(configuration, thingCount);
 }

Roboid.h

@@ -1,9 +1,9 @@
 #pragma once
 
-#include <Activation.h>
-#include <Placement.h>
-#include <Propulsion.h>
-#include <Sensing.h>
+#include "Activation.h"
+#include "Placement.h"
+#include "Propulsion.h"
+#include "Sensing.h"
 
 class Roboid {
  public:

Sensing.cpp

@@ -1,6 +1,6 @@
-#include <DistanceSensor.h>
-#include <Sensing.h>
-#include <Switch.h>
+#include "DistanceSensor.h"
+#include "Sensing.h"
+//#include <Switch.h>
 
 #include <math.h>
 #include <algorithm>
@@ -41,6 +41,21 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
   }
 }
 
+unsigned int Sensing::GetSensorCount() {
+    return this->sensorCount;
+}
+
+Sensor* Sensing::GetSensor(unsigned int sensorId) {
+    if (sensorId >= this->sensorCount)
+        return nullptr;
+
+    Thing* thing = this->sensorPlacements[sensorId].thing;
+    if (thing->isSensor)
+        return (Sensor*)thing;
+
+    return nullptr;
+}
+
 float Sensing::DistanceForward(float angle) {
   float minDistance = INFINITY;
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {

Sensing.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include <Placement.h>
+#include "Placement.h"
 
 #include <list>
 
@@ -27,6 +27,9 @@ class Sensing {
   // void AddSensors(SensorPlacement* sensors, unsigned int sensorCount);
   void AddSensors(Placement* sensors, unsigned int sensorCount);
 
+  unsigned int GetSensorCount();
+  Sensor* GetSensor(unsigned int sensorId);
+
   float DistanceForward(float angle = 90);
 
   /// @brief Distance to the closest object on the left

Sensor.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include <Thing.h>
+#include "Thing.h"
 
 class Sensor : public Thing {
  public:

Thing.h

@@ -1,6 +1,8 @@
 #pragma once
 
 class Thing {
- public:
-  bool isSensor;
+public:
+	Thing() { isSensor = false, isMotor = false; }
+	bool isSensor;
+	bool isMotor;
 };