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