All modules and core of the app was created

main.cpp

@@ -1,54 +1,36 @@
-#include "Filters/Kalman2DFilter.hpp"
-#include "GyverBME280.h"
-#include "I2Cdev.h"
-#include "MPU6050_6Axis_MotionApps20.h"
-#include "RF/BluetoothDispatcher.hpp"
-#include "Sensors/Barometer.hpp"
-#include "Sensors/BatteryController.hpp"
-#include "Sensors/MPU.hpp"
-#include "Wire.h"
-#include "board_pins.h"
-#include <Arduino.h>
+#include "App.hpp"
+#include "BoardI2C.hpp"
+//#include "MPU6050_6Axis_MotionApps20.h"
 
-
-TwoWire i2c = TwoWire(0);
-Barometer barometer(new GyverBME280(i2c));
-MPU mpu(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c));
-Kalman2DFilter kalman2d(10.f, 1.f, 1.8f);
-BatteryController battery;
+BoardI2C i2c;
+/*
 BluetoothDispatcher bluetoothDispatcher(new BluetoothSerial());
+Sensors *sensors = nullptr; */
+
+static Application *app = nullptr;
 
 void setup() {
     Serial.begin(115200);
-    Serial.setDebugOutput(true);
-
-    Serial.print("Ininitialize I2C...");
-    Serial.println(i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000));
-
-    Serial.print("Ininitialize BMP280...");
-    Serial.println(barometer.initialize());
-
-    Serial.print("Ininitialize MPU6050...");
-    Serial.println(mpu.initialize());
-
-    Serial.print("Ininitialize Bluetooth...");
-    Serial.println(bluetoothDispatcher.initialize());
-
-    battery.initialize();
-    barometer.measureBaseAltitudeSync();
-
-    Serial.println("System initialized");
+    app = new Application(new FlightDispatcher(
+      new BluetoothDispatcher(new BluetoothSerial()),
+      new FlightController(
+        new Sensors(new Barometer(new GyverBME280(i2c)), new MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)),
+                    new Kalman2DFilter(10.f, 1.f, 1.8f), new BatteryController()),
+        new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 1), 1),
+        new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 2), 2),
+        new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 3), 3))));
+    /*sensors = new Sensors(
+    new Barometer(new GyverBME280(i2c)), new MPU(new
+  MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)), new Kalman2DFilter(10.f, 1.f, 1.8f),
+  new BatteryController()); sensors->measureBaseAltitudeSync();
+  bluetoothDispatcher.initialize();
+  Serial.println("System initialized");*/
 }
 
 void loop() {
-    bool err;
-    barometer.tick();
-    if (mpu.tick(err) && !err) {
-        float kalmanAltitude, ZVelocityAltitude;
-        float barometerAltitude = barometer.flightHeight();
-        kalman2d.filter(mpu.accZInertial(), barometerAltitude, kalmanAltitude, ZVelocityAltitude);
-        //Serial.print(barometerAltitude);
-    }
-    bluetoothDispatcher.tick();
-    delay(1);
+    app->tick();
+    /*
+  sensors->tick();
+  bluetoothDispatcher.tick();
+  delay(1);*/
 }