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

Improvements

Browse Source
This commit is contained in:
Pascal Serrarens 2023-12-04 10:38:07 +01:00
parent 7c68558d80
commit b2914e437b
7 changed files with 106 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Motor.cpp
View File

@ -1,15 +1,9 @@
#include "Motor.h"
#include <time.h>
// #include <Arduino.h>
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;
}

20
Perception.cpp Normal file
View File

@ -0,0 +1,20 @@
#include "Perception.h"
#include "Angle.h"
#include <Arduino.h>
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;
}

11
Perception.h
View File

@ -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;

59
Propulsion.cpp
View File

@ -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;
}
}

24
Propulsion.h
View File

@ -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;

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

6
Thing.h
View File

@ -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;