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

Removed the need for placement

Browse Source
This commit is contained in:
Pascal Serrarens 2024-01-04 15:27:43 +01:00
parent c3cc48e723
commit 70ba3eae9c
12 changed files with 72 additions and 173 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
DifferentialDrive.cpp
View File

@ -5,27 +5,30 @@
DifferentialDrive::DifferentialDrive(){};
DifferentialDrive::DifferentialDrive(Placement leftMotorPlacement,
Placement rightMotorPlacement) {
DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor,
float separation) {
this->motorCount = 2;
this->placement = new Placement[2];
this->placement[0] = leftMotorPlacement;
this->placement[1] = rightMotorPlacement;
Motor *leftMotor = (Motor *)leftMotorPlacement.thing;
this->motors = new Motor *[2];
this->motors[0] = leftMotor;
this->motors[1] = rightMotor;
float distance = separation / 2;
leftMotor->direction = Motor::Direction::CounterClockwise;
Motor *rightMotor = (Motor *)rightMotorPlacement.thing;
leftMotor->position.angle = -90;
leftMotor->position.distance = distance;
rightMotor->direction = Motor::Direction::Clockwise;
rightMotor->position.angle = 90;
rightMotor->position.distance = distance;
}
void DifferentialDrive::SetTargetSpeeds(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;
Motor *motor = motors[motorIx];
if (motor == nullptr)
continue;
float xPosition = placement[motorIx].position.x;
if (motor->type == Thing::UncontrolledMotorType) {
float xPosition = motors[motorIx]->position.angle;
if (xPosition < 0)
motor->SetSpeed(leftSpeed);
else if (xPosition > 0)
@ -50,8 +53,8 @@ void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
}
Polar DifferentialDrive::GetVelocity() {
Motor *leftMotor = (Motor *)placement[0].thing;
Motor *rightMotor = (Motor *)placement[1].thing;
Motor *leftMotor = motors[0];
Motor *rightMotor = motors[1];
float leftSpeed = leftMotor->GetSpeed();
float rightSpeed = rightMotor->GetSpeed();
float speed = (leftSpeed + rightSpeed) / 2;
@ -62,8 +65,8 @@ Polar DifferentialDrive::GetVelocity() {
}
float DifferentialDrive::GetAngularVelocity() {
Motor *leftMotor = (Motor *)placement[0].thing;
Motor *rightMotor = (Motor *)placement[1].thing;
Motor *leftMotor = motors[0];
Motor *rightMotor = motors[1];
float leftSpeed = leftMotor->GetSpeed();
float rightSpeed = rightMotor->GetSpeed();
float angularVelocity = leftSpeed - rightSpeed;

6
DifferentialDrive.h
View File

@ -25,8 +25,10 @@ public:
/// driving forward, while the right motor will turn Clockwise.
/// @note When not using controlled motors, the placement of the motors is
/// irrelevant.
DifferentialDrive(Placement leftMotorPlacement,
Placement rightMotorPlacement);
// DifferentialDrive(Placement leftMotorPlacement,
// Placement rightMotorPlacement);
DifferentialDrive(Motor *leftMotor, Motor *rightMotor, float separation = 1);
/// @brief Set the target speeds of the motors directly
/// @param leftSpeed The target speed of the left Motor

47
Perception.cpp
View File

@ -7,14 +7,17 @@
#include <math.h>
Perception::Perception() {
this->trackedObjects = new TrackedObject *[maxObjectCount];
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
this->trackedObjects[objIx] = nullptr;
}
Perception::Perception(Placement *sensors, unsigned int sensorCount)
Perception::Perception(Sensor **sensors, unsigned int sensorCount)
: Perception() {
this->sensorCount = sensorCount;
this->sensorPlacements = (Placement *)sensors;
this->sensors = new Sensor *[this->sensorCount];
for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
this->sensors[sensorIx] = sensors[sensorIx];
}
unsigned int Perception::GetSensorCount() { return this->sensorCount; }
@ -23,16 +26,13 @@ Sensor *Perception::GetSensor(unsigned int sensorId) {
if (sensorId >= this->sensorCount)
return nullptr;
Thing *thing = this->sensorPlacements[sensorId].thing;
if (thing->IsSensor())
return (Sensor *)thing;
return nullptr;
Sensor *sensor = this->sensors[sensorId];
return sensor;
}
Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Sensor *sensor = (Sensor *)this->sensorPlacements[sensorIx].thing;
Sensor *sensor = this->sensors[sensorIx];
if (sensor->type == sensorType)
return sensor;
}
@ -108,6 +108,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
else {
if (obj->IsTheSameAs(this->trackedObjects[objIx])) {
this->trackedObjects[objIx]->Refresh(obj->position);
delete obj;
return;
}
// Is this the fartest object we see?
@ -128,9 +129,11 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
// If this object is closer than the farthest object, then replace it
else if (obj->position.distance <
this->trackedObjects[farthestObjIx]->position.distance) {
delete this->trackedObjects[farthestObjIx];
this->trackedObjects[farthestObjIx] = obj;
// we may want to destroy the fartest object, but if it is created
// externally, other links may still exist...
} else {
// No available slot, delete trackedobject
delete obj;
}
}
@ -154,23 +157,22 @@ void Perception::Update(float currentTimeMs) {
// Update sensing
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement thingPlacement = sensorPlacements[sensorIx];
Thing *thing = thingPlacement.thing;
if (thing == nullptr)
Sensor *sensor = sensors[sensorIx];
if (sensor == nullptr)
continue;
if (thing->type == Thing::DistanceSensorType) {
DistanceSensor *distanceSensor = (DistanceSensor *)thing;
if (sensor->type == Thing::DistanceSensorType) {
DistanceSensor *distanceSensor = (DistanceSensor *)sensor;
float distance = distanceSensor->GetDistance();
float angle = thingPlacement.horizontalDirection;
float angle = sensor->position.angle;
Polar position = Polar(angle, distance);
AddTrackedObject(distanceSensor, position);
} else if (thing->type == Thing::SwitchType) {
Switch *switchSensor = (Switch *)thing;
if (switchSensor != nullptr && switchSensor->IsOn()) {
Polar position =
Polar(thingPlacement.horizontalDirection, nearbyDistance);
} else if (sensor->type == Thing::SwitchType) {
Switch *switchSensor = (Switch *)sensor;
if (switchSensor->IsOn()) {
Polar position = Polar(sensor->position.angle, nearbyDistance);
AddTrackedObject(switchSensor, position);
}
}
@ -186,10 +188,9 @@ void Perception::Update(float currentTimeMs) {
if (roboid != nullptr && roboid->networkSync != nullptr)
roboid->networkSync->DestroyObject(obj);
this->trackedObjects[objIx] = nullptr;
delete obj;
}
}
if (this->trackedObjects[0] != nullptr) {
}
}
void Perception::UpdatePose(Polar translation) {

17
Perception.h
View File

@ -1,11 +1,12 @@
#pragma once
#include "Placement.h"
#include "Sensor.h"
#include "TrackedObject.h"
#include "VectorAlgebra/Polar.h"
#include "VectorAlgebra/Quaternion.h"
// #include <vector.h>
namespace Passer {
namespace RoboidControl {
@ -20,7 +21,7 @@ public:
/// @brief Create a perception setup with the given Sensors
/// @param sensors The Placement of Sensors on the Roboid
/// @param sensorCount The number of sensors in the placement array
Perception(Placement *sensors, unsigned int sensorCount);
Perception(Sensor **sensors, unsigned int sensorCount);
/// @brief The roboid of this perception system
Roboid *roboid = nullptr;
@ -122,17 +123,17 @@ public:
float nearbyDistance = 0.3F;
public:
/// @brief The Placement of the Sensors used for Perception
Placement *sensorPlacements = nullptr;
/// @brief The Sensors used for Perception
Sensor **sensors = nullptr;
/// @brief The number of Sensors used for Perception
unsigned int sensorCount = 0;
float lastUpdateTimeMs = 0;
static const unsigned char maxObjectCount = 7;
TrackedObject *trackedObjects[maxObjectCount]; // 7 is typically the maximum
// number of object which can
// be tracked by a human
unsigned char maxObjectCount = 7; // 7 is typically the maximum
// number of object which can
// be tracked by a human
TrackedObject **trackedObjects;
};
} // namespace RoboidControl

18
Placement.cpp
View File

@ -1,18 +0,0 @@
#include "Placement.h"
Placement::Placement() {
this->position = Vector3::zero;
this->thing = nullptr;
}
Placement::Placement(Thing *thing, Vector3 position, float horizontalDirection,
float verticalDirection)
: horizontalDirection(horizontalDirection),
verticalDirection(verticalDirection) {
this->thing = thing;
this->position = position;
}
Placement::Placement(Thing *thing, float horizontalDirection,
float verticalDirection)
: Placement(thing, Vector3::zero, horizontalDirection, verticalDirection) {}

82
Placement.h
View File

@ -1,82 +0,0 @@
#pragma once
#include "Thing.h"
#include "VectorAlgebra/Vector2.h"
#include "VectorAlgebra/Vector3.h"
namespace Passer {
namespace RoboidControl {
/// @brief A plament is used to specify where a Thing is placed on the Roboid.
/// @details
/// 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 Placement of a Thing on a Roboid
/// @param thing The Thing which is placed
/// @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 a Roboid without position
/// @param thing The Thing which is place
/// @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 verticalDirection = 0.0F);
/// @brief Default constructor with a zero placement
Placement();
/// @brief The parent placement in the Roboid hierarchy
/// @remark Reserved for future use
Placement *parent = nullptr;
/// @brief An array of children of this placement in the Roboid hierarchy
/// @remark Reserved for future use
Placement **children = nullptr;
/// @brief The number of children of this placemet in the Roboid hierarchy
/// @remark Reserved for future use
unsigned int childCount = 0;
/// @brief The Thing which is placed
Thing *thing;
/// @brief The position of the Thing in carthesian coordinates
Vector3 position;
/// @brief The angle or direction of the Thing in the horizontal plane
float horizontalDirection;
/// @brief The angle or direction of the Thing in the vertical plane
float verticalDirection;
};
} // namespace RoboidControl
} // namespace Passer
using namespace Passer::RoboidControl;

20
Propulsion.cpp
View File

@ -4,7 +4,7 @@
#include "VectorAlgebra/FloatSingle.h"
Propulsion::Propulsion() {
this->placement = nullptr;
this->motors == nullptr;
this->motorCount = 0;
}
@ -14,22 +14,8 @@ Motor *Propulsion::GetMotor(unsigned int motorId) {
if (motorId >= this->motorCount)
return nullptr;
Thing *thing = this->placement[motorId].thing;
if (thing->IsMotor())
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->IsMotor())
return placement;
return nullptr;
Motor *motor = this->motors[motorId];
return motor;
}
void Propulsion::Update(float currentTimeMs) {}

6
Propulsion.h
View File

@ -1,7 +1,6 @@
#pragma once
#include "Motor.h"
#include "Placement.h"
#include "VectorAlgebra/Polar.h"
#include "VectorAlgebra/Quaternion.h"
#include "VectorAlgebra/Vector2.h"
@ -37,7 +36,7 @@ public:
/// @param motorIx The index of the Motor
/// @return Returns the Placement or a nullptr when no Placement with the give
/// index could be found
Placement *GetMotorPlacement(unsigned int motorIx);
// Placement *GetMotorPlacement(unsigned int motorIx);
/// @brief Sets the forward and rotation speed of a (grounded) Roboid
/// @param forward The target forward speed
@ -76,7 +75,8 @@ protected:
/// @brief The number of motors used for Propulsion
unsigned int motorCount = 0;
/// @brief The Placement of the motors used for Propulsion
Placement *placement = nullptr;
// Placement *placement = nullptr;
Motor **motors = nullptr;
};
} // namespace RoboidControl

2
Switch.cpp
View File

@ -1,5 +1,5 @@
#include "Switch.h"
Switch::Switch() {}
Switch::Switch() { this->type = Thing::SwitchType; }
bool Switch::IsOn() { return false; }

4
Thing.cpp
View File

@ -2,7 +2,9 @@
using namespace Passer::RoboidControl;
Thing::Thing() { this->type = (unsigned int)Type::Undetermined; }
Thing::Thing() : position(Polar::zero) {
this->type = (unsigned int)Type::Undetermined;
}
const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch;
const unsigned int Thing::DistanceSensorType =

4
Thing.h
View File

@ -1,5 +1,7 @@
#pragma once
#include "VectorAlgebra/Polar.h"
namespace Passer {
namespace RoboidControl {
@ -30,6 +32,8 @@ public:
/// @returns True when the Thing is a Sensor and False otherwise
bool IsSensor();
Polar position;
protected:
/// @brief Bitmask for Motor type
static const unsigned int MotorType = 0x8000;

2
TrackedObject.h
View File

@ -62,7 +62,7 @@ public:
protected:
static constexpr unsigned char maxConfidence = 255;
static constexpr unsigned char confidenceDropSpeed = 2;
static constexpr unsigned char confidenceDropSpeed = 1; // 2;
unsigned char confidence;
};