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 "Angle.h"
#include <Arduino.h>
#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;

58
Sensing.cpp
View File

@ -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,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)