I am self-learning robotics, C++ and good object oriented design. I have asked various questions over the past couple of weeks: 1 and 2 that have lead to the following design.
The design goal is for portability. It is a Go-To-Goal Differential Drive Robot with PID to regulate the behavior.
I am hoping my design is robust enough so that once I have more understanding of dynamic vehicle models, that I can simply plug those in without modifications ; the O in SOLID :)
please note I have not compiled this code yet, I am not looking for a syntax check, just rough design review
The state of the robot is represented as follow:
struct SensorConfig {
// required config values
};
class Sensor {
public:
virtual ~Sensor() = default;
// basic checks to ensure sensor is still
// functioning correctly
virtual bool IsActive() = 0;
//
virtual void Init(const SensorConfig& config) = 0;
virtual vector<double> const ReadMeasurement() = 0;
};
class State {
public:
virtual ~State() = default;
virtual void Update(const vector<double> values) = 0;
virtual void Update(const vector<Sensor>& sensors) = 0;
// overloading the operator so that I can measure
// the error between target state and current state
// each model can have its own way of deciding how to do that
virtual vector<double> operator- const (const State& state) = 0;
};
class UnicycleModel : public State {
public:
UnicyleModel(double velocity, double omega, double wheel_radius, double distance_between_wheels);
virtual void Update(vector<Sensor>& sensors)
{
if (sensors.size() < 2)
{
// FAIL with error message
}
Sensor r_wheel_odometry = sensors.at(0);
Sensor l_wheel_odometry = sensors.at(1);
double r_rotation_rate = r_wheel_odometry.ReadMeasurement().at(0);
double l_rotation_rate = r_wheel_odometry.ReadMeasurement().at(0);
// apply unicyle model equations to
// set velocity_ and omega_;
};
// just an example
virtual vector<double> operator- const (const State& state)
{
// example
return vector{state.omega - this->omega_, state.velocity - this->velocity};
}
private:
double velocity_;
double omega_;
double wheel_radius;
double distance_between_wheels_;
};
class LongitudinalDynamicsModel : public State {
public:
LongitudinalDynamicsModel(double velocity, double theta, double phi, // etc);
virtual void Update(vector<Sensor>& sensors)
{
// apply vehicle dynamics equations to
// set state;
};
virtual vector<double> operator- const (const State& state)
{
// example
return vector{state.velocity - this->velocity, //etc};
}
private:
// state representation
};
class LatDynamicsModel : public State {
public:
LatDynamicsModel(double velocity, double theta, double phi, // etc);
virtual void Update(vector<Sensor>& sensors)
{
// apply vehicle dynamics equations to
// set state;
};
virtual vector<double> operator- const (const State& state)
{
// example
return vector{state.velocity - this->velocity, //etc};
}
private:
// state representation
};
The go-to-goal behavior is represented as
class PID {
public:
virtual ~PID() = default;
virtual State const CalculateError(const State& target_state, const State& current_state) = 0;
virtual void SetProportionalGain(const double& kp) = 0;
virtual void SetIntegralGain(const double& ki) = 0;
virtual void SetDifferentialGain(const double& kd) = 0;
};
class Actuator {
public:
virtual ~Actuator() = default;
virtual bool Apply(const State& error) = 0;
};
class Motor {
pubic:
Motor(Pwm speed_pin, Ouput control_a, Output control_b);
inline bool Start();
inline bool Stop();
inline bool SpinForward();
inline bool SpinBackward();
};
class UniCycleDriver : public Actuator {
public:
UniCycleDriver(Motor& right_motor, Motor& left_motor);
virtual bool Apply(const State& error) {
};
inline bool Start();
inline bool Stop();
inline bool DriveForward();
inline bool DriveBackward();
inline const bool IsDrivnigForward();
private:
bool is_driving_forward_;
Motor right_motor_;
Motor left_motor_;
};
class FrontWheelDriver : public Actuator {
public:
virtual bool Apply(const State& error) {
};
};
Then finally, the main part of the system, where the control loops will take place
// this class should be instantiated in main along with calls to factories required to instantiate the sensors, states, etc..
class ControlAgent {
// set target state
// measure current state
// calculate error
// apply error
public:
ControlAgent(const vector<Sensor>& sensors,
PID& regulator,
Actuator& actuator);
void SetTargetState(const State& state);
void ClosedLoop()
{
State error = regulator.CalculateError(target_state_, current_state_);
while (target - error > some_threshold)
{
actuator.Apply(error);
current_state_.Update(sensor);
error = regulator.CalculateError(target_state_, current_state_);
}
};
private:
State current_state_;
State target_state_;
};
