Brushed motors drivers were implemented

src/Logic/FlightController.cpp

@@ -38,6 +38,7 @@ void FlightController::tick() {
     if (hasUpdates) {
         _checkBatteryCharge();
     }
+    // TODO: don't apply pid values to motors while DeviceStatus != IsFlying
     switch (_status) {
         case DeviceStatus::Idle:
             break;
@@ -105,16 +106,12 @@ void FlightController::_takeoff() {
     // TODO: implement takeoff
     _status = DeviceStatus::IsFlying;
     _tailRotorController->enable();
-    _rotor1->enable();
-    _rotor2->enable();
 }
 
 void FlightController::_boarding() {
     if (_sensors->flightHeight() <= _defaultStopMotorHeight) {
         // TODO: stop motors and setpoints
         _status = DeviceStatus::Idle;
-        _rotor1->disable();
-        _rotor2->disable();
         _tailRotorController->disable();
     } else {
         // TODO: implement boarding
@@ -148,21 +145,24 @@ void FlightController::newPitchStickPosition(int16_t newYawStickPostition) {
     // TODO: implement stick input
 }
 
-void FlightController::setRotor1Duty(uint32_t rotor1Duty) {
+void FlightController::setRotor1Duty(int16_t rotor1Duty) {
     if (_status == DeviceStatus::Idle) {
-        _rotor1->setDuty(rotor1Duty);
+        auto duty = (int16_t) map((long) rotor1Duty, 0, 5000, 0, 1023);
+        _rotor1->setExtendedDuty(duty);
     }
 }
 
-void FlightController::setRotor2Duty(uint32_t rotor2Duty) {
+void FlightController::setRotor2Duty(int16_t rotor2Duty) {
     if (_status == DeviceStatus::Idle) {
-        _rotor2->setDuty(rotor2Duty);
+        auto duty = (int16_t) map((long) rotor2Duty, 0, 5000, 0, 1023);
+        _rotor2->setExtendedDuty(duty);
     }
 }
 
-void FlightController::setRotor3Duty(uint32_t rotor3Duty) {
+void FlightController::setRotor3Duty(int16_t rotor3Duty) {
     if (_status == DeviceStatus::Idle) {
-        _tailRotorController->setRotorDuty(rotor3Duty);
+        auto duty = (int16_t) map((long) rotor3Duty, -5000, 5000, -1023, 1023);
+        _tailRotorController->setRotorDuty(duty);
     }
 }
 

src/Logic/FlightController.hpp

@@ -3,7 +3,6 @@
 
 #include "PIDController.hpp"
 #include "Sensors/Sensors.hpp"
-#include <memory>
 
 enum DeviceStatus {
     Idle = 0,
@@ -58,11 +57,11 @@ public:
 
     [[deprecated]] void newPitchStickPosition(int16_t newPitchStickPostition);
 
-    void setRotor1Duty(uint32_t rotor1Duty);
+    void setRotor1Duty(int16_t rotor1Duty);
 
-    void setRotor2Duty(uint32_t rotor2Duty);
+    void setRotor2Duty(int16_t rotor2Duty);
 
-    void setRotor3Duty(uint32_t rotor3Duty);
+    void setRotor3Duty(int16_t rotor3Duty);
 
     void stopAllRotors();
 

src/Logic/FlightDispatcher.cpp

@@ -79,13 +79,13 @@ void FlightDispatcher::_onNewMessageReceived(char *package) {
                     _flightController->newPitchStickPosition(value.toInt16());
                     break;
                 case SH("r1"):
-                    _flightController->setRotor1Duty(value.toInt32());
+                    _flightController->setRotor1Duty(value.toInt16());
                     break;
                 case SH("r2"):
-                    _flightController->setRotor2Duty(value.toInt32());
+                    _flightController->setRotor2Duty(value.toInt16());
                     break;
                 case SH("r3"):
-                    _flightController->setRotor3Duty(value.toInt32());
+                    _flightController->setRotor3Duty(value.toInt16());
                     break;
                 case SH("stop"):
                     _flightController->stopAllRotors();

src/Logic/PIDController.hpp

@@ -9,10 +9,11 @@
 class PIDController : public SavedPidRegulator {
 public:
     PIDController(float p, float i, float d, BrushedMotor *rotor,
-                  const char *name = "PID", uint16_t dt = 10)
+                  const char *name = "PID", uint16_t dt = 10, const bool isEnabled = false)
             : SavedPidRegulator(p, i, d, name, dt) {
         assert(rotor != nullptr);
         _rotor = rotor;
+        _isEnabled = isEnabled;
         this->_dt = dt;
         setLimits(0, _rotor->maxDuty());
         _pid_timer = millis();
@@ -20,27 +21,30 @@ public:
     }
 
     void enable() {
-        _rotor->enable();
+        _isEnabled = true;
+        _rotor->setDuty(0);
     }
 
     void disable() {
-        _rotor->disable();
+        _isEnabled = false;
+        _rotor->setDuty(0);
     }
 
     void tick() {
-        if (millis() - _pid_timer >= _dt) {
+        if (millis() - _pid_timer >= _dt and _isEnabled) {
-            _rotor->setDuty((uint32_t) getResultTimer());
+            _rotor->setDuty((uint16_t) getResultTimer());
             _pid_timer = millis();
         }
     }
 
-    void setRotorDuty(uint32_t duty) {
+    void setRotorDuty(int16_t duty) {
         /* Used only for test purposes */
-        _rotor->setDuty(duty);
+        _rotor->setExtendedDuty(duty);
     }
 
 private:
     uint16_t _dt;
     BrushedMotor *_rotor = nullptr;
     uint32_t _pid_timer = 0;
+    bool _isEnabled = false;
 };

src/Motor/BrushedMotor.cpp

@@ -3,28 +3,71 @@
 
 #include "BrushedMotor.hpp"
 
-BrushedMotor::BrushedMotor(uint32_t PwmAGpioNum, uint32_t PwmBGpioNum, uint32_t PwmFreqHz,
+BrushedMotor::BrushedMotor(uint32_t pwmAGpioNum, uint32_t pwmBGpioNum, uint32_t frequency,
-                           uint32_t McpwmResolutionHz, int McpwmGroupId) {
+                           uint8_t channel, uint8_t resolution, const char *name, bool forward_directional) {
-    _mcpwmResolutionHz = McpwmResolutionHz;
+    _resolution = resolution;
-    _pwmAGpioNum = McpwmGroupId;
+    _pwmAGpioNum = pwmAGpioNum;
-    _pwmBGpioNum = PwmBGpioNum;
+    _pwmBGpioNum = pwmBGpioNum;
-    _pwmFreqHz = PwmFreqHz;
+    _channel = channel;
+    _frequency = frequency;
+    _forward_directional = forward_directional;
+    _tag_name = name;
+    pinMode(_pwmAGpioNum, OUTPUT);
+    pinMode(_pwmBGpioNum, OUTPUT);
+    ledcAttachPin(_pwmAGpioNum, _channel);
+    ledcSetup(_channel, _frequency, _resolution);
+    coast();
 }
 
-void BrushedMotor::enable() {}
+void BrushedMotor::setDuty(uint32_t duty) const {
-
+    ledcWrite(_channel, duty);
-void BrushedMotor::disable() {}
-
-void BrushedMotor::setDuty(uint16_t duty) {}
-
-uint16_t BrushedMotor::maxDuty() {
-    return 0;
 }
 
-void BrushedMotor::forward() {}
+void BrushedMotor::setExtendedDuty(int32_t duty) {
+    if (duty > 0) {
+        forward();
+    } else if (duty < 0) {
+        backward();
+    }
+    setDuty(abs(duty));
+}
 
-void BrushedMotor::reverse() {}
+uint32_t BrushedMotor::maxDuty() const {
+    return pow(2, _resolution) - 1;
+}
 
-void BrushedMotor::coast() {}
+void BrushedMotor::_apply_directional() const {
+    auto current_duty = ledcRead(_channel);
+    if (_forward_directional) {
+        ledcDetachPin(_pwmBGpioNum);
+        ledcAttachPin(_pwmAGpioNum, _channel);
+    } else {
+        ledcDetachPin(_pwmAGpioNum);
+        ledcAttachPin(_pwmBGpioNum, _channel);
+    }
+    setDuty(current_duty);
+}
 
-void BrushedMotor::brake() {}
+void BrushedMotor::forward() {
+    if (_forward_directional)
+        return;
+    ESP_LOGI(_tag_name.c_str(), "Moving forward");
+    _forward_directional = true;
+    _apply_directional();
+}
+
+void BrushedMotor::backward() {
+    if (!_forward_directional)
+        return;
+    _forward_directional = false;
+    _apply_directional();
+}
+
+void BrushedMotor::reverse() {
+    _forward_directional = !_forward_directional;
+    _apply_directional();
+}
+
+void BrushedMotor::coast() {
+    setDuty(0);
+}

src/Motor/BrushedMotor.hpp

@@ -1,40 +1,41 @@
 // This is a personal academic project. Dear PVS-Studio, please check it.
 // PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
 
-#include <stdint.h>
+#include <cstdint>
+#include "Arduino.h"
+#include "string.h"
+#include "driver/ledc.h"
 
-// TODO: implement class
 
 class BrushedMotor {
-    /* Driver for native MCPWM controller. */
+    // TODO: rewrite with MCPWM
-    // TODO: define default values
 public:
-    BrushedMotor(uint32_t PwmAGpioNum, uint32_t PwmBGpioNum, uint32_t PwmFreqHz,
+    BrushedMotor(uint32_t pwmAGpioNum, uint32_t pwmBGpioNum, uint32_t frequency,
-                 uint32_t McpwmResolutionHz, int McpwmGroupId = 0);
+                 uint8_t channel, uint8_t resolution, const char *name = "Rotor1", bool forward_directional = true);
 
-    void setDuty(uint16_t duty);
+    void setDuty(uint32_t duty) const;
 
-    uint16_t maxDuty();
+    void setExtendedDuty(int32_t duty);
 
-    void enable();
+    [[nodiscard]] uint32_t maxDuty() const;
-
-    void disable();
 
     void forward();
 
+    void backward();
+
     void reverse();
 
     void coast();
 
-    void brake();
+private:
+    uint8_t _channel;
+    uint32_t _frequency;
+    uint32_t _resolution;
 
-protected:
-    /* MCPWM group */
-    int _mcpwmGroupId = 0;
-    uint32_t _mcpwmResolutionHz;
-
-    /* Brushed motor properties */
     uint32_t _pwmAGpioNum;
     uint32_t _pwmBGpioNum;
-    uint32_t _pwmFreqHz;
+    bool _forward_directional;
+    String _tag_name = "BrushedMotor";
+
+    void _apply_directional() const;
 };

src/Sensors/BatteryController.hpp

@@ -11,7 +11,7 @@ public:
         _battery_data_switch_pin = battery_data_switch_pin;
     }
 
-    void initialize() {
+    void initialize() const {
         pinMode(_battery_data_switch_pin, OUTPUT);
         digitalWrite(_battery_data_switch_pin, HIGH);
         analogReadResolution(12);
@@ -19,19 +19,21 @@ public:
         adcAttachPin(_battery_pin);
     }
 
-    float measureVoltage() {
-        return analogRead(BATTERY_DATA_PIN) * 3.3f / 4096.f;
-    }
-
     int percent(const float minVoltage = 7.2f, const float maxVoltage = 8.4f,
-                const float r1 = 3.3f, const float r2 = 2.f, const float k = 2.87f) {
+                const float r1 = 3.f, const float r2 = 2.f, const float k = 2.85f) {
-        auto batteryVoltage = measureVoltage() * ((r1 + r2) / k);
+        // It's magic. I will return and refactor it.
-        auto percent = ((batteryVoltage - minVoltage) / (maxVoltage - minVoltage)) * 100;
+        float pinVoltage = analogReadMilliVolts(_battery_pin) / 1000.f;
-        return constrain(round(percent), 0, 100);
+        // k = (r1 + r2) / r2
+        float batteryVoltage = pinVoltage * k;
+        return constrain(_map(batteryVoltage, minVoltage, maxVoltage, 0, 100), 0, 100);
     }
 
 private:
     uint16_t _battery_pin;
     uint16_t _battery_data_switch_pin;
     static constexpr const char *_tag = "BatteryController";
+
+    static float _map(float x, float in_min, float in_max, float out_min, float out_max) {
+        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+    }
 };

src/main.cpp

@@ -21,10 +21,10 @@ void setup() {
                                 HeightSensor::RangeMeterOnly),
                     new SavedPidRegulator(3.5f, 0.0015f, 0.01f, "HeightControl"),  // height
                     new SavedPidRegulator(2.f, 12.f, 0.f, "YawControl"),  // yaw
-                    new BrushedMotor(1, 2, 3, 4, 1),
+                    new BrushedMotor(23, 16, 30000, 0, 10),
-                    new BrushedMotor(1, 2, 3, 4, 1),
+                    new BrushedMotor(26, 27, 30000, 1, 10),
                     new PIDController(0.6f, 3.5f, 0.03f,
-                                      new BrushedMotor(1, 2, 3, 4, 3),
+                                      new BrushedMotor(25, 33, 30000, 3, 10),
                                       "RollControl"),  // pitch
                     true)));
 }