Improvements

Motor.cpp

@@ -1,15 +1,9 @@
 #include "Motor.h"
 #include <time.h>
 
-// #include <Arduino.h>
+Motor::Motor() { type = (int)Thing::UncontrolledMotorType; }
 
-Motor::Motor() {
+float Motor::GetSpeed() { return this->currentTargetSpeed; }
-  type = Thing::MotorType;
-}
-
-float Motor::GetSpeed() {
-  return this->currentTargetSpeed;
-}
 
 void Motor::SetSpeed(float targetSpeed) {
   this->currentTargetSpeed = targetSpeed;

Perception.cpp

@@ -0,0 +1,20 @@
+#include "Perception.h"
+#include "Angle.h"
+#include <Arduino.h>
+
+DistanceSensor *Perception::GetSensor(float angle) {
+  angle = Angle::Normalize(angle);
+
+  for (unsigned int ix = 0; ix < this->sensorCount; ix++) {
+    Placement placement = this->sensorPlacements[ix];
+    if (abs(placement.direction.y - angle) < 0.01F)
+      return (DistanceSensor *)placement.thing;
+  }
+
+  DistanceSensor *distanceSensor = new DistanceSensor();
+  Serial.printf("New sensor ADDED %f \n", angle);
+  Placement *newPlacement = new Placement(distanceSensor, angle);
+  AddSensors(newPlacement, 1);
+
+  return distanceSensor;
+}

Perception.h

@@ -3,8 +3,13 @@
 namespace Passer {
 namespace RoboidControl {
 
-using Perception = Sensing;
+// using Perception = Sensing;
 
-}
+class Perception : public Sensing {
+public:
+  DistanceSensor *GetSensor(float angle);
+};
+
+} // namespace RoboidControl
 } // namespace Passer
 using namespace Passer::RoboidControl;

Propulsion.cpp

@@ -10,10 +10,10 @@ Propulsion::Propulsion() {
   this->motorCount = 0;
 }
 
-void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
+void Propulsion::AddMotors(Placement *things, unsigned int thingCount) {
   this->motorCount = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
-    Thing* thing = things[thingIx].thing;
+    Thing *thing = things[thingIx].thing;
     if ((thing->type & Thing::MotorType) != 0)
       motorCount++;
     if (thing->type == (int)Thing::Type::ControlledMotor)
@@ -23,56 +23,58 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
 
   unsigned int motorIx = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
-    Thing* thing = things[thingIx].thing;
+    Thing *thing = things[thingIx].thing;
     if ((thing->type & Thing::MotorType) != 0)
       this->placement[motorIx++] = things[thingIx];
   }
 }
 
-void Propulsion::AddQuadcopter(Quadcopter* quadcopter) {
+void Propulsion::AddQuadcopter(Quadcopter *quadcopter) {
   this->quadcopter = quadcopter;
 }
 
-unsigned int Propulsion::GetMotorCount() {
+unsigned int Propulsion::GetMotorCount() { return this->motorCount; }
-  return this->motorCount;
-}
 
-Motor* Propulsion::GetMotor(unsigned int motorId) {
+Motor *Propulsion::GetMotor(unsigned int motorId) {
   if (motorId >= this->motorCount)
     return nullptr;
 
-  Thing* thing = this->placement[motorId].thing;
+  Thing *thing = this->placement[motorId].thing;
   if ((thing->type & Thing::MotorType) != 0)
-    return (Motor*)thing;
+    return (Motor *)thing;
+
+  return nullptr;
+}
+
+Placement *Propulsion::GetMotorPlacement(unsigned int motorId) {
+  if (motorId >= this->motorCount)
+    return nullptr;
+
+  Placement *placement = &this->placement[motorId];
+  if ((placement->thing->type & Thing::MotorType) != 0)
+    return placement;
 
   return nullptr;
 }
 
 void Propulsion::Update(float currentTimeMs) {
-  // time_t currentTime = time(NULL);
-  // float timeStep =
-  //     currentTimeMs -
-  //     this->lastUpdateTime;  // difftime(currentTime, this->lastUpdateTime);
-
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Thing* thing = placement[motorIx].thing;
+    Thing *thing = placement[motorIx].thing;
     if (thing->type == Thing::ControlledMotorType) {
-      ControlledMotor* motor = (ControlledMotor*)thing;
+      ControlledMotor *motor = (ControlledMotor *)thing;
       motor->Update(currentTimeMs);
     }
   }
   this->lastUpdateTime = currentTimeMs;
 }
 
-void Propulsion::SetMaxSpeed(float maxSpeed) {
+void Propulsion::SetMaxSpeed(float maxSpeed) { this->maxSpeed = abs(maxSpeed); }
-  this->maxSpeed = abs(maxSpeed);
-}
 
 void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Thing* thing = placement[motorIx].thing;
+    Thing *thing = placement[motorIx].thing;
     if (thing->type == Thing::UncontrolledMotorType) {
-      Motor* motor = (Motor*)thing;
+      Motor *motor = (Motor *)thing;
       if (motor == nullptr)
         continue;
 
@@ -83,7 +85,7 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
         motor->SetSpeed(rightSpeed);
 
     } else if (thing->type == Thing::ControlledMotorType) {
-      ControlledMotor* motor = (ControlledMotor*)placement[motorIx].thing;
+      ControlledMotor *motor = (ControlledMotor *)placement[motorIx].thing;
       if (motor == nullptr)
         continue;
 
@@ -139,16 +141,13 @@ void Propulsion::SetTwistVelocity(float forwardVelocity,
   SetDiffDriveVelocities(leftVelocity, rightVelocity);
 }
 
-void Propulsion::SetLinearSpeed(Vector3 velocity,
+void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed,
-                                float yawSpeed,
                                 float rollSpeed) {
   if (quadcopter != nullptr)
     quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
 }
 
-Quadcopter* Propulsion::GetQuadcopter() {
+Quadcopter *Propulsion::GetQuadcopter() { return quadcopter; }
-  return quadcopter;
-}
 
 /// @brief Odometer returns the total distance traveled since start
 /// @return The total distance
@@ -160,9 +159,9 @@ Quadcopter* Propulsion::GetQuadcopter() {
 float Propulsion::GetOdometer() {
   float odometer = 0;
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Thing* thing = placement[motorIx].thing;
+    Thing *thing = placement[motorIx].thing;
     if ((thing->type & Thing::ControlledMotorType) != 0) {
-      ControlledMotor* motor = (ControlledMotor*)thing;
+      ControlledMotor *motor = (ControlledMotor *)thing;
       odometer += motor->encoder->GetDistance() / this->motorCount;
     }
   }

Propulsion.h

@@ -10,18 +10,19 @@ namespace Passer {
 namespace RoboidControl {
 
 class Propulsion {
- public:
+public:
   /// @brief Setup sensing
   Propulsion();
 
   void Update(float currentTimeMs);
 
-  void AddMotors(Placement* motors, unsigned int motorCount);
+  void AddMotors(Placement *motors, unsigned int motorCount);
-  void AddQuadcopter(Quadcopter* quadcopter);
+  void AddQuadcopter(Quadcopter *quadcopter);
-  Quadcopter* GetQuadcopter();
+  Quadcopter *GetQuadcopter();
 
   unsigned int GetMotorCount();
-  Motor* GetMotor(unsigned int motorIx);
+  Motor *GetMotor(unsigned int motorIx);
+  Placement *GetMotorPlacement(unsigned int motorIx);
 
   /// @brief Set the maximum linear speed.
   /// @param maxSpeed The new maximum linear speed
@@ -41,8 +42,7 @@ class Propulsion {
   void SetTwistVelocity(float forward, float yaw);
 
   // Think: drones
-  void SetLinearSpeed(Vector3 direction,
+  void SetLinearSpeed(Vector3 direction, float yawSpeed = 0.0F,
-                      float yawSpeed = 0.0F,
                       float rollSpeed = 0.0F);
 
   // Position control (or actually, distance control)
@@ -50,11 +50,11 @@ class Propulsion {
 
   float GetOdometer();
 
-  Placement* placement = nullptr;
+  Placement *placement = nullptr;
   unsigned int motorCount = 0;
 
- protected:
+protected:
-  Quadcopter* quadcopter = nullptr;
+  Quadcopter *quadcopter = nullptr;
 
   float maxSpeed = 1;
 

Sensing.cpp

@@ -4,13 +4,13 @@
 
 #include <math.h>
 
-SensorPlacement::SensorPlacement(DistanceSensor* distanceSensor,
+SensorPlacement::SensorPlacement(DistanceSensor *distanceSensor,
                                  Vector2 direction) {
   this->distanceSensor = distanceSensor;
   this->switchSensor = nullptr;
   this->direction = direction;
 }
-SensorPlacement::SensorPlacement(Switch* switchSensor, Vector2 direction) {
+SensorPlacement::SensorPlacement(Switch *switchSensor, Vector2 direction) {
   this->distanceSensor = nullptr;
   this->switchSensor = switchSensor;
   this->direction = direction;
@@ -18,10 +18,10 @@ SensorPlacement::SensorPlacement(Switch* switchSensor, Vector2 direction) {
 
 Sensing::Sensing() {}
 
-void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
+void Sensing::AddSensors(Placement *things, unsigned int thingCount) {
   sensorCount = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
-    Thing* thing = things[thingIx].thing;
+    Thing *thing = things[thingIx].thing;
     if ((thing->type & Thing::SensorType) != 0)
       sensorCount++;
   }
@@ -30,24 +30,22 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
 
   unsigned int sensorIx = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
-    Thing* thing = things[thingIx].thing;
+    Thing *thing = things[thingIx].thing;
     if ((thing->type & Thing::SensorType) != 0) {
       sensorPlacements[sensorIx++] = things[thingIx];
     }
   }
 }
 
-unsigned int Sensing::GetSensorCount() {
+unsigned int Sensing::GetSensorCount() { return this->sensorCount; }
-  return this->sensorCount;
-}
 
-Sensor* Sensing::GetSensor(unsigned int sensorId) {
+Sensor *Sensing::GetSensor(unsigned int sensorId) {
   if (sensorId >= this->sensorCount)
     return nullptr;
 
-  Thing* thing = this->sensorPlacements[sensorId].thing;
+  Thing *thing = this->sensorPlacements[sensorId].thing;
   if (thing->type & Thing::SensorType != 0)
-    return (Sensor*)thing;
+    return (Sensor *)thing;
 
   return nullptr;
 }
@@ -56,11 +54,11 @@ 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;
+    Sensor *sensor = (Sensor *)placement.thing;
     if (sensor->type & Thing::SensorType != 0)
       continue;
 
-    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
+    DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing;
     float sensorAngle = placement.direction.z;
     if (sensorAngle > 0 && sensorAngle < angle) {
       minDistance = fmin(minDistance, distanceSensor->GetDistance());
@@ -73,12 +71,12 @@ 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;
+    Sensor *sensor = (Sensor *)placement.thing;
     if (sensor->type & Thing::SensorType != 0)
       continue;
 
-    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
+    DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing;
-    float sensorAngle = placement.direction.x;
+    float sensorAngle = placement.direction.y;
     // Serial.printf("  distance sensor: %f %f 0\n", -angle, sensorAngle);
     if (sensorAngle < 0 && sensorAngle > -angle) {
       minDistance = fmin(minDistance, distanceSensor->GetDistance());
@@ -89,15 +87,16 @@ float Sensing::DistanceLeft(float angle) {
 
 float Sensing::DistanceRight(float angle) {
   float minDistance = INFINITY;
+  Serial.printf("  distance sensor count: %d\n", sensorCount);
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
-    Sensor* sensor = (Sensor*)placement.thing;
+    Sensor *sensor = (Sensor *)placement.thing;
-    if (sensor->type & Thing::DistanceSensorType != 0)
+    if (sensor->type != Thing::DistanceSensorType)
       continue;
 
-    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
+    DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing;
-    float sensorAngle = placement.direction.x;
+    float sensorAngle = placement.direction.y;
-    // Serial.printf("  distance sensor: 0 %f %f\n", sensorAngle, angle);
+    Serial.printf("  distance sensor: 0 %f %f\n", sensorAngle, angle);
     if (sensorAngle > 0 && sensorAngle < angle) {
       minDistance = fmin(minDistance, distanceSensor->GetDistance());
     }
@@ -109,11 +108,11 @@ 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;
+    Sensor *sensor = (Sensor *)placement.thing;
     if (sensor->type & Thing::SensorType != 0)
       continue;
 
-    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
+    DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing;
     float sensorAngle = placement.direction.y; // not correct!
     if (sensorAngle > 0 && sensorAngle < angle) {
       minDistance = fmin(minDistance, distanceSensor->GetDistance());
@@ -126,11 +125,11 @@ 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;
+    Sensor *sensor = (Sensor *)placement.thing;
     if (sensor->type & Thing::SensorType != 0)
       continue;
 
-    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
+    DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing;
     float sensorAngle = placement.direction.y; // not correct!
     if (sensorAngle < 0 && sensorAngle > -angle) {
       minDistance = fmin(minDistance, distanceSensor->GetDistance());
@@ -147,16 +146,16 @@ bool Sensing::SwitchOn(float fromAngle, float toAngle) {
     Placement placement = sensorPlacements[sensorIx];
     float angle = placement.direction.y;
     if (angle > fromAngle && angle < toAngle) {
-      Thing* thing = placement.thing;
+      Thing *thing = placement.thing;
       if (thing == nullptr)
         continue;
 
       if ((thing->type & (int)Thing::Type::DistanceSensor) != 0) {
-        DistanceSensor* distanceSensor = (DistanceSensor*)thing;
+        DistanceSensor *distanceSensor = (DistanceSensor *)thing;
         if (distanceSensor != nullptr && distanceSensor->IsOn())
           return true;
       } else if ((thing->type & (int)Thing::Type::Switch) != 0) {
-        Switch* switchSensor = (Switch*)thing;
+        Switch *switchSensor = (Switch *)thing;
         if (switchSensor != nullptr && switchSensor->IsOn())
           return true;
       }
@@ -183,7 +182,7 @@ float Sensing::GetDistance(float angle) {
       Placement placement = sensorPlacements[sensorIx];
       float placementAngle = placement.direction.x;
       if (placementAngle == angle) {
-        DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
+        DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing;
         return distanceSensor->GetDistance();
       }
     }
@@ -201,9 +200,7 @@ void Sensing::SetRange(float min, float max) {
   this->rangeMaximum = max;
 }
 
-float* Sensing::GetDepthMap() {
+float *Sensing::GetDepthMap() { return this->depthMap; }
-  return this->depthMap;
-}
 
 void Sensing::SetDepthMap(float angle, float distance) {
   if (angle < rangeMinimum || angle > rangeMaximum)

Thing.h

@@ -5,7 +5,7 @@ namespace RoboidControl {
 
 /// @brief A thing is a functional component on a robot
 class Thing {
- public:
+public:
   Thing() { type = (int)Type::Undetermined; }
 
   enum class Type {