From b2914e437b58b6e7c620bc0adcec541dfecf9510 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 4 Dec 2023 10:38:07 +0100 Subject: [PATCH] Improvements --- Motor.cpp | 12 +++------- Perception.cpp | 20 ++++++++++++++++ Perception.h | 11 ++++++--- Propulsion.cpp | 59 ++++++++++++++++++++++----------------------- Propulsion.h | 24 +++++++++---------- Sensing.cpp | 65 ++++++++++++++++++++++++-------------------------- Thing.h | 6 ++--- 7 files changed, 106 insertions(+), 91 deletions(-) create mode 100644 Perception.cpp diff --git a/Motor.cpp b/Motor.cpp index d083033..f18a7c9 100644 --- a/Motor.cpp +++ b/Motor.cpp @@ -1,15 +1,9 @@ #include "Motor.h" #include -// #include +Motor::Motor() { type = (int)Thing::UncontrolledMotorType; } -Motor::Motor() { - type = Thing::MotorType; -} - -float Motor::GetSpeed() { - return this->currentTargetSpeed; -} +float Motor::GetSpeed() { return this->currentTargetSpeed; } void Motor::SetSpeed(float targetSpeed) { this->currentTargetSpeed = targetSpeed; @@ -27,6 +21,6 @@ bool Motor::Drive(float distance) { return true; } - SetSpeed(distance < 0 ? -1 : 1); // max speed + SetSpeed(distance < 0 ? -1 : 1); // max speed return false; } \ No newline at end of file diff --git a/Perception.cpp b/Perception.cpp new file mode 100644 index 0000000..5e51f54 --- /dev/null +++ b/Perception.cpp @@ -0,0 +1,20 @@ +#include "Perception.h" +#include "Angle.h" +#include + +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; + } + + DistanceSensor *distanceSensor = new DistanceSensor(); + Serial.printf("New sensor ADDED %f \n", angle); + Placement *newPlacement = new Placement(distanceSensor, angle); + AddSensors(newPlacement, 1); + + return distanceSensor; +} \ No newline at end of file diff --git a/Perception.h b/Perception.h index 7390d9b..48584d6 100644 --- a/Perception.h +++ b/Perception.h @@ -3,8 +3,13 @@ namespace Passer { namespace RoboidControl { -using Perception = Sensing; +// using Perception = Sensing; -} -} // namespace Passer +class Perception : public Sensing { +public: + DistanceSensor *GetSensor(float angle); +}; + +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/Propulsion.cpp b/Propulsion.cpp index 6a3c5d2..fd3449d 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -10,10 +10,10 @@ Propulsion::Propulsion() { this->motorCount = 0; } -void Propulsion::AddMotors(Placement* things, unsigned int thingCount) { +void Propulsion::AddMotors(Placement *things, unsigned int thingCount) { this->motorCount = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { - Thing* thing = things[thingIx].thing; + Thing *thing = things[thingIx].thing; if ((thing->type & Thing::MotorType) != 0) motorCount++; if (thing->type == (int)Thing::Type::ControlledMotor) @@ -23,56 +23,58 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) { unsigned int motorIx = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { - Thing* thing = things[thingIx].thing; + Thing *thing = things[thingIx].thing; if ((thing->type & Thing::MotorType) != 0) this->placement[motorIx++] = things[thingIx]; } } -void Propulsion::AddQuadcopter(Quadcopter* quadcopter) { +void Propulsion::AddQuadcopter(Quadcopter *quadcopter) { this->quadcopter = quadcopter; } -unsigned int Propulsion::GetMotorCount() { - return this->motorCount; -} +unsigned int Propulsion::GetMotorCount() { return this->motorCount; } -Motor* Propulsion::GetMotor(unsigned int motorId) { +Motor *Propulsion::GetMotor(unsigned int motorId) { if (motorId >= this->motorCount) return nullptr; - Thing* thing = this->placement[motorId].thing; + Thing *thing = this->placement[motorId].thing; if ((thing->type & Thing::MotorType) != 0) - return (Motor*)thing; + return (Motor *)thing; + + return nullptr; +} + +Placement *Propulsion::GetMotorPlacement(unsigned int motorId) { + if (motorId >= this->motorCount) + return nullptr; + + Placement *placement = &this->placement[motorId]; + if ((placement->thing->type & Thing::MotorType) != 0) + return placement; return nullptr; } void Propulsion::Update(float currentTimeMs) { - // time_t currentTime = time(NULL); - // float timeStep = - // currentTimeMs - - // this->lastUpdateTime; // difftime(currentTime, this->lastUpdateTime); - for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - Thing* thing = placement[motorIx].thing; + Thing *thing = placement[motorIx].thing; if (thing->type == Thing::ControlledMotorType) { - ControlledMotor* motor = (ControlledMotor*)thing; + ControlledMotor *motor = (ControlledMotor *)thing; motor->Update(currentTimeMs); } } this->lastUpdateTime = currentTimeMs; } -void Propulsion::SetMaxSpeed(float maxSpeed) { - this->maxSpeed = abs(maxSpeed); -} +void Propulsion::SetMaxSpeed(float maxSpeed) { this->maxSpeed = abs(maxSpeed); } void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - Thing* thing = placement[motorIx].thing; + Thing *thing = placement[motorIx].thing; if (thing->type == Thing::UncontrolledMotorType) { - Motor* motor = (Motor*)thing; + Motor *motor = (Motor *)thing; if (motor == nullptr) continue; @@ -83,7 +85,7 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) { motor->SetSpeed(rightSpeed); } else if (thing->type == Thing::ControlledMotorType) { - ControlledMotor* motor = (ControlledMotor*)placement[motorIx].thing; + ControlledMotor *motor = (ControlledMotor *)placement[motorIx].thing; if (motor == nullptr) continue; @@ -139,16 +141,13 @@ void Propulsion::SetTwistVelocity(float forwardVelocity, SetDiffDriveVelocities(leftVelocity, rightVelocity); } -void Propulsion::SetLinearSpeed(Vector3 velocity, - float yawSpeed, +void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed, float rollSpeed) { if (quadcopter != nullptr) quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed); } -Quadcopter* Propulsion::GetQuadcopter() { - return quadcopter; -} +Quadcopter *Propulsion::GetQuadcopter() { return quadcopter; } /// @brief Odometer returns the total distance traveled since start /// @return The total distance @@ -160,9 +159,9 @@ Quadcopter* Propulsion::GetQuadcopter() { float Propulsion::GetOdometer() { float odometer = 0; for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - Thing* thing = placement[motorIx].thing; + Thing *thing = placement[motorIx].thing; if ((thing->type & Thing::ControlledMotorType) != 0) { - ControlledMotor* motor = (ControlledMotor*)thing; + ControlledMotor *motor = (ControlledMotor *)thing; odometer += motor->encoder->GetDistance() / this->motorCount; } } diff --git a/Propulsion.h b/Propulsion.h index 06edc39..8317d50 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -10,18 +10,19 @@ namespace Passer { namespace RoboidControl { class Propulsion { - public: +public: /// @brief Setup sensing Propulsion(); void Update(float currentTimeMs); - void AddMotors(Placement* motors, unsigned int motorCount); - void AddQuadcopter(Quadcopter* quadcopter); - Quadcopter* GetQuadcopter(); + void AddMotors(Placement *motors, unsigned int motorCount); + void AddQuadcopter(Quadcopter *quadcopter); + Quadcopter *GetQuadcopter(); unsigned int GetMotorCount(); - Motor* GetMotor(unsigned int motorIx); + Motor *GetMotor(unsigned int motorIx); + Placement *GetMotorPlacement(unsigned int motorIx); /// @brief Set the maximum linear speed. /// @param maxSpeed The new maximum linear speed @@ -41,8 +42,7 @@ class Propulsion { void SetTwistVelocity(float forward, float yaw); // Think: drones - void SetLinearSpeed(Vector3 direction, - float yawSpeed = 0.0F, + void SetLinearSpeed(Vector3 direction, float yawSpeed = 0.0F, float rollSpeed = 0.0F); // Position control (or actually, distance control) @@ -50,11 +50,11 @@ class Propulsion { float GetOdometer(); - Placement* placement = nullptr; + Placement *placement = nullptr; unsigned int motorCount = 0; - protected: - Quadcopter* quadcopter = nullptr; +protected: + Quadcopter *quadcopter = nullptr; float maxSpeed = 1; @@ -66,6 +66,6 @@ class Propulsion { float lastUpdateTime; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/Sensing.cpp b/Sensing.cpp index c55a03a..92e1a36 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,24 +30,22 @@ 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; } @@ -56,11 +54,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()); @@ -73,12 +71,12 @@ 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; - float sensorAngle = placement.direction.x; + 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) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); @@ -89,15 +87,16 @@ 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; - if (sensor->type & Thing::DistanceSensorType != 0) + Sensor *sensor = (Sensor *)placement.thing; + if (sensor->type != Thing::DistanceSensorType) continue; - DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - float sensorAngle = placement.direction.x; - // Serial.printf(" distance sensor: 0 %f %f\n", sensorAngle, angle); + 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()); } @@ -109,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()); } @@ -126,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()); } @@ -147,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; } @@ -183,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(); } } @@ -201,9 +200,7 @@ 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) diff --git a/Thing.h b/Thing.h index 861f218..db6dd6b 100644 --- a/Thing.h +++ b/Thing.h @@ -5,7 +5,7 @@ namespace RoboidControl { /// @brief A thing is a functional component on a robot class Thing { - public: +public: Thing() { type = (int)Type::Undetermined; } enum class Type { @@ -31,6 +31,6 @@ class Thing { unsigned int type = (int)Type::Undetermined; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file