Skip to content

Commit 78bd118

Browse files
taketook34taketook34
committed
Issue #14-2: Code style fixes. Error enumeration updates. Adding tests for PID regulator and motorsController
1 parent f330f8b commit 78bd118

File tree

6 files changed

+231
-11
lines changed

6 files changed

+231
-11
lines changed

‎src/kpi_rover_ecu/include/motor.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,8 @@ class Motor {
2020
int GetEncoderCounter();
2121

2222
private:
23+
float GetSpeedError(int _ticks, float _timeSegment);
24+
float GetTimeSegment();
2325
int MotorSet(int inputRPM);
2426

2527
PIDRegulator pidRegulator_;

‎src/kpi_rover_ecu/src/motor.cpp‎

Lines changed: 19 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -58,27 +58,35 @@ int Motor::MotorStop() const {
5858
return 0;
5959
}
6060

61-
int Motor::GetEncoderCounter() {
62-
const int kEncoderTicks = rc_encoder_read(motorNumber_);
63-
int pid_encoder_ticks = kEncoderTicks;
64-
if (!inverted_) {
65-
pid_encoder_ticks *= -1;
66-
}
67-
61+
float Motor::GetTimeSegment() {
6862
const auto kCurrentTimePoint = std::chrono::high_resolution_clock::now();
6963
const std::chrono::duration<double, std::milli> kElapsedMilliseconds = kCurrentTimePoint - lastTimePoint_;
7064
lastTimePoint_ = kCurrentTimePoint;
71-
const auto kTimeDt = static_cast<float>(kElapsedMilliseconds.count());
65+
return static_cast<float>(kElapsedMilliseconds.count());
66+
}
7267

73-
const float kRevolutions = static_cast<float>(pid_encoder_ticks) / static_cast<float>(kLoopTicks);
68+
float Motor::GetSpeedError(int _ticks, float _timeSegment) {
69+
const float kRevolutions = static_cast<float>(_ticks) / static_cast<float>(kLoopTicks);
7470
const float kInputPoint =
75-
static_cast<float>(std::round((kRevolutions * kSecondsMinute * kMiliSecondsSeconds) / kTimeDt)) *
71+
static_cast<float>(std::round((kRevolutions * kSecondsMinute * kMiliSecondsSeconds) / _timeSegment)) *
7672
kSpeedIndexMultipler;
7773
actualRpm_ = static_cast<int>(kInputPoint);
7874
const float kError = static_cast<float>(setpointRpm_) - kInputPoint;
79-
8075
std::cout << "set point " << setpointRpm_ << " current point " << kInputPoint << " error " << kError << '\n';
8176

77+
return kError;
78+
}
79+
80+
int Motor::GetEncoderCounter() {
81+
const int kEncoderTicks = rc_encoder_read(motorNumber_);
82+
int pid_encoder_ticks = kEncoderTicks;
83+
if (!inverted_) {
84+
pid_encoder_ticks *= -1;
85+
}
86+
87+
const float kTimeDt = GetTimeSegment();
88+
const float kError = GetSpeedError(pid_encoder_ticks, kTimeDt);
89+
8290
const int kPidOutput = pidRegulator_.Run(kError, kTimeDt);
8391
rc_encoder_write(motorNumber_, 0);
8492

‎src/kpi_rover_ecu/tests/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,8 @@ add_executable(unitTests
3030
test_motor.cpp
3131
test_protocol.cpp
3232
test_message_queue.cpp
33+
test_motor_controller.cpp
34+
test_pid_regulator.cpp
3335
)
3436

3537
target_link_libraries(unitTests

‎src/kpi_rover_ecu/tests/test_motor.cpp‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,17 @@ TEST_F(MotorTest, MotorGoAboveMaxRPM) {
6666
EXPECT_EQ(0, result);
6767
}
6868

69+
// Test MotorGo with RPM below -maximum
70+
TEST_F(MotorTest, MotorGoBelowMinusMaxRPM) {
71+
// Verify behaviour with RPM below -maximum (should clamp to -MAX_RPM)
72+
EXPECT_CALL(GetMockRCMotor(), set(0, ::testing::DoubleEq(-1))).WillOnce(::testing::Return(0));
73+
74+
motor = new Motor(0, false, {1.5f, 0.056f, 1.5f});
75+
int result = motor->MotorGo(-Motor::kMaxRpm - 1);
76+
77+
EXPECT_EQ(0, result);
78+
}
79+
6980
// Test MotorGo with set function returning error
7081
TEST_F(MotorTest, MotorGoSetError) {
7182
EXPECT_CALL(GetMockRCMotor(), set(0, ::testing::_)).WillOnce(::testing::Return(-1));

‎src/kpi_rover_ecu/tests/test_motor_controller.cpp‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
#include <gmock/gmock.h>
2+
#include <gtest/gtest.h>
3+
4+
#include <iostream>
5+
#include <vector>
6+
7+
#include "motor.h"
8+
#include "motorConfig.h"
9+
#include "motorsController.h"
10+
11+
// Include librobotcontrol mocks
12+
#include "mocks/librobotcontrol/include/mock_rc_encoder.h"
13+
#include "mocks/librobotcontrol/include/mock_rc_motor.h"
14+
#include "mocks/librobotcontrol/include/mock_rc_start_stop.h"
15+
#include "mocks/librobotcontrol/include/mock_rc_time.h"
16+
17+
class MotorControllerTest : public ::testing::Test {
18+
protected:
19+
MotorController motor_controller;
20+
const uint8_t kMotorNumber = 4;
21+
22+
void SetUp() override {
23+
// Reset all mock objects
24+
ResetMockRCMotor();
25+
ResetMockRCEncoder();
26+
setupDefaultMockActions(); // Ensure the mock is initialized
27+
ResetMockRCTime();
28+
29+
// Set up default behaviors for mocks instead of expectations
30+
ON_CALL(getStartStopMock(), kill_existing_process(::testing::_)).WillByDefault(::testing::Return(0));
31+
ON_CALL(getStartStopMock(), enable_signal_handler()).WillByDefault(::testing::Return(0));
32+
ON_CALL(GetMockRCMotor(), init_freq(::testing::_)).WillByDefault(::testing::Return(0));
33+
ON_CALL(GetMockRCEncoder(), init()).WillByDefault(::testing::Return(0));
34+
35+
// Initialize with the same configuration as in the provided example
36+
std::vector<MotorConfig> motor_configs = {
37+
MotorConfig(1, false, {1.5f, 0.056f, 1.5f}),
38+
MotorConfig(2, false, {1.5f, 0.056f, 1.5f}),
39+
MotorConfig(3, true, {1.5f, 0.056f, 1.5f}),
40+
MotorConfig(4, true, {1.5f, 0.056f, 1.5f})
41+
};
42+
43+
motor_controller.Init(motor_configs, kMotorNumber);
44+
}
45+
46+
void TearDown() override {
47+
// Clean up expectations for Destroy
48+
EXPECT_CALL(GetMockRCMotor(), cleanup()).WillOnce(::testing::Return(0));
49+
EXPECT_CALL(GetMockRCEncoder(), cleanup()).WillOnce(::testing::Return(0));
50+
51+
motor_controller.Destroy();
52+
53+
// Clean up mock objects to prevent memory leaks
54+
DestroyMockRCMotor();
55+
DestroyMockRCEncoder();
56+
DestroyStartStopMock();
57+
DestroyMockRCTime();
58+
}
59+
};
60+
61+
// Test MotorController::Init
62+
TEST_F(MotorControllerTest, Init) {
63+
// Expectations are already set in SetUp, no additional checks needed here
64+
ASSERT_EQ(motor_controller.GetMotorsNumber(), kMotorNumber);
65+
}
66+
67+
// Test MotorController::SetMotorRPM
68+
TEST_F(MotorControllerTest, SetMotorRPM) {
69+
int channel = 1;
70+
int rpm = 1000;
71+
72+
EXPECT_CALL(GetMockRCMotor(), set(channel, ::testing::_)).WillOnce(::testing::Return(0));
73+
74+
int result = motor_controller.SetMotorRPM(channel - 1, rpm); // Convert to 0-based index
75+
ASSERT_EQ(result, 0);
76+
}
77+
78+
// Test MotorController::SetMotorRPM with invalid channel
79+
TEST_F(MotorControllerTest, SetMotorRPMInvalidChannel) {
80+
int invalid_channel = kMotorNumber + 1; // Out of range
81+
int rpm = 1000;
82+
83+
int result = motor_controller.SetMotorRPM(invalid_channel, rpm);
84+
ASSERT_EQ(result, -1);
85+
}
86+
87+
// Test MotorController::StopMotor
88+
TEST_F(MotorControllerTest, StopMotor) {
89+
int channel = 1;
90+
91+
EXPECT_CALL(GetMockRCMotor(), brake(channel)).WillOnce(::testing::Return(0));
92+
93+
int result = motor_controller.StopMotor(channel - 1); // Convert to 0-based index
94+
ASSERT_EQ(result, 0);
95+
}
96+
97+
// Test MotorController::GetEncoderCounter
98+
TEST_F(MotorControllerTest, GetEncoderCounter) {
99+
int channel = 1;
100+
int encoder_value = 42;
101+
102+
EXPECT_CALL(GetMockRCEncoder(), read(channel)).WillOnce(::testing::Return(encoder_value));
103+
104+
int result = motor_controller.GetEncoderCounter(channel - 1); // Convert to 0-based index
105+
ASSERT_EQ(result, encoder_value);
106+
}
107+
108+
// Test MotorController::Destroy
109+
TEST_F(MotorControllerTest, Destroy) {
110+
// Expectations are already set in TearDown, no additional checks needed here
111+
SUCCEED();
112+
}

‎src/kpi_rover_ecu/tests/test_pid_regulator.cpp‎

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
#include <gmock/gmock.h>
2+
#include <gtest/gtest.h>
3+
4+
#include <array>
5+
#include <cmath>
6+
7+
#include "../include/PIDRegulator.h"
8+
9+
// Test fixture for PIDRegulator tests
10+
class PIDRegulatorTest : public ::testing::Test {
11+
protected:
12+
PIDRegulator pid_regulator;
13+
14+
void SetUp() override {
15+
// Initialize PIDRegulator with some coefficients
16+
std::array<float, 3> coefficients = {1.0f, 0.5f, 0.1f};
17+
pid_regulator.Init(coefficients);
18+
}
19+
};
20+
21+
TEST_F(PIDRegulatorTest, Init) {
22+
std::array<float, 3> coefficients = {2.0f, 1.0f, 0.2f};
23+
pid_regulator.Init(coefficients);
24+
25+
// Run the PID regulator with a known error and time delta to check if the coefficients are set correctly
26+
float error = 1.0f;
27+
float time_dt = 1.0f;
28+
int result = pid_regulator.Run(error, time_dt);
29+
30+
// Check if the result is as expected
31+
float p_term = coefficients[0] * error;
32+
float i_term = coefficients[1] * error * time_dt;
33+
float d_term = coefficients[2] * (error / time_dt);
34+
35+
int expected = static_cast<int>(p_term + i_term + d_term);
36+
ASSERT_EQ(result, expected);
37+
}
38+
39+
TEST_F(PIDRegulatorTest, Run_PositiveError) {
40+
float error = 10.0f;
41+
float time_dt = 1.0f;
42+
43+
int result = pid_regulator.Run(error, time_dt);
44+
45+
// Expected result calculation
46+
float p_term = 1.0f * error;
47+
float i_term = 0.5f * error * time_dt;
48+
float d_term = 0.1f * (error / time_dt);
49+
50+
int expected = static_cast<int>(p_term + i_term + d_term);
51+
52+
ASSERT_EQ(result, expected);
53+
}
54+
55+
TEST_F(PIDRegulatorTest, Run_NegativeError) {
56+
float error = -10.0f;
57+
float time_dt = 1.0f;
58+
59+
int result = pid_regulator.Run(error, time_dt);
60+
61+
// Expected result calculation
62+
float p_term = 1.0f * error;
63+
float i_term = 0.5f * error * time_dt;
64+
float d_term = 0.1f * (error / time_dt);
65+
66+
int expected = static_cast<int>(p_term + i_term + d_term);
67+
68+
ASSERT_EQ(result, expected);
69+
}
70+
71+
TEST_F(PIDRegulatorTest, Run_ZeroTimeDt) {
72+
float error = 10.0f;
73+
float time_dt = 0.0f;
74+
75+
int result = pid_regulator.Run(error, time_dt);
76+
77+
// Expected result calculation
78+
float p_term = 1.0f * error;
79+
float i_term = 0.5f * error * time_dt;
80+
float d_term = 0.0f; // Derivative term should be zero when time_dt is zero
81+
82+
int expected = static_cast<int>(p_term + i_term);
83+
84+
ASSERT_EQ(result, expected);
85+
}

0 commit comments

Comments
 (0)