RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Removed printfs

Browse Source
This commit is contained in:
Pascal Serrarens 2023-12-04 10:44:54 +01:00
parent b2914e437b
commit d348070092
2 changed files with 35 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Perception.cpp
View File

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

54
Sensing.cpp
View File

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