Skip to content

Commit 19af7fb

Browse files
gabrielfrasantosgithub-actions[bot]
gabrielfrasantos
and
github-actions[bot]
authored
feat: improve controllers (#244)
* improve pid incremental * improve lqg and lqr * improve controllers * improve other controllers * Fix sonar score * remove interface from coverage analysis * Apply suggestions from code review Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> * fix windows build --------- Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
1 parent 9f8844a commit 19af7fb

17 files changed

Lines changed: 1173 additions & 840 deletions

‎.claude/agents/modernizer.md‎

Lines changed: 0 additions & 71 deletions
This file was deleted.

‎.github/agents/modernizer.agent.md‎

Lines changed: 0 additions & 74 deletions
This file was deleted.

‎numerical/controllers/implementations/PidIncremental.hpp‎

Lines changed: 24 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -14,11 +14,9 @@ namespace controllers
1414
class PidIncrementalBase
1515
{
1616
public:
17+
void Reset();
1718
void SetPoint(QNumberType setPoint);
18-
void SetLimits(PidLimits<QNumberType> limits);
1919
void SetTunings(PidTunings<QNumberType> tunnings);
20-
void Enable();
21-
void Disable();
2220
QNumberType Process(QNumberType processVariable);
2321

2422
protected:
@@ -50,13 +48,15 @@ namespace controllers
5048
{
5149
public:
5250
PidIncrementalAsynchronous(PidDriver<QNumberType>& driver, std::chrono::system_clock::duration sampleTime, PidTunings<QNumberType> tunnings, PidLimits<QNumberType> limits);
51+
~PidIncrementalAsynchronous();
52+
PidIncrementalAsynchronous(const PidIncrementalAsynchronous&) = delete;
53+
PidIncrementalAsynchronous& operator=(const PidIncrementalAsynchronous&) = delete;
54+
PidIncrementalAsynchronous(PidIncrementalAsynchronous&&) = delete;
55+
PidIncrementalAsynchronous& operator=(PidIncrementalAsynchronous&&) = delete;
5356

54-
// Implementation of AsynchronousPidController
57+
void Reset() override;
5558
void SetPoint(QNumberType setPoint) override;
56-
void SetLimits(PidLimits<QNumberType> limits) override;
5759
void SetTunings(PidTunings<QNumberType> tunnings) override;
58-
void Enable() override;
59-
void Disable() override;
6060

6161
private:
6262
PidDriver<QNumberType>& driver;
@@ -71,12 +71,9 @@ namespace controllers
7171
public:
7272
PidIncrementalSynchronous(PidTunings<QNumberType> tunnings, PidLimits<QNumberType> limits);
7373

74-
// Implementation of SynchronousPidController
74+
void Reset() override;
7575
void SetPoint(QNumberType setPoint) override;
76-
void SetLimits(PidLimits<QNumberType> limits) override;
7776
void SetTunings(PidTunings<QNumberType> tunnings) override;
78-
void Enable() override;
79-
void Disable() override;
8077
QNumberType Process(QNumberType processVariable) override;
8178
};
8279

@@ -93,35 +90,20 @@ namespace controllers
9390
}
9491

9592
template<class QNumberType>
96-
void PidIncrementalBase<QNumberType>::SetPoint(QNumberType _setPoint)
97-
{
98-
this->setPointValue = _setPoint;
99-
this->hasSetPoint = true;
100-
}
101-
102-
template<class QNumberType>
103-
void PidIncrementalBase<QNumberType>::Enable()
93+
void PidIncrementalBase<QNumberType>::Reset()
10494
{
10595
u = QNumberType(0.0f);
10696
u_1 = QNumberType(0.0f);
107-
10897
e = QNumberType(0.0f);
10998
e_1 = QNumberType(0.0f);
11099
e_2 = QNumberType(0.0f);
111100
}
112101

113102
template<class QNumberType>
114-
void PidIncrementalBase<QNumberType>::Disable()
115-
{
116-
hasSetPoint = false;
117-
}
118-
119-
template<class QNumberType>
120-
void PidIncrementalBase<QNumberType>::SetLimits(PidLimits<QNumberType> _limits)
103+
void PidIncrementalBase<QNumberType>::SetPoint(QNumberType _setPoint)
121104
{
122-
really_assert(_limits.max > _limits.min);
123-
124-
this->limits = _limits;
105+
this->setPointValue = _setPoint;
106+
this->hasSetPoint = true;
125107
}
126108

127109
template<class QNumberType>
@@ -173,31 +155,25 @@ namespace controllers
173155
{
174156
this->driver.ControlAction(this->PidIncrementalBase<QNumberType>::Process(processVariable));
175157
});
158+
driver.Start(sampleTime);
176159
}
177160

178-
template<class QNumberType>
179-
void PidIncrementalAsynchronous<QNumberType>::SetPoint(QNumberType _setPoint)
161+
template<typename QNumberType>
162+
PidIncrementalAsynchronous<QNumberType>::~PidIncrementalAsynchronous()
180163
{
181-
PidIncrementalBase<QNumberType>::SetPoint(_setPoint);
164+
driver.Stop();
182165
}
183166

184167
template<class QNumberType>
185-
void PidIncrementalAsynchronous<QNumberType>::Enable()
168+
void PidIncrementalAsynchronous<QNumberType>::Reset()
186169
{
187-
PidIncrementalBase<QNumberType>::Enable();
188-
driver.Start(sampleTime);
189-
}
190-
191-
template<typename QNumberType>
192-
void PidIncrementalAsynchronous<QNumberType>::Disable()
193-
{
194-
driver.Stop();
170+
PidIncrementalBase<QNumberType>::Reset();
195171
}
196172

197173
template<class QNumberType>
198-
void PidIncrementalAsynchronous<QNumberType>::SetLimits(PidLimits<QNumberType> limits)
174+
void PidIncrementalAsynchronous<QNumberType>::SetPoint(QNumberType _setPoint)
199175
{
200-
PidIncrementalBase<QNumberType>::SetLimits(limits);
176+
PidIncrementalBase<QNumberType>::SetPoint(_setPoint);
201177
}
202178

203179
template<class QNumberType>
@@ -213,15 +189,15 @@ namespace controllers
213189
}
214190

215191
template<class QNumberType>
216-
void PidIncrementalSynchronous<QNumberType>::SetPoint(QNumberType setPoint)
192+
void PidIncrementalSynchronous<QNumberType>::Reset()
217193
{
218-
PidIncrementalBase<QNumberType>::SetPoint(setPoint);
194+
PidIncrementalBase<QNumberType>::Reset();
219195
}
220196

221197
template<class QNumberType>
222-
void PidIncrementalSynchronous<QNumberType>::SetLimits(PidLimits<QNumberType> limits)
198+
void PidIncrementalSynchronous<QNumberType>::SetPoint(QNumberType setPoint)
223199
{
224-
PidIncrementalBase<QNumberType>::SetLimits(limits);
200+
PidIncrementalBase<QNumberType>::SetPoint(setPoint);
225201
}
226202

227203
template<class QNumberType>
@@ -230,18 +206,6 @@ namespace controllers
230206
PidIncrementalBase<QNumberType>::SetTunings(tunnings);
231207
}
232208

233-
template<class QNumberType>
234-
void PidIncrementalSynchronous<QNumberType>::Enable()
235-
{
236-
PidIncrementalBase<QNumberType>::Enable();
237-
}
238-
239-
template<class QNumberType>
240-
void PidIncrementalSynchronous<QNumberType>::Disable()
241-
{
242-
PidIncrementalBase<QNumberType>::Disable();
243-
}
244-
245209
template<class QNumberType>
246210
OPTIMIZE_FOR_SPEED
247211
QNumberType

‎numerical/controllers/implementations/test/TestBangBangHysteresis.cpp‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -91,3 +91,39 @@ TEST_F(TestBangBangHysteresis, custom_output_levels)
9191
EXPECT_FLOAT_EQ(customRelay.Update(0.0f), -1.0f);
9292
EXPECT_FLOAT_EQ(customRelay.Update(0.2f), 1.0f);
9393
}
94+
95+
TEST_F(TestBangBangHysteresis, reset_to_high_state)
96+
{
97+
relay.Reset(controllers::RelayState::High);
98+
99+
EXPECT_EQ(relay.State(), controllers::RelayState::High);
100+
EXPECT_FLOAT_EQ(relay.Update(0.0f), 1.0f);
101+
EXPECT_EQ(relay.State(), controllers::RelayState::High);
102+
}
103+
104+
TEST_F(TestBangBangHysteresis, stays_low_inside_band)
105+
{
106+
float output = relay.Update(0.1f);
107+
108+
EXPECT_EQ(relay.State(), controllers::RelayState::Low);
109+
EXPECT_FLOAT_EQ(output, 0.0f);
110+
}
111+
112+
TEST_F(TestBangBangHysteresis, does_not_switch_high_below_upper_threshold)
113+
{
114+
float output = relay.Update(0.19f);
115+
116+
EXPECT_EQ(relay.State(), controllers::RelayState::Low);
117+
EXPECT_FLOAT_EQ(output, 0.0f);
118+
}
119+
120+
TEST_F(TestBangBangHysteresis, reset_then_update_matches_fresh_instance)
121+
{
122+
relay.Update(0.3f);
123+
relay.Reset();
124+
125+
controllers::BangBangHysteresis<float> fresh{ -0.2f, 0.2f, 0.0f, 1.0f };
126+
127+
EXPECT_FLOAT_EQ(relay.Update(0.1f), fresh.Update(0.1f));
128+
EXPECT_EQ(relay.State(), fresh.State());
129+
}

‎numerical/controllers/implementations/test/TestFeedforward2Dof.cpp‎

Lines changed: 24 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,9 @@ TEST_F(TestFeedforward2Dof, passes_correct_error_to_feedback)
4646
EXPECT_CALL(ff, Evaluate(0.4f)).WillOnce(testing::Return(0.0f));
4747
EXPECT_CALL(fb, Process(0.3f)).WillOnce(testing::Return(0.3f));
4848

49-
controller.Compute(0.4f, 0.1f);
49+
float result{ controller.Compute(0.4f, 0.1f) };
50+
51+
EXPECT_NEAR(result, 0.3f, math::Tolerance<float>());
5052
}
5153

5254
TEST_F(TestFeedforward2Dof, passes_reference_to_feedforward)
@@ -87,6 +89,26 @@ TEST_F(TestFeedforward2Dof, perfect_feedforward_zero_error)
8789
EXPECT_NEAR(result, 0.7f, math::Tolerance<float>());
8890
}
8991

92+
TEST_F(TestFeedforward2Dof, output_is_clamped_negative)
93+
{
94+
EXPECT_CALL(ff, Evaluate(testing::_)).WillOnce(testing::Return(-0.9f));
95+
EXPECT_CALL(fb, Process(testing::_)).WillOnce(testing::Return(-0.9f));
96+
97+
float result{ controller.Compute(-1.0f, 0.0f) };
98+
99+
EXPECT_NEAR(result, -1.0f, math::Tolerance<float>());
100+
}
101+
102+
TEST_F(TestFeedforward2Dof, zero_feedback_reduces_to_feedforward)
103+
{
104+
EXPECT_CALL(ff, Evaluate(0.5f)).WillOnce(testing::Return(0.4f));
105+
EXPECT_CALL(fb, Process(testing::_)).WillOnce(testing::Return(0.0f));
106+
107+
float result{ controller.Compute(0.5f, 0.1f) };
108+
109+
EXPECT_NEAR(result, 0.4f, math::Tolerance<float>());
110+
}
111+
90112
TEST_F(TestFeedforward2Dof, reset_delegates_to_feedback)
91113
{
92114
EXPECT_CALL(fb, Reset()).Times(1);
@@ -97,7 +119,7 @@ TEST_F(TestFeedforward2Dof, reset_delegates_to_feedback)
97119
TEST_F(TestFeedforward2Dof, negative_reference_handled)
98120
{
99121
EXPECT_CALL(ff, Evaluate(-0.3f)).WillOnce(testing::Return(-0.2f));
100-
EXPECT_CALL(fb, Process(testing::_)).WillOnce(testing::Return(-0.1f));
122+
EXPECT_CALL(fb, Process(-0.3f)).WillOnce(testing::Return(-0.1f));
101123

102124
float result{ controller.Compute(-0.3f, 0.0f) };
103125

0 commit comments

Comments
 (0)