diff --git a/apps/ServoService/servoService.cpp b/apps/ServoService/servoService.cpp
index 01d8f151..0e04f978 100644
--- a/apps/ServoService/servoService.cpp
+++ b/apps/ServoService/servoService.cpp
@@ -38,7 +38,8 @@ core::ErrorCode ServoService::Run(std::stop_token token) {
 core::ErrorCode ServoService::Initialize(
       const std::unordered_map<std::string, std::string>& parms) {
     //  this->servo_controller.Init(parms, std::make_unique<i2c::I2CController>());
-    this->servo_controller.Init(parms, std::make_unique<simba::i2c::I2CController>());
+    this->servo_controller.Init(parms, std::make_unique<simba::i2c::I2CController>(),
+      std::make_unique<gpio::GPIOController>());
     main_servo_status_event =
       std::make_shared<com::someip::EventSkeleton>("ServoApp/servoStatusEvent");
     vent_servo_status_event =
diff --git a/mw/gpio_server/controller/BUILD b/mw/gpio_server/controller/BUILD
index 8c951992..899a5a81 100644
--- a/mw/gpio_server/controller/BUILD
+++ b/mw/gpio_server/controller/BUILD
@@ -7,9 +7,12 @@ cc_library(
         "//core/logger:Logger",
     ],
     srcs = ["gpio_controller.cpp"],
-    hdrs = ["gpio_controller.hpp"],
+    hdrs = [
+        "gpio_controller.hpp",
+        "Igpio_controller.h",
+        ],
     visibility = [
         "//apps:__subpackages__",
         "//mw:__subpackages__",
         ],
-)
\ No newline at end of file
+)
diff --git a/mw/gpio_server/controller/Igpio_controller.h b/mw/gpio_server/controller/Igpio_controller.h
new file mode 100644
index 00000000..a7b7a75a
--- /dev/null
+++ b/mw/gpio_server/controller/Igpio_controller.h
@@ -0,0 +1,26 @@
+/**
+ * @file Igpio_controller.h
+ * @author Michał Mańkowski (m.mankowski2004@gmail.com)
+ * @brief Interface for gpio controller
+ * @version 0.1
+ * @date 2024-06-16
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+#ifndef MW_GPIO_SERVER_CONTROLLER_IGPIO_CONTROLLER_H_
+#define MW_GPIO_SERVER_CONTROLLER_IGPIO_CONTROLLER_H_
+#include <optional>
+#include "core/common/error_code.h"
+
+namespace simba {
+namespace gpio {
+class IGPIOController {
+ public:
+    virtual core::ErrorCode SetPinValue(uint8_t actuatorID, int8_t value) = 0;
+    virtual std::optional<int8_t> GetPinValue(uint8_t actuatorID) = 0;
+    virtual ~IGPIOController() = default;
+};
+}  // namespace gpio
+}  // namespace simba
+#endif  // MW_GPIO_SERVER_CONTROLLER_IGPIO_CONTROLLER_H_
diff --git a/mw/gpio_server/controller/gpio_controller.hpp b/mw/gpio_server/controller/gpio_controller.hpp
index 83032e7c..c735cda5 100644
--- a/mw/gpio_server/controller/gpio_controller.hpp
+++ b/mw/gpio_server/controller/gpio_controller.hpp
@@ -22,19 +22,20 @@
 #include "core/gpio/GPIO_digital_driver.h"
 #include "mw/gpio_server/data/header.hpp"
 #include "core/logger/Logger.h"
+#include "mw/gpio_server/controller/Igpio_controller.h"
 
 namespace simba {
 namespace gpio {
 
 
-class GPIOController {
+class GPIOController : public IGPIOController{
  private:
     std::unique_ptr<com::soc::ISocketStream> sock_;
  public:
     GPIOController() {}
     explicit GPIOController(std::unique_ptr<com::soc::ISocketStream> socket);
-    core::ErrorCode SetPinValue(uint8_t actuatorID, int8_t value);
-    std::optional<int8_t> GetPinValue(uint8_t actuatorID);
+    core::ErrorCode SetPinValue(uint8_t actuatorID, int8_t value) override;
+    std::optional<int8_t> GetPinValue(uint8_t actuatorID) override;
 };
 
 }  // namespace gpio
diff --git a/mw/gpio_server/controller/mock/BUILD b/mw/gpio_server/controller/mock/BUILD
new file mode 100644
index 00000000..0e86e198
--- /dev/null
+++ b/mw/gpio_server/controller/mock/BUILD
@@ -0,0 +1,12 @@
+cc_library(
+    name = "mock_gpio_controller",
+    srcs = [
+        "mock_gpio_controller.h",
+        ],
+    visibility = ["//visibility:public"],
+    deps=[
+        "@com_google_googletest//:gtest_main",
+        "//mw/gpio_server/controller:gpio_controller",
+    ],
+    testonly = True,
+)
diff --git a/mw/gpio_server/controller/mock/mock_gpio_controller.h b/mw/gpio_server/controller/mock/mock_gpio_controller.h
new file mode 100644
index 00000000..18f1a871
--- /dev/null
+++ b/mw/gpio_server/controller/mock/mock_gpio_controller.h
@@ -0,0 +1,25 @@
+/**
+ * @file mock_gpio_controller.h
+ * @author Michał Mańkowski (m.mankowski2004@gmail.com)
+ * @brief Mock gpio controller
+ * @version 0.1
+ * @date 2024-06-16
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+#ifndef MW_GPIO_SERVER_CONTROLLER_MOCK_MOCK_GPIO_CONTROLLER_H_
+#define MW_GPIO_SERVER_CONTROLLER_MOCK_MOCK_GPIO_CONTROLLER_H_
+
+#include "gmock/gmock.h"
+#include "mw/gpio_server/controller/Igpio_controller.h"
+namespace simba {
+namespace mock {
+class MOCKGPIOController : public gpio::IGPIOController {
+ public:
+  MOCK_METHOD(core::ErrorCode, SetPinValue, (uint8_t actuatorID, int8_t value), (override));
+  MOCK_METHOD(std::optional<int8_t>, GetPinValue, (uint8_t actuatorID), (override));
+};
+}  // namespace mock
+}  // namespace simba
+#endif  // MW_GPIO_SERVER_CONTROLLER_MOCK_MOCK_GPIO_CONTROLLER_H_
diff --git a/mw/i2c_service/controller/Ii2c_controller.h b/mw/i2c_service/controller/Ii2c_controller.h
index a078a456..ce6920f3 100644
--- a/mw/i2c_service/controller/Ii2c_controller.h
+++ b/mw/i2c_service/controller/Ii2c_controller.h
@@ -12,8 +12,9 @@
 #define MW_I2C_SERVICE_CONTROLLER_II2C_CONTROLLER_H_
 #include <vector>
 #include <optional>
+#include <memory>
 #include "mw/i2c_service/data/header.h"
-
+#include "communication-core/sockets/stream_ipc_socket.h"
 
 namespace simba {
 namespace i2c {
@@ -23,6 +24,7 @@ class II2CController{
                         uint8_t address, const std::vector<uint8_t>& payload) = 0;
 
  public:
+  virtual core::ErrorCode Init(std::unique_ptr<com::soc::StreamIpcSocket> socket) = 0;
   /**
    * @brief 
    * 
diff --git a/mw/i2c_service/controller/i2c_controller.h b/mw/i2c_service/controller/i2c_controller.h
index 130aa5b4..ba680b1a 100644
--- a/mw/i2c_service/controller/i2c_controller.h
+++ b/mw/i2c_service/controller/i2c_controller.h
@@ -31,7 +31,7 @@ class I2CController: public II2CController{
                         uint8_t address, const std::vector<uint8_t>& payload) override;
 
  public:
-  core::ErrorCode Init(std::unique_ptr<com::soc::StreamIpcSocket> socket);
+  core::ErrorCode Init(std::unique_ptr<com::soc::StreamIpcSocket> socket) override;
   /**
    * @brief 
    * 
diff --git a/mw/i2c_service/controller/mock/mock_i2ccontroller.h b/mw/i2c_service/controller/mock/mock_i2ccontroller.h
index 4c0de47f..87e72039 100644
--- a/mw/i2c_service/controller/mock/mock_i2ccontroller.h
+++ b/mw/i2c_service/controller/mock/mock_i2ccontroller.h
@@ -14,6 +14,7 @@
 
 #include <vector>
 #include <optional>
+#include <memory>
 #include "gmock/gmock.h"
 #include "mw/i2c_service/controller/i2c_controller.h"
 
@@ -30,6 +31,7 @@ class MockI2CController : public i2c::II2CController {
         (const uint8_t address, const uint8_t reg, const uint8_t size), (override));
   MOCK_METHOD(std::optional<std::vector<uint8_t>>, WriteRead, (const uint8_t address,
         const uint8_t WriteData, const uint8_t ReadSize), (override));
+  MOCK_METHOD(core::ErrorCode, Init, (std::unique_ptr<com::soc::StreamIpcSocket> socket), (override));
 };
 }  // namespace mock
 }  // namespace simba
diff --git a/mw/i2c_service/controller/pca9685/controller.cpp b/mw/i2c_service/controller/pca9685/controller.cpp
index d74fccf0..f9b59b4a 100644
--- a/mw/i2c_service/controller/pca9685/controller.cpp
+++ b/mw/i2c_service/controller/pca9685/controller.cpp
@@ -39,31 +39,50 @@ namespace {
 
 PCA9685::PCA9685() {
 }
-void PCA9685::Init(const std::unordered_map<std::string, std::string>& parms, std::unique_ptr<I2CController> i2c) {
-  this->i2c_ = std::move(i2c);
+core::ErrorCode PCA9685::Init(const std::unordered_map<std::string, std::string>& parms,
+ std::unique_ptr<II2CController> i2c, std::unique_ptr<gpio::IGPIOController> gpio) {
+  if (this->setI2C(std::move(i2c)) != core::ErrorCode::kOk || this->setGPIO(std::move(gpio)) != core::ErrorCode::kOk) {
+    AppLogger::Warning("Failed pointer assignment");
+    return core::ErrorCode::kInitializeError;
+  }
   this->i2c_->Init(std::make_unique<com::soc::StreamIpcSocket>());
   this->app_name = parms.at("app_name");
   std::string file_path = "/opt/"+this->app_name+"/etc/config.json";
   std::ifstream file(file_path);
   if (!file.is_open()) {
       AppLogger::Warning("Cant find file on path "+file_path);
-      return;
+      return core::ErrorCode::kInitializeError;;
   }
   nlohmann::json data = nlohmann::json::parse(file);
   auto db = this->ReadConfig(data);
   if (!db.has_value()) {
     AppLogger::Error("Servo config does not exist");
-    return;
+    return core::ErrorCode::kInitializeError;;
   }
   this->db_ = db.value();
   for (auto chan : this->db_) {
       this->SetServo(chan.second.channel, chan.second.off_pos);
   }
+  return core::ErrorCode::kOk;
+}
+core::ErrorCode PCA9685::setI2C(std::unique_ptr<II2CController> i2c) {
+  if (!i2c) {
+    return core::ErrorCode::kInitializeError;
+  }
+  this->i2c_ = std::move(i2c);
+  return core::ErrorCode::kOk;
+}
+core::ErrorCode PCA9685::setGPIO(std::unique_ptr<gpio::IGPIOController> gpio) {
+  if (!gpio) {
+    return core::ErrorCode::kInitializeError;
+  }
+  this->gpio_ = std::move(gpio);
+  return core::ErrorCode::kOk;
 }
 void PCA9685::MosfetFunc(const uint8_t &mosfet_id, const uint8_t &mosfet_time) {
-    this->gpio_.SetPinValue(mosfet_id, 1);
+    this->gpio_->SetPinValue(mosfet_id, 1);
     std::this_thread::sleep_for(std::chrono::milliseconds(mosfet_time));
-    this->gpio_.SetPinValue(mosfet_id, 0);
+    this->gpio_->SetPinValue(mosfet_id, 0);
 }
 
 core::ErrorCode PCA9685::AutoSetServoPosition(const uint8_t &actuator_id, const uint8_t &state) {
diff --git a/mw/i2c_service/controller/pca9685/controller.hpp b/mw/i2c_service/controller/pca9685/controller.hpp
index 5414e566..311da51b 100644
--- a/mw/i2c_service/controller/pca9685/controller.hpp
+++ b/mw/i2c_service/controller/pca9685/controller.hpp
@@ -18,8 +18,10 @@
 #include <future> // NOLINT
 #include <memory>
 
+#include "mw/i2c_service/controller/Ii2c_controller.h"
 #include "mw/i2c_service/controller/i2c_controller.h"
 #include "mw/gpio_server/controller/gpio_controller.hpp"
+#include "mw/gpio_server/controller/Igpio_controller.h"
 #include "core/json/json_parser.h"
 namespace simba {
 namespace i2c {
@@ -40,8 +42,8 @@ struct Servo {
 
 class PCA9685 {
  private:
-  std::unique_ptr<I2CController> i2c_;
-  gpio::GPIOController gpio_;
+  std::unique_ptr<II2CController> i2c_;
+  std::unique_ptr<gpio::IGPIOController> gpio_;
   std::string app_name;
  protected:
   std::unordered_map<uint8_t, Servo> db_;
@@ -49,9 +51,12 @@ class PCA9685 {
   core::ErrorCode SetServo(uint8_t channel, uint16_t pos);
   void MosfetFunc(const uint8_t &mosfet_id, const uint8_t &mosfet_time);
   std::vector<uint8_t> GenerateData(const uint8_t &channel, const uint16_t &pos);
+  core::ErrorCode setI2C(std::unique_ptr<II2CController> adc_);
+  core::ErrorCode setGPIO(std::unique_ptr<gpio::IGPIOController> gpio);
  public:
   PCA9685();
-  void Init(const std::unordered_map<std::string, std::string>& parms, std::unique_ptr<I2CController> i2c);
+  core::ErrorCode Init(const std::unordered_map<std::string, std::string>& parms, std::unique_ptr<II2CController> i2c,
+    std::unique_ptr<gpio::IGPIOController> gpio);
   core::ErrorCode AutoSetServoPosition(const uint8_t &actuator_id, const uint8_t &state);
   std::optional<uint8_t> ReadServoPosition(const uint8_t &actuator_id);
 };
diff --git a/mw/i2c_service/tests/BUILD b/mw/i2c_service/tests/BUILD
index cff41572..a6b5a056 100644
--- a/mw/i2c_service/tests/BUILD
+++ b/mw/i2c_service/tests/BUILD
@@ -17,7 +17,9 @@ cc_test(
     visibility = ["//visibility:public"],
     deps=[
         "@com_google_googletest//:gtest_main",
-        "//mw/i2c_service/controller/pca9685:pca9685_controller"
+        "//mw/i2c_service/controller/pca9685:pca9685_controller",
+        "//mw/gpio_server/controller/mock:mock_gpio_controller",
+        "//mw/i2c_service/controller/mock:mock_i2c_controller",
     ],
 )
 cc_test(
diff --git a/mw/i2c_service/tests/test_adcsensor.cc b/mw/i2c_service/tests/test_adcsensor.cc
index cc2006e5..b9dbb931 100644
--- a/mw/i2c_service/tests/test_adcsensor.cc
+++ b/mw/i2c_service/tests/test_adcsensor.cc
@@ -29,8 +29,8 @@ class TestWrapper : public simba::i2c::ADCSensorController {
     this->setConfig(db_);
   }
 
-  void TestSetPtr(std::unique_ptr<simba::mock::MockADS7828> adc_) {
-    this->setPtr(std::move(adc_));
+  simba::core::ErrorCode TestSetPtr(std::unique_ptr<simba::i2c::IADS7828> adc_) {
+    return this->setPtr(std::move(adc_));
   }
 };
 
@@ -85,7 +85,86 @@ INSTANTIATE_TEST_SUITE_P(TestReadConfigParams, TestReadConfig, ::testing::Values
             "a": 100,
             "b": -20
         }
-        ]})", std::unordered_map<uint8_t, simba::i2c::SensorConfig>{{10, {0x1, 123, -12}}, {11, {0x2, 100, -20}}})
+        ]})", std::unordered_map<uint8_t, simba::i2c::SensorConfig>{{10, {0x1, 123, -12}}, {11, {0x2, 100, -20}}}),
+    std::make_tuple(R"({  })", std::unordered_map<uint8_t, simba::i2c::SensorConfig>{}),
+    std::make_tuple(R"({
+    "sensors": [
+    ]
+    })", std::unordered_map<uint8_t, simba::i2c::SensorConfig>{}),
+    std::make_tuple(R"({
+    "sensors": [
+        {
+            "actuator_id": 11,
+            "channel": 2,
+            "R": 150,
+            "RES_MIN":0,
+            "RES_MAX":0.3,
+            "A_MIN": 4,
+            "A_MAX": 20
+        }
+        ]})", std::unordered_map<uint8_t, simba::i2c::SensorConfig>{{11, {0x2, 0.125, -0.075}}}),
+    std::make_tuple(R"({
+    "sensors": [
+        {
+            "actuator_id": 11,
+            "channel": 2,
+            "R": 150,
+            "RES_MAX":0.3,
+            "A_MIN": 4,
+            "A_MAX": 20
+        }
+        ]})", std::unordered_map<uint8_t, simba::i2c::SensorConfig>{}),
+    std::make_tuple(R"({
+    "sensors": [
+        {
+            "actuator_id": 11,
+            "channel": 2,
+            "R": 150,
+            "RES_MIN":0,
+            "A_MIN": 4,
+            "A_MAX": 20
+        }
+        ]})", std::unordered_map<uint8_t, simba::i2c::SensorConfig>{}),
+    std::make_tuple(R"({
+    "sensors": [
+        {
+            "actuator_id": 11,
+            "channel": 2,
+            "RES_MIN"0:,
+            "RES_MAX":0.3,
+            "A_MIN": 4,
+            "A_MAX": 20
+        }
+        ]})", std::unordered_map<uint8_t, simba::i2c::SensorConfig>{}),
+    std::make_tuple(R"({
+    "sensors": [
+        {
+            "actuator_id": 11,
+            "channel": 2,
+            "R": 150,
+            "RES_MIN":0,
+            "RES_MAX":0.3,
+            "A_MAX": 20
+        }
+        ]})", std::unordered_map<uint8_t, simba::i2c::SensorConfig>{}),
+    std::make_tuple(R"({
+    "sensors": [
+        {
+            "actuator_id": 11,
+            "channel": 2,
+            "R": 150,
+            "RES_MIN":0,
+            "RES_MAX":0.3,
+            "A_MIN": 4
+        }
+        ]})", std::unordered_map<uint8_t, simba::i2c::SensorConfig>{}),
+    std::make_tuple(R"({
+    "sensors": [
+        {
+            "actuator_id": 11,
+            "channel": 2
+        }
+        ]})", std::unordered_map<uint8_t, simba::i2c::SensorConfig>{})
 ));
 
 TEST_P(TestReadConfig, ReadConfigCheck) {
@@ -112,11 +191,15 @@ class TestGetValue : public ::testing::TestWithParam<std::tuple<std::unordered_m
 // tuple<config, sensor_id, mockAdcVoltage, expectedResult>
 INSTANTIATE_TEST_SUITE_P(TestGetValueParams, TestGetValue, ::testing::Values(
   std::make_tuple(std::unordered_map<uint8_t, simba::i2c::SensorConfig>
-  {{10, {1, 123, -12}}, {11, {1, 100, -20}}}, 11, std::optional<float>{3.2}, std::optional<float>{300}),
+    {{10, {1, 123, -12}}, {11, {1, 100, -20}}}, 11, std::optional<float>{3.2}, std::optional<float>{300}),
+  std::make_tuple(std::unordered_map<uint8_t, simba::i2c::SensorConfig>
+    {{10, {1, 123, -12}}, {11, {1, 100, -20}}}, 11, std::optional<float>{1}, std::optional<float>{80}),
   std::make_tuple(std::unordered_map<uint8_t, simba::i2c::SensorConfig>
-  {{10, {1, 123, -12}}, {11, {1, 100, -20}}}, 11, std::optional<float>{1}, std::optional<float>{80}),
+    {{1, {10, 200, -40}}}, 1, std::optional<float>{3.2}, std::optional<float>{600}),
+  std::make_tuple(std::unordered_map<uint8_t, simba::i2c::SensorConfig>{
+    }, 1, std::optional<float>{3.2}, std::optional<float>{}),
   std::make_tuple(std::unordered_map<uint8_t, simba::i2c::SensorConfig>
-  {{1, {10, 200, -40}}}, 1, std::optional<float>{3.2}, std::optional<float>{600})
+    {{1, {10, 200, -40}}}, 1, std::optional<float>{}, std::optional<float>{})
 ));
 
 TEST_P(TestGetValue, GetValueCheck) {
@@ -135,3 +218,10 @@ TEST_P(TestGetValue, GetValueCheck) {
     wrapper.TestSetPtr(std::move(mock_adc_));
     EXPECT_EQ(wrapper.GetValue(sensor_id), expectedResult);
 }
+
+TEST(TestADCSensor, SetPtrTest) {
+    TestWrapper wrapper{};
+    auto adc = std::make_unique<simba::i2c::ADS7828>();
+    EXPECT_EQ(wrapper.TestSetPtr(nullptr), simba::core::ErrorCode::kInitializeError);
+    EXPECT_EQ(wrapper.TestSetPtr(std::move(adc)), simba::core::ErrorCode::kOk);
+}
diff --git a/mw/i2c_service/tests/test_pca9685.cpp b/mw/i2c_service/tests/test_pca9685.cpp
index 49b92021..0098a4ca 100644
--- a/mw/i2c_service/tests/test_pca9685.cpp
+++ b/mw/i2c_service/tests/test_pca9685.cpp
@@ -1,6 +1,7 @@
 /**
  * @file test_pca9685.cpp
  * @author Mateusz Krajewski (matikrajek42@gmail.com)
+ * @author Michał Mańkowski (m.mankowski2004@gmail.com)
  * @brief 
  * @version 0.1
  * @date 2024-05-13
@@ -15,8 +16,12 @@
 #include <string>
 
 #include "mw/i2c_service/controller/pca9685/controller.hpp"
+#include "mw/gpio_server/controller/mock/mock_gpio_controller.h"
+#include "mw/i2c_service/controller/mock/mock_i2ccontroller.h"
 
-
+namespace {
+  constexpr uint8_t PCA9685_ADDRESS = 112;
+}
 class TestWrapper : public simba::i2c::PCA9685 {
  public:
   std::vector<uint8_t> GetData(const uint8_t &channel, const uint16_t &pos) {
@@ -48,6 +53,18 @@ class TestWrapper : public simba::i2c::PCA9685 {
   std::optional<uint8_t> read_pos(const uint8_t &actuator_id) {
     return this->ReadServoPosition(actuator_id);
   }
+  simba::core::ErrorCode TestSetGpio(std::unique_ptr<simba::gpio::IGPIOController> gpio) {
+    return this->setGPIO(std::move(gpio));
+  }
+  simba::core::ErrorCode TestSetI2C(std::unique_ptr<simba::i2c::II2CController> i2c) {
+    return this->setI2C(std::move(i2c));
+  }
+  void TestMosfetFunc(const uint8_t &mosfet_id, const uint8_t &mosfet_time) {
+    this->MosfetFunc(mosfet_id, mosfet_time);
+  }
+  simba::core::ErrorCode TestSetServo(uint8_t channel, uint16_t pos) {
+    return this->SetServo(channel, pos);
+  }
 };
 
 TEST(TESTCheckServoPOS, TestPOS) {
@@ -58,14 +75,22 @@ TEST(TESTCheckServoPOS, TestPOS) {
   EXPECT_EQ(wrapper.read_pos(1).value(), 0);
 }
 
-TEST(TestPCAController, DataCreateTest) {
+class DataCreateTest : public ::testing::TestWithParam<std::tuple<uint8_t, uint16_t, std::vector<uint8_t>>>{
+};
+
+// tuple<channel, pos, expected>
+INSTANTIATE_TEST_SUITE_P(DataCreateTestParams, DataCreateTest, ::testing::Values(
+  std::make_tuple(0, 0, std::vector<uint8_t>{0, 1, 0xFE, 121, 0x6, 0, 0x7, 0, 0x8, 0, 0x9, 0}),
+  std::make_tuple(2, 2048, std::vector<uint8_t>{0, 1, 0xFE, 121, 14, 0, 15, 0, 16, 0, 17, 8})
+));
+
+TEST_P(DataCreateTest, GenerateDataTest) {
     TestWrapper wrapper{};
-    auto res = wrapper.GetData(0, 0);
-    auto res2 = wrapper.GetData(2, 2048);
-    std::vector<uint8_t> vec{0, 1, 0xFE, 121, 0x6, 0, 0x7, 0, 0x8, 0, 0x9, 0};
-    std::vector<uint8_t> vec2{0, 1, 0xFE, 121, 14, 0, 15, 0, 16, 0, 17, 8};
-    ASSERT_EQ(vec, res);
-    EXPECT_EQ(vec2, res2);
+    auto params = GetParam();
+    uint8_t channel = std::get<0>(params);
+    uint16_t pos = std::get<1>(params);
+    auto expected = std::get<2>(params);
+    EXPECT_EQ(wrapper.GetData(channel, pos), expected);
 }
 
 class PCA9685ConfigTests : public ::testing::TestWithParam<std::tuple<uint16_t, std::string, uint16_t,
@@ -140,3 +165,53 @@ TEST_P(PCA9685ConfigTests, READ_DATA_TEST) {
   EXPECT_EQ(servo.servo_delay, std::get<11>(GetParam()));
   EXPECT_EQ(servo.mosfet_time, std::get<12>(GetParam()));
 };
+
+TEST(TestPCAController, MosfetFuncTest) {
+    TestWrapper wrapper{};
+    auto mock_gpio = std::make_unique<simba::mock::MOCKGPIOController>();
+    const uint8_t mosfet_id = 1;
+    const uint8_t mosfet_time = 1;
+    EXPECT_CALL(*mock_gpio, SetPinValue(mosfet_id, ::testing::_))
+      .Times(2);
+    wrapper.TestSetGpio(std::move(mock_gpio));
+    wrapper.TestMosfetFunc(mosfet_id, mosfet_time);
+}
+
+TEST(TestPCAController, SetServoTest) {
+    TestWrapper wrapper{};
+    auto mock_i2c = std::make_unique<simba::mock::MockI2CController>();
+    constexpr uint8_t channel = 1;
+    constexpr uint16_t pos = 1;
+    EXPECT_CALL(*mock_i2c, Write(PCA9685_ADDRESS, ::testing::_))
+      .WillOnce(::testing::Return(simba::core::ErrorCode::kOk))
+      .WillOnce(::testing::Return(simba::core::ErrorCode::kConnectionError));
+    wrapper.TestSetI2C(std::move(mock_i2c));
+    EXPECT_EQ(wrapper.TestSetServo(channel, pos), simba::core::ErrorCode::kOk);
+    EXPECT_EQ(wrapper.TestSetServo(channel, pos), simba::core::ErrorCode::kConnectionError);
+}
+
+TEST(TestPCAController, AutoSetServoPosTest) {
+    TestWrapper wrapper{};
+    wrapper.Init();
+    auto mock_gpio = std::make_unique<simba::mock::MOCKGPIOController>();
+    auto mock_i2c = std::make_unique<simba::mock::MockI2CController>();
+    EXPECT_CALL(*mock_gpio, SetPinValue(::testing::_, ::testing::_))
+      .Times(2);
+    EXPECT_CALL(*mock_i2c, Write(PCA9685_ADDRESS, ::testing::_))
+      .Times(2);
+    wrapper.TestSetGpio(std::move(mock_gpio));
+    wrapper.TestSetI2C(std::move(mock_i2c));
+    EXPECT_EQ(wrapper.AutoSetServoPosition(0, 0), simba::core::ErrorCode::kNotDefine);
+    EXPECT_EQ(wrapper.AutoSetServoPosition(1, 0), simba::core::ErrorCode::kError);
+    EXPECT_EQ(wrapper.AutoSetServoPosition(1, 1), simba::core::ErrorCode::kOk);
+}
+
+TEST(TestPCAController, SetPtrsTest) {
+    TestWrapper wrapper{};
+    auto gpio = std::make_unique<simba::gpio::GPIOController>();
+    auto i2c = std::make_unique<simba::i2c::I2CController>();
+    EXPECT_EQ(wrapper.TestSetGpio(nullptr), simba::core::ErrorCode::kInitializeError);
+    EXPECT_EQ(wrapper.TestSetGpio(std::move(gpio)), simba::core::ErrorCode::kOk);
+    EXPECT_EQ(wrapper.TestSetI2C(nullptr), simba::core::ErrorCode::kInitializeError);
+    EXPECT_EQ(wrapper.TestSetI2C(std::move(i2c)), simba::core::ErrorCode::kOk);
+}