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

Updated Thing

Browse Source
This commit is contained in:
Pascal Serrarens 2023-12-05 11:13:13 +01:00
parent 7c5a531e0c
commit f33b02311c
5 changed files with 107 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Perception.cpp
View File

@ -13,7 +13,7 @@ void Perception::AddSensors(Placement* things, unsigned int thingCount) {
sensorCount = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing;
if ((thing->type & Thing::SensorType) != 0)
if (thing->IsSensor())
sensorCount++;
}
@ -22,11 +22,10 @@ void Perception::AddSensors(Placement* things, unsigned int thingCount) {
unsigned int sensorIx = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing;
if ((thing->type & Thing::SensorType) != 0) {
if (thing->IsSensor())
sensorPlacements[sensorIx++] = things[thingIx];
}
}
}
unsigned int Perception::GetSensorCount() {
return this->sensorCount;
@ -37,7 +36,7 @@ Sensor* Perception::GetSensor(unsigned int sensorId) {
return nullptr;
Thing* thing = this->sensorPlacements[sensorId].thing;
if ((thing->type & Thing::SensorType) != 0)
if (thing->IsSensor())
return (Sensor*)thing;
return nullptr;
@ -48,7 +47,7 @@ float Perception::DistanceForward(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing;
if ((sensor->type & Thing::SensorType) != 0)
if (sensor->IsSensor())
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@ -67,12 +66,11 @@ float Perception::DistanceLeft(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing;
if ((sensor->type & Thing::SensorType) != 0)
if (sensor->IsSensor())
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
float sensorAngle = placement.horizontalDirection;
// Serial.printf(" distance sensor: %f %f 0\n", -angle, sensorAngle);
if (sensorAngle < 0 && sensorAngle > -angle) {
minDistance = fmin(minDistance, distanceSensor->GetDistance());
}
@ -102,7 +100,7 @@ float Perception::DistanceUp(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing;
if ((sensor->type & Thing::SensorType) != 0)
if (sensor->IsSensor())
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@ -119,7 +117,7 @@ float Perception::DistanceDown(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing;
if ((sensor->type & Thing::SensorType) != 0)
if (sensor->IsSensor())
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@ -143,11 +141,11 @@ bool Perception::SwitchOn(float fromAngle, float toAngle) {
if (thing == nullptr)
continue;
if ((thing->type & (int)Thing::Type::DistanceSensor) != 0) {
if (thing->type == Thing::DistanceSensorType) {
DistanceSensor* distanceSensor = (DistanceSensor*)thing;
if (distanceSensor != nullptr && distanceSensor->IsOn())
return true;
} else if ((thing->type & (int)Thing::Type::Switch) != 0) {
} else if (thing->type == Thing::SwitchType) {
Switch* switchSensor = (Switch*)thing;
if (switchSensor != nullptr && switchSensor->IsOn())
return true;

51
Placement.h
View File

@ -11,26 +11,57 @@ namespace Passer {
namespace RoboidControl {
/// @brief A plament is used to specify where a Thing is placed on the Roboid.
///
/// It is not always necessary to exactly specify the position and orientation
/// of a Thing. You can use a simple constructor in that case:
///
/// \code
/// Thing* thing = new Thing();
/// Placement p = Placement(thing);
/// \endcode
///
/// The thing can be placed using carthesian coordinates in meters, while the
/// orientation is specified with the horizontal and vertical direction. Whenö
/// both horizontal and vertical direction are zero, the Thing is aligned with
/// the parent thing in the hierarchy. When there is no parent, the thing is
/// directed such that it is looking forward.
///
/// \code
/// Thing* thing = new Thing();
/// Placement p = Placement(thing, Vector3(-0.04F, 0.0F, 0.06F), 0.0F, 0.0F);
/// \endcode
/// In the example above, the thing is placed 4 cm to the left and 6 cm to the
/// front relative to the parent. The orientation is the same as the parent. The
/// second line can be simplified, as the default angles are zero and a Vector2
/// position can be used which is a placement in the horizontal plane:
///
/// \code
/// Placement p = Placement(thing, Vector2(-0.04F, 0.06F));
/// \endcode
class Placement {
public:
/// @brief Default constructor with a zero placement
Placement();
/// @brief Placement of a Thing on the Roboid
/// @brief Placement of a Thing on a Roboid
/// @param thing The Thing which is placed
/// @param position The position of the Thing in carthesian coordinated
/// @param horizontalDirection The horizontal direction angle of the Thing
/// @param verticalAngle The vertical direction angle of the Thing
/// @param position The position of the Thing in carthesian coordinates
/// @param horizontalDirection The horizontal direction angle of the Thing.
/// Negative angles are to the left.
/// @param verticalAngle The vertical direction angle of the Thing. Negative
/// angles are downward.
Placement(Thing* thing,
Vector3 position = Vector3::zero,
float horizontalDirection = 0.0F,
float verticalAngle = 0.0F);
/// @brief Placement of a Thing on the roboit without position
/// @brief Placement of a Thing on a Roboid without position
/// @param thing The Thing which is place
/// @param horizontalDirection The horizontal direction angle of the Thing
/// @param verticalAngle The vertical direction angle of the Thing
/// @param horizontalDirection The horizontal direction angle of the Thing.
/// Negative angles are to the left.
/// @param verticalDirection The vertical direction angle of the Thing.
/// Negative angles are downward.
Placement(Thing* thing,
float horizontalDirection,
float verticalAnlge = 0.0F);
float verticalDirection = 0.0F);
/// @brief Default constructor with a zero placement
Placement();
/// @brief The parent placement in the Roboid hierarchy
/// @remark Reserved for future use

71
Propulsion.cpp
View File

@ -14,9 +14,9 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
this->motorCount = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing;
if ((thing->type & Thing::MotorType) != 0)
if (thing->IsMotor())
motorCount++;
if (thing->type == (int)Thing::Type::ControlledMotor)
if (thing->type == Thing::ControlledMotorType)
hasOdometer = true;
}
this->placement = new Placement[motorCount];
@ -24,15 +24,11 @@ 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;
if ((thing->type & Thing::MotorType) != 0)
if (thing->IsMotor())
this->placement[motorIx++] = things[thingIx];
}
}
// void Propulsion::AddQuadcopter(Quadcopter* quadcopter) {
// this->quadcopter = quadcopter;
// }
unsigned int Propulsion::GetMotorCount() {
return this->motorCount;
}
@ -42,7 +38,7 @@ Motor* Propulsion::GetMotor(unsigned int motorId) {
return nullptr;
Thing* thing = this->placement[motorId].thing;
if ((thing->type & Thing::MotorType) != 0)
if (thing->IsMotor())
return (Motor*)thing;
return nullptr;
@ -53,7 +49,7 @@ Placement* Propulsion::GetMotorPlacement(unsigned int motorId) {
return nullptr;
Placement* placement = &this->placement[motorId];
if ((placement->thing->type & Thing::MotorType) != 0)
if (placement->thing->IsMotor())
return placement;
return nullptr;
@ -74,45 +70,6 @@ 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;
// if (thing->type == Thing::UncontrolledMotorType) {
// Motor *motor = (Motor *)thing;
// if (motor == nullptr)
// continue;
// float xPosition = placement[motorIx].position.x;
// if (xPosition < 0)
// motor->SetSpeed(leftSpeed);
// else if (xPosition > 0)
// motor->SetSpeed(rightSpeed);
// } else if (thing->type == Thing::ControlledMotorType) {
// ControlledMotor *motor = (ControlledMotor *)placement[motorIx].thing;
// if (motor == nullptr)
// continue;
// float xPosition = placement[motorIx].position.x;
// if (xPosition < 0)
// motor->SetTargetSpeed(leftSpeed);
// else if (xPosition > 0)
// motor->SetTargetSpeed(rightSpeed);
// }
// };
// }
// void Propulsion::SetDiffDriveVelocities(float leftVelocity,
// float rightVelocity) {
// for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
// // Placement placement = placement[motorIx];
// // if (placement.position.x < 0)
// // placement.controlledMotor->SetTargetVelocity(leftVelocity);
// // else if (placement.position.x > 0)
// // placement.controlledMotor->SetTargetVelocity(rightVelocity);
// };
// }
void Propulsion::SetTwistSpeed(float forward, float yaw) {}
void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}
@ -120,23 +77,7 @@ void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}
void Propulsion::SetTwistSpeed(Vector3 linear,
float yaw,
float pitch,
float roll) {
// if (quadcopter != nullptr)
// quadcopter->SetTwistSpeed(linear, yaw);
// else
// SetTwistSpeed(linear.z, yaw);
}
// void Propulsion::SetLinearSpeed(Vector3 velocity,
// float yawSpeed,
// float rollSpeed) {
// if (quadcopter != nullptr)
// quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
// }
// Quadcopter* Propulsion::GetQuadcopter() {
// return quadcopter;
// }
float roll) {}
/// @brief Odometer returns the total distance traveled since start
/// @return The total distance

23
Thing.cpp Normal file
View File

@ -0,0 +1,23 @@
#include "Thing.h"
using namespace Passer::RoboidControl;
Thing::Thing() {
this->type = (unsigned int)Type::Undetermined;
}
const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch;
const unsigned int Thing::DistanceSensorType =
SensorType | (unsigned int)Type::DistanceSensor;
const unsigned int Thing::ControlledMotorType =
MotorType | (unsigned int)Type::ControlledMotor;
const unsigned int Thing::UncontrolledMotorType =
MotorType | (unsigned int)Type::UncontrolledMotor;
bool Thing::IsMotor() {
return (type & Thing::MotorType) != 0;
}
bool Thing::IsSensor() {
return (type & Thing::SensorType) != 0;
}

39
Thing.h
View File

@ -6,29 +6,40 @@ namespace RoboidControl {
/// @brief A thing is a functional component on a robot
class Thing {
public:
Thing() { type = (int)Type::Undetermined; }
/// @brief Default constructor for a Thing
Thing();
/// @brief The type of Thing
unsigned int type;
static const unsigned int SwitchType;
static const unsigned int DistanceSensorType;
static const unsigned int ControlledMotorType;
static const unsigned int UncontrolledMotorType;
/// @brief Check if the Thing is a Motor
/// @returns True when the Thing is a Motor and False otherwise
bool IsMotor();
/// @brief Check if the Thing is a Sensor
/// @returns True when the Thing is a Sensor and False otherwise
bool IsSensor();
protected:
/// @brief Bitmask for Motor type
static const unsigned int MotorType = 0x8000;
/// @brief Bitmap for Sensor type
static const unsigned int SensorType = 0x4000;
/// @brief Basic Thing types
enum class Type {
Undetermined,
/// Sensor,
// Sensor,
Switch,
DistanceSensor,
// Motor,
ControlledMotor,
UncontrolledMotor,
};
static const unsigned int MotorType = 0x8000;
static const unsigned int SensorType = 0x4000;
static const unsigned int SwitchType =
SensorType | (unsigned int)Type::Switch;
static const unsigned int DistanceSensorType =
SensorType | (unsigned int)Type::DistanceSensor;
static const unsigned int ControlledMotorType =
MotorType | (unsigned int)Type::ControlledMotor;
static const unsigned int UncontrolledMotorType =
MotorType | (unsigned int)Type::UncontrolledMotor;
unsigned int type = (int)Type::Undetermined;
};
} // namespace RoboidControl