From d348070092878f7dd28f14bf4d69289693b66ee2 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 4 Dec 2023 10:44:54 +0100 Subject: [PATCH] Removed printfs --- Perception.cpp | 11 +++++----- Sensing.cpp | 58 ++++++++++++++++++++++++++------------------------ 2 files changed, 35 insertions(+), 34 deletions(-) diff --git a/Perception.cpp b/Perception.cpp index 5e51f54..15c7ac2 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -1,19 +1,18 @@ #include "Perception.h" -#include "Angle.h" #include +#include "Angle.h" -DistanceSensor *Perception::GetSensor(float angle) { +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; + return (DistanceSensor*)placement.thing; } - DistanceSensor *distanceSensor = new DistanceSensor(); - Serial.printf("New sensor ADDED %f \n", angle); - Placement *newPlacement = new Placement(distanceSensor, angle); + DistanceSensor* distanceSensor = new DistanceSensor(); + Placement* newPlacement = new Placement(distanceSensor, angle); AddSensors(newPlacement, 1); return distanceSensor; diff --git a/Sensing.cpp b/Sensing.cpp index 92e1a36..85bb126 100644 --- a/Sensing.cpp +++ b/Sensing.cpp @@ -4,13 +4,13 @@ #include -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,22 +30,24 @@ 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() { return this->sensorCount; } +unsigned int Sensing::GetSensorCount() { + 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; } @@ -54,11 +56,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()); @@ -71,11 +73,11 @@ 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.y; // Serial.printf(" distance sensor: %f %f 0\n", -angle, sensorAngle); if (sensorAngle < 0 && sensorAngle > -angle) { @@ -87,16 +89,14 @@ 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) continue; - DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing; + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; 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()); } @@ -108,12 +108,12 @@ 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; - float sensorAngle = placement.direction.y; // not correct! + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + float sensorAngle = placement.direction.y; // not correct! if (sensorAngle > 0 && sensorAngle < angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } @@ -125,12 +125,12 @@ 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; - float sensorAngle = placement.direction.y; // not correct! + DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; + float sensorAngle = placement.direction.y; // not correct! if (sensorAngle < 0 && sensorAngle > -angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } @@ -146,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; } @@ -182,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(); } } @@ -200,7 +200,9 @@ 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)