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

Formatting

Browse Source
This commit is contained in:
Pascal Serrarens 2023-11-16 10:45:15 +01:00
parent 1abe40815e
commit a227f247e0
1 changed files with 41 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
Sensing.cpp
View File

@ -1,15 +1,12 @@
#include "DistanceSensor.h"
#include "Sensing.h" #include "Sensing.h"
//#include <Switch.h> #include "DistanceSensor.h"
// #include <Switch.h>
#include <math.h> #include <math.h>
#include <algorithm> #include <algorithm>
extern "C" { SensorPlacement::SensorPlacement(DistanceSensor* distanceSensor,
#include "debug.h" Vector2 direction) {
}
SensorPlacement::SensorPlacement(DistanceSensor* distanceSensor, Vector2 direction) {
this->distanceSensor = distanceSensor; this->distanceSensor = distanceSensor;
this->switchSensor = nullptr; this->switchSensor = nullptr;
this->direction = direction; this->direction = direction;
@ -22,7 +19,8 @@ SensorPlacement::SensorPlacement(Switch* switchSensor, Vector2 direction) {
Sensing::Sensing() {} Sensing::Sensing() {}
// void Sensing::AddSensors(SensorPlacement* sensors, unsigned int sensorCount) { // void Sensing::AddSensors(SensorPlacement* sensors, unsigned int sensorCount)
// {
// this->sensors = sensors; // this->sensors = sensors;
// this->sensorCount = sensorCount; // this->sensorCount = sensorCount;
// } // }
@ -46,18 +44,18 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
} }
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) if (sensorId >= this->sensorCount)
return nullptr;
Thing* thing = this->sensorPlacements[sensorId].thing;
if (thing->isSensor)
return (Sensor*)thing;
return nullptr; return nullptr;
Thing* thing = this->sensorPlacements[sensorId].thing;
if (thing->isSensor)
return (Sensor*)thing;
return nullptr;
} }
float Sensing::DistanceForward(float angle) { float Sensing::DistanceForward(float angle) {
@ -114,37 +112,37 @@ float Sensing::DistanceRight(float angle) {
} }
float Sensing::DistanceUp(float angle) { 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->isDistanceSensor == false) if (sensor->isDistanceSensor == false)
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());
}
} }
return minDistance; }
return minDistance;
} }
float Sensing::DistanceDown(float angle) { 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->isDistanceSensor == false) if (sensor->isDistanceSensor == false)
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());
}
} }
return minDistance; }
return minDistance;
} }
bool Sensing::SwitchOn(float fromAngle, float toAngle) { bool Sensing::SwitchOn(float fromAngle, float toAngle) {
@ -157,7 +155,8 @@ bool Sensing::SwitchOn(float fromAngle, float toAngle) {
if (angle > fromAngle && angle < toAngle) { if (angle > fromAngle && angle < toAngle) {
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
// if (placement.switchSensor != nullptr && placement.switchSensor->IsOn()) // if (placement.switchSensor != nullptr &&
// placement.switchSensor->IsOn())
// return true; // return true;
// else // else
if (distanceSensor != nullptr && distanceSensor->IsOn()) if (distanceSensor != nullptr && distanceSensor->IsOn())