Reverse merge main

This commit is contained in:
Pascal Serrarens 2023-12-06 13:04:58 +01:00
commit 938bd7ceda
19 changed files with 144 additions and 393 deletions

2
.gitmodules vendored
View File

@ -1,3 +1,3 @@
[submodule "VectorAlgebra"]
path = VectorAlgebra
url = http://gitlab.passervr.com/passer/cpp/vectoralgebra.git
url = ../vectoralgebra.git

View File

@ -6,6 +6,7 @@
namespace Passer {
namespace RoboidControl {
/// @brief Activation function for control
class Activation {
public:
static float HeavisideStep(float inputValue, float bias = 0); // Range: {0,1}

View File

@ -38,3 +38,14 @@ void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
SetTargetSpeeds(leftSpeed, rightSpeed);
}
void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
SetTwistSpeed(linear.y, yaw);
}
void DifferentialDrive::SetTwistSpeed(Vector3 linear,
float yaw,
float pitch,
float roll) {
SetTwistSpeed(linear.z, yaw);
}

View File

@ -5,15 +5,57 @@
namespace Passer {
namespace RoboidControl {
/// @brief A two-wheeled Propulsion method
///
/// The wheels are put at either side of the roboid with the following behaviour
/// * When both wheels spin forward, the Roboid moves forward
/// * When both wheels spin backward, the Roboid moves backward
/// * When both wheels are spinning in opposite directions, the Roboid rotates
/// wihout moving forward or backward
/// * When just one wheel is spinning, the Roboid turnes while moving forward or
/// backward.
class DifferentialDrive : public Propulsion {
public:
/// @brief Default constructor
DifferentialDrive();
/// @brief Setup of the DifferentialDrive with the Placement of the motors
/// @param leftMotorPlacement Placement of the left Motor
/// @param rightMotorPlacement Placement of the right Motor
/// In this setup, the left motor Direction will be CounterClockWise when
/// 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);
/// @brief Set the target speeds of the motors directly
/// @param leftSpeed The target speed of the left Motor
/// @param rightSpeed The target speed of the right Motor
void SetTargetSpeeds(float leftSpeed, float rightSpeed);
/// @brief Controls the motors through forward and rotation speeds
/// @param forward The target forward speed of the Roboid
/// @param yaw The target rotation speed of the Roboid
virtual void SetTwistSpeed(float forward, float yaw) override;
// virtual void SetTwistSpeed(float forward, float yaw, float pitch) override;
/// @brief Controls the motors through forward and rotation speeds
/// @param linear The target linear speed of the Roboid
/// @param yaw The target rotation speed of the Roboid
/// @note As a DifferentialDrive cannot move sideward, this function has the
/// same effect as using the void SetTwistSpeed(float forward, float yaw)
/// function.
virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F);
/// @brief Controls the motors through forward and rotation speeds
/// @param linear The target linear speed
/// @param yaw The target rotation speed around the vertical axis
/// @param pitch Pitch is not supported and is ignored
/// @param roll Roll is not supported and is ignores
/// @note As a DifferentialDrive cannot move sideward or vertical, this
/// function has the same effect as using the void SetTwistSpeed(float
/// forward, float yaw) function.
virtual void SetTwistSpeed(Vector3 linear,
float yaw = 0.0F,
float pitch = 0.0F,
float roll = 0.0F);
};
} // namespace RoboidControl

View File

@ -12,16 +12,7 @@ float DistanceSensor::GetDistance() {
return distance;
};
void DistanceSensor::SetDistance(float distance) {
this->distance = distance;
}; // for simulation purposes
bool DistanceSensor::IsOn() {
bool DistanceSensor::ObjectNearby() {
bool isOn = GetDistance() <= triggerDistance;
return isOn;
}
bool DistanceSensor::isOff() {
bool isOff = GetDistance() > triggerDistance;
return isOff;
}

View File

@ -8,21 +8,26 @@ namespace RoboidControl {
/// @brief A Sensor which can measure the distance to the nearest object
class DistanceSensor : public Sensor {
public:
/// @brief Default constructor
DistanceSensor();
/// @brief Creates a DistanceSensor with the given trigger distance
/// @param triggerDistance The distance at which the sensors indicates that a
/// object is close by
DistanceSensor(float triggerDistance);
/// @brief Determine the distance to the nearest object
/// @return the measured distance in meters to the nearest object
virtual float GetDistance();
void SetDistance(float distance);
/// @brief The distance at which ObjectNearby triggers
float triggerDistance = 1;
bool IsOn();
bool isOff();
/// @brief Indicate that an object is nearby
/// @return True when an object is nearby
bool ObjectNearby();
protected:
/// @brief Distance to the closest object
float distance = 0;
};

View File

@ -1,28 +0,0 @@
#include "Encoder.h"
Encoder::Encoder(unsigned char transitionsPerRotation,
unsigned char distancePerRotation) {
//: Encoder::Encoder() {
this->transitionsPerRevolution = transitionsPerRotation;
this->distancePerRevolution = distancePerRotation;
}
int Encoder::GetPulseCount() {
return 0;
}
float Encoder::GetDistance() {
return 0;
}
float Encoder::GetPulsesPerSecond(float currentTimeMs) {
return 0;
}
float Encoder::GetRevolutionsPerSecond(float currentTimeMs) {
return 0;
}
float Encoder::GetSpeed(float currentTimeMs) {
return 0;
}

View File

@ -1,51 +0,0 @@
#pragma once
namespace Passer {
namespace RoboidControl {
/// @brief An Encoder measures the rotations of an axle using a rotary sensor
/// Some encoders are able to detect direction, while others can not.
class Encoder {
public:
/// @brief Creates a sensor which measures distance from pulses
/// @param transitionsPerRevolution The number of pulse edges which make a
/// full rotation
/// @param distancePerRevolution The distance a wheel travels per full
/// rotation
Encoder(unsigned char transitionsPerRevolution = 1,
unsigned char distancePerRevolution = 1);
/// @brief Get the total number of pulses since the previous call
/// @return The number of pulses, is zero or greater
virtual int GetPulseCount();
/// @brief Get the pulse speed since the previous call
/// @param currentTimeMs The clock time in milliseconds
/// @return The average pulses per second in the last period.
virtual float GetPulsesPerSecond(float currentTimeMs);
/// @brief Get the distance traveled since the previous call
/// @return The distance in meters.
virtual float GetDistance();
/// @brief Get the rotation speed since the previous call
/// @param currentTimeMs The clock time in milliseconds
/// @return The speed in rotations per second
virtual float GetRevolutionsPerSecond(float currentTimeMs);
/// @brief Get the speed since the previous call
/// @param currentTimeMs The clock time in milliseconds
/// @return The speed in meters per second.
/// @note this value is dependent on the accurate setting of the
/// transitionsPerRevolution and distancePerRevolution parameters;
virtual float GetSpeed(float currentTimeMs);
/// @brief The numer of pulses corresponding to a full rotation of the axle
unsigned char transitionsPerRevolution = 1;
/// @brief The number of revolutions which makes the wheel move forward 1
/// meter
unsigned char distancePerRevolution = 1;
};
} // namespace RoboidControl
} // namespace Passer
using namespace Passer::RoboidControl;

View File

@ -12,19 +12,3 @@ float Motor::GetSpeed() {
void Motor::SetSpeed(float targetSpeed) {
this->currentTargetSpeed = targetSpeed;
}
bool Motor::Drive(float distance) {
if (!this->driving) {
this->startTime = time(NULL);
this->targetDistance = distance >= 0 ? distance : -distance;
this->driving = true;
}
double duration = difftime(time(NULL), this->startTime);
if (duration >= this->targetDistance) {
this->driving = false;
return true;
}
SetSpeed(distance < 0 ? -1 : 1); // max speed
return false;
}

View File

@ -27,13 +27,8 @@ class Motor : public Thing {
/// forward)
virtual float GetSpeed();
bool Drive(float distance);
protected:
float currentTargetSpeed = 0;
bool driving = false;
float targetDistance = 0;
time_t startTime = 0;
};
} // namespace RoboidControl

View File

@ -12,24 +12,6 @@ Perception::Perception(Placement* sensors, unsigned int sensorCount) {
this->sensorPlacements = (Placement*)sensors;
}
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->IsSensor())
sensorCount++;
}
sensorPlacements = new Placement[sensorCount];
unsigned int sensorIx = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing;
if (thing->IsSensor())
sensorPlacements[sensorIx++] = things[thingIx];
}
}
unsigned int Perception::GetSensorCount() {
return this->sensorCount;
}
@ -187,7 +169,7 @@ float Perception::GetDistance(float fromHorizontalAngle,
return minDistance;
}
bool Perception::SwitchOn(float fromAngle, float toAngle) {
bool Perception::ObjectNearby(float fromAngle, float toAngle) {
if (toAngle < fromAngle)
return false;
@ -201,7 +183,7 @@ bool Perception::SwitchOn(float fromAngle, float toAngle) {
if (thing->type == Thing::DistanceSensorType) {
DistanceSensor* distanceSensor = (DistanceSensor*)thing;
if (distanceSensor != nullptr && distanceSensor->IsOn())
if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
return true;
} else if (thing->type == Thing::SwitchType) {
Switch* switchSensor = (Switch*)thing;
@ -213,66 +195,3 @@ bool Perception::SwitchOn(float fromAngle, float toAngle) {
return false;
}
unsigned int Perception::ToDepthMapIndex(float angle) {
unsigned int depthMapIx =
(unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) *
(float)resolution);
return depthMapIx;
}
// float Perception::GetDistance(float angle) {
// if (depthMap != nullptr) {
// if (angle < rangeMinimum || angle > rangeMaximum)
// return INFINITY;
// unsigned int depthMapIx = ToDepthMapIndex(angle);
// return depthMap[depthMapIx];
// } else {
// for (unsigned int sensorIx = 0; sensorIx < this->sensorCount;
// sensorIx++)
// {
// Placement placement = sensorPlacements[sensorIx];
// float placementAngle = placement.horizontalDirection;
// if (placementAngle == angle) {
// DistanceSensor* distanceSensor =
// (DistanceSensor*)placement.thing; return
// distanceSensor->GetDistance();
// }
// }
// }
// return INFINITY;
// }
void Perception::SetResolution(unsigned int resolution) {
this->resolution = resolution;
this->depthMap = new float[this->resolution];
}
void Perception::SetRange(float min, float max) {
this->rangeMinimum = min;
this->rangeMaximum = max;
}
float* Perception::GetDepthMap() {
return this->depthMap;
}
void Perception::SetDepthMap(float angle, float distance) {
if (angle < rangeMinimum || angle > rangeMaximum)
return;
unsigned int depthMapIx = ToDepthMapIndex(angle);
depthMap[depthMapIx] = distance;
}
Sensor* Perception::GetSensor(float angle) {
angle = Angle::Normalize(angle);
for (unsigned int ix = 0; ix < this->sensorCount; ix++) {
Placement placement = this->sensorPlacements[ix];
float delta = placement.horizontalDirection - angle;
if (delta > -0.01F && delta < 0.01F)
return (Sensor*)placement.thing;
}
return nullptr;
}

View File

@ -6,9 +6,10 @@
namespace Passer {
namespace RoboidControl {
/// @brief Module to which keeps track of objects around the roboid
class Perception {
public:
/// @brief Setup perception
/// @brief Default Constructor
Perception();
/// @brief Template to make it possible to leave out ths sensorCount
@ -18,51 +19,16 @@ class Perception {
inline Perception(Placement (&sensors)[sensorCount]) {
Perception(sensors, sensorCount);
}
/// @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);
void AddSensors(Placement* sensors, unsigned int sensorCount);
/// @brief Get the number of Sensors
/// @return The number of sensors, zero when no sensors are present
unsigned int GetSensorCount();
Sensor* GetSensor(unsigned int sensorId);
/*
float DistanceForward(float angle = 90);
/// @brief Distance to the closest object on the left
/// @return distance in meters, INFINITY when no object is detected.
/// @note An object is on the left when the `angle` is between -180 and 0
/// degrees.
/// @note An object dead straight (0 degrees) is not reported.
float DistanceLeft() { return DistanceLeft(180); }
/// @brief Distance to the closest object on the left
/// @param angle the maximum angle on the left used for detection.
/// @return distance in meters, INFINITY when no object is detected.
/// @note An object is on the left when the `angle` is between -`angle` and
0
/// degrees.
/// @note An object dead straight (0 degrees) is not reported.
/// @note When an object is beyond `angle` meters, it is not reported.
float DistanceLeft(float angle);
/// @brief Distance to the closest object on the right
/// @return distance in meters, INFINITY when no object is detected
/// @note An object is on the right when the `angle` is between 0 and 180
/// degrees
/// @note An object dead straight (0 degrees) is not reported
float DistanceRight() { return DistanceRight(180); }
/// @brief Distance to the closest object on the right
/// @param angle the maximum angle on the left used for detection.
/// @return distance in meters, INFINITY when no object is detected
/// @note An object is on the left when the `angle` is between 0 and `angle`
/// degrees.
/// @note An object dead straight (0 degrees) is not reported.
/// @note When an object is beyond `angle` meters, it is not reported.
float DistanceRight(float angle);
float DistanceUp() { return DistanceUp(180); }
float DistanceUp(float angle);
float DistanceDown() { return DistanceDown(180); }
float DistanceDown(float angle);
*/
float GetDistance(float fromAngle, float toAngle);
float GetDistance(float fromHorizontalAngle,
float toHorizontalAngle,
@ -70,27 +36,22 @@ class Perception {
float toVerticalAngle);
// float GetDistance(float angle);
bool SwitchOn(float fromAngle, float toAngle);
void SetResolution(unsigned int resolution);
void SetRange(float min, float max);
float* GetDepthMap();
unsigned int ToDepthMapIndex(float angle);
void SetDepthMap(float angle, float distance);
Sensor* GetSensor(float angle);
/// @brief Checks if an object is close within the give range in the
/// horizontal plane
/// @param fromAngle Start angle in the horizontal plane
/// @param toAngle End angle in the horizontal plane
/// @return True is an object is closeby
/// @note Whether an object is closeby depends on the Distance Sensor
/// @remark This function is likely to change in the near future
bool ObjectNearby(float fromAngle, float toAngle);
protected:
// SensorPlacement* sensors = nullptr;
/// @brief The Placement of the Sensors used for Perception
Placement* sensorPlacements = nullptr;
/// @brief The number of Sensors used for Perception
unsigned int sensorCount = 0;
unsigned int resolution;
float rangeMinimum;
float rangeMaximum;
float* depthMap = nullptr;
};
} // namespace RoboidControl
} // namespace Passer
using namespace Passer::RoboidControl;
using namespace Passer::RoboidControl;

View File

@ -7,23 +7,6 @@ Propulsion::Propulsion() {
this->motorCount = 0;
}
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->IsMotor())
motorCount++;
}
this->placement = new Placement[motorCount];
unsigned int motorIx = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing;
if (thing->IsMotor())
this->placement[motorIx++] = things[thingIx];
}
}
unsigned int Propulsion::GetMotorCount() {
return this->motorCount;
}
@ -50,15 +33,8 @@ Placement* Propulsion::GetMotorPlacement(unsigned int motorId) {
return nullptr;
}
void Propulsion::Update(float currentTimeMs) {
this->lastUpdateTime = currentTimeMs;
}
void Propulsion::SetMaxSpeed(float maxSpeed) {
if (maxSpeed < 0)
maxSpeed = 0;
this->maxSpeed = maxSpeed;
}
void Propulsion::Update(float currentTimeMs) {}
void Propulsion::SetTwistSpeed(float forward, float yaw) {}

View File

@ -7,47 +7,65 @@
namespace Passer {
namespace RoboidControl {
/// @brief The Propulsion module for a Roboid is used to move the Roboid in
/// space
///
/// Usually, a specific implementation of the propulsion module is used for a
/// robot. This base class does not implement the functions to move the Roboid
/// around.
class Propulsion {
public:
/// @brief Setup sensing
/// @brief Default Constructor for Propulsion
Propulsion();
/// @brief Update the propulsion state of the Roboid
/// @param currentTimeMs The time in milliseconds when calling this
void Update(float currentTimeMs);
void AddMotors(Placement* motors, unsigned int motorCount);
// void AddQuadcopter(Quadcopter* quadcopter);
// Quadcopter* GetQuadcopter();
/// @brief Get the number of motors in this roboid
/// @return The number of motors. Zero when no motors are present
unsigned int GetMotorCount();
/// @brief Get a specific motor
/// @param motorIx The index of the motor
/// @return Returns the motor or a nullptr when no motor with the given index
/// could be found
Motor* GetMotor(unsigned int motorIx);
/// @brief Get the Placement of a specific Motor
/// @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);
/// @brief Set the maximum linear speed.
/// @param maxSpeed The new maximum linear speed
/// For controlled motors, this is rpm.
/// For calibrated controlled motors, this is m/s
/// For uncontrolled motors this is a value between 0 and 1 where 1 is the
/// maximum speed of the motor
void SetMaxSpeed(float maxSpeed);
// Velocity control
virtual void SetTwistSpeed(float forward, float yaw = 0.0F); // 2D
virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F); // 2D plus
/// @brief Sets the forward and rotation speed of a (grounded) Roboid
/// @param forward The target forward speed
/// @param yaw The target rotation speed around the vertical axis
/// This function is typically used for Roboid which are driving on the
/// ground.
virtual void SetTwistSpeed(float forward, float yaw = 0.0F);
/// @brief Sets the forward, sideward and rotation speed of a (grounded)
/// Roboid
/// @param linear The target linear (forward, sideward) speed
/// @param yaw The target rotation speed around the vertical axis
/// This function is typically used for Roboid which are driving on the ground
/// which have to ability to move sideward
virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F);
/// @brief Set the target 3D linear and 3D rotation speed of a (flying) Roboid
/// @param linear The target linear speed
/// @param yaw The target rotation speed around the vertical axis
/// @param pitch The target rotation speed around the sideward axis
/// @param roll The target rotation speed around hte forward axis
virtual void SetTwistSpeed(Vector3 linear,
float yaw = 0.0F,
float pitch = 0.0F,
float roll = 0.0F); // 3D
Placement* placement = nullptr;
unsigned int motorCount = 0;
float roll = 0.0F);
protected:
float maxSpeed = 1;
bool driving = false;
float targetDistance = 0;
time_t startTime;
float lastUpdateTime;
/// @brief The number of motors used for Propulsion
unsigned int motorCount = 0;
/// @brief The Placement of the motors used for Propulsion
Placement* placement = nullptr;
};
} // namespace RoboidControl

View File

@ -1,38 +0,0 @@
#include "Quadcopter.h"
Quadcopter::Quadcopter() {}
void Quadcopter::SetTwistSpeed(float forward, float yaw) {
this->velocity = Vector3::forward * forward;
this->yawSpeed = yaw;
}
void Quadcopter::SetTwistSpeed(Vector2 linear, float yaw) {
this->velocity = Vector3(linear.x, 0.0F, linear.y);
this->yawSpeed = yaw;
}
void Quadcopter::SetTwistSpeed(Vector3 velocity,
float yaw,
float pitch,
float roll) {
this->velocity = velocity;
this->yawSpeed = yaw;
this->rollSpeed = roll;
this->pitchSpeed = pitch;
}
Vector3 Quadcopter::GetTargetVelocity() {
return this->velocity;
}
float Quadcopter::GetPitchSpeed() {
return this->pitchSpeed;
}
float Quadcopter::GetYawSpeed() {
return this->yawSpeed;
}
float Quadcopter::GetRollSpeed() {
return this->rollSpeed;
}

View File

@ -1,37 +0,0 @@
#pragma once
#include "Propulsion.h"
#include "Thing.h"
#include "Vector3.h"
namespace Passer {
namespace RoboidControl {
/// @brief Support for Quadcopter as a propulsion method
class Quadcopter : public Propulsion {
public:
/// @brief Default constuctor
Quadcopter();
virtual void SetTwistSpeed(float forward, float yaw = 0.0F) override;
virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F) override;
virtual void SetTwistSpeed(Vector3 linear,
float yaw = 0.0F,
float pitch = 0.0F,
float roll = 0.0F) override;
Vector3 GetTargetVelocity();
float GetYawSpeed();
float GetPitchSpeed();
float GetRollSpeed();
protected:
Vector3 velocity = Vector3::zero;
float pitchSpeed = 0.0F;
float yawSpeed = 0.0F;
float rollSpeed = 0.0F;
};
} // namespace RoboidControl
} // namespace Passer
using namespace Passer::RoboidControl;

View File

@ -6,16 +6,25 @@
namespace Passer {
namespace RoboidControl {
/// @brief A Roboid is used to control autonomous robots
class Roboid {
public:
/// @brief Default constructor for a Roboid
Roboid();
Roboid(Placement configuration[], unsigned int thingCount);
/// @brief Creates a Roboid with Perception and Propulsion abilities
/// @param perception The Perception implementation to use for this Roboid
/// @param propulsion The Propulsion implementation to use for this Roboid
Roboid(Perception* perception, Propulsion* propulsion);
Perception* perception;
Propulsion* propulsion;
/// @brief Update the state of the Roboid
/// @param currentTimeMs The time in milliseconds when calling this
/// function
void Update(float currentTimeMs);
/// @brief The Perception module of this Roboid
Perception* perception;
/// @brief The Propulsion module of this Roboid
Propulsion* propulsion;
};
} // namespace RoboidControl
} // namespace Passer

View File

@ -8,6 +8,7 @@ namespace RoboidControl {
/// @brief A sensor is a thing which can perform measurements in the environment
class Sensor : public Thing {
public:
/// @brief Default Constructor for a Sensor
Sensor();
};

View File

@ -1,8 +0,0 @@
#pragma once
class Servo {
public:
Servo();
void SetTargetAngle(float angle) = 0;
};