Improvements

Motor.cpp

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

@@ -33,9 +33,7 @@ void Propulsion::AddQuadcopter(Quadcopter* quadcopter) {
   this->quadcopter = quadcopter;
 }
 
-unsigned int Propulsion::GetMotorCount() {
-  return this->motorCount;
-}
+unsigned int Propulsion::GetMotorCount() { return this->motorCount; }
 
 Motor *Propulsion::GetMotor(unsigned int motorId) {
   if (motorId >= this->motorCount)
@@ -48,12 +46,18 @@ Motor* Propulsion::GetMotor(unsigned int motorId) {
   return nullptr;
 }
 
-void Propulsion::Update(float currentTimeMs) {
-  // time_t currentTime = time(NULL);
-  // float timeStep =
-  //     currentTimeMs -
-  //     this->lastUpdateTime;  // difftime(currentTime, this->lastUpdateTime);
+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) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
     Thing *thing = placement[motorIx].thing;
     if (thing->type == Thing::ControlledMotorType) {
@@ -64,9 +68,7 @@ void Propulsion::Update(float currentTimeMs) {
   this->lastUpdateTime = currentTimeMs;
 }
 
-void Propulsion::SetMaxSpeed(float maxSpeed) {
-  this->maxSpeed = abs(maxSpeed);
-}
+void Propulsion::SetMaxSpeed(float maxSpeed) { this->maxSpeed = abs(maxSpeed); }
 
 void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
@@ -139,16 +141,13 @@ void Propulsion::SetTwistVelocity(float forwardVelocity,
   SetDiffDriveVelocities(leftVelocity, rightVelocity);
 }
 
-void Propulsion::SetLinearSpeed(Vector3 velocity,
-                                float yawSpeed,
+void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed,
                                 float rollSpeed) {
   if (quadcopter != nullptr)
     quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
 }
 
-Quadcopter* Propulsion::GetQuadcopter() {
-  return quadcopter;
-}
+Quadcopter *Propulsion::GetQuadcopter() { return quadcopter; }
 
 /// @brief Odometer returns the total distance traveled since start
 /// @return The total distance

Propulsion.h

@@ -22,6 +22,7 @@ class Propulsion {
 
   unsigned int GetMotorCount();
   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,
-                      float yawSpeed = 0.0F,
+  void SetLinearSpeed(Vector3 direction, float yawSpeed = 0.0F,
                       float rollSpeed = 0.0F);
 
   // Position control (or actually, distance control)

Sensing.cpp

@@ -37,9 +37,7 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
   }
 }
 
-unsigned int Sensing::GetSensorCount() {
-  return this->sensorCount;
-}
+unsigned int Sensing::GetSensorCount() { return this->sensorCount; }
 
 Sensor *Sensing::GetSensor(unsigned int sensorId) {
   if (sensorId >= this->sensorCount)
@@ -78,7 +76,7 @@ float Sensing::DistanceLeft(float angle) {
       continue;
 
     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;
-    if (sensor->type & Thing::DistanceSensorType != 0)
+    if (sensor->type != Thing::DistanceSensorType)
       continue;
 
     DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing;
-    float sensorAngle = placement.direction.x;
-    // Serial.printf("  distance sensor: 0 %f %f\n", sensorAngle, angle);
+    float sensorAngle = placement.direction.y;
+    Serial.printf("  distance sensor: 0 %f %f\n", sensorAngle, angle);
     if (sensorAngle > 0 && sensorAngle < angle) {
       minDistance = fmin(minDistance, distanceSensor->GetDistance());
     }
@@ -201,9 +200,7 @@ void Sensing::SetRange(float min, float max) {
   this->rangeMaximum = max;
 }
 
-float* Sensing::GetDepthMap() {
-  return this->depthMap;
-}
+float *Sensing::GetDepthMap() { return this->depthMap; }
 
 void Sensing::SetDepthMap(float angle, float distance) {
   if (angle < rangeMinimum || angle > rangeMaximum)