From 47f91eeae5a3e301474110671444a2f52b8ac6fa Mon Sep 17 00:00:00 2001 From: gogacoder Date: Tue, 19 Mar 2024 19:10:52 +0700 Subject: [PATCH] Brushed motors drivers were implemented --- src/Logic/FlightController.cpp | 20 ++++----- src/Logic/FlightController.hpp | 7 ++- src/Logic/FlightDispatcher.cpp | 6 +-- src/Logic/PIDController.hpp | 18 +++++--- src/Motor/BrushedMotor.cpp | 79 ++++++++++++++++++++++++++-------- src/Motor/BrushedMotor.hpp | 39 +++++++++-------- src/main.cpp | 6 +-- 7 files changed, 111 insertions(+), 64 deletions(-) diff --git a/src/Logic/FlightController.cpp b/src/Logic/FlightController.cpp index 8d5b108..d972e5c 100644 --- a/src/Logic/FlightController.cpp +++ b/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); } } diff --git a/src/Logic/FlightController.hpp b/src/Logic/FlightController.hpp index b3b5550..95b4ec2 100644 --- a/src/Logic/FlightController.hpp +++ b/src/Logic/FlightController.hpp @@ -3,7 +3,6 @@ #include "PIDController.hpp" #include "Sensors/Sensors.hpp" -#include 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(); diff --git a/src/Logic/FlightDispatcher.cpp b/src/Logic/FlightDispatcher.cpp index e43b2d9..81dd715 100644 --- a/src/Logic/FlightDispatcher.cpp +++ b/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(); diff --git a/src/Logic/PIDController.hpp b/src/Logic/PIDController.hpp index 74c736c..3e93809 100644 --- a/src/Logic/PIDController.hpp +++ b/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) { - _rotor->setDuty((uint32_t) getResultTimer()); + if (millis() - _pid_timer >= _dt and _isEnabled) { + _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; }; \ No newline at end of file diff --git a/src/Motor/BrushedMotor.cpp b/src/Motor/BrushedMotor.cpp index 89f3556..d0f929a 100644 --- a/src/Motor/BrushedMotor.cpp +++ b/src/Motor/BrushedMotor.cpp @@ -3,28 +3,71 @@ #include "BrushedMotor.hpp" -BrushedMotor::BrushedMotor(uint32_t PwmAGpioNum, uint32_t PwmBGpioNum, uint32_t PwmFreqHz, - uint32_t McpwmResolutionHz, int McpwmGroupId) { - _mcpwmResolutionHz = McpwmResolutionHz; - _pwmAGpioNum = McpwmGroupId; - _pwmBGpioNum = PwmBGpioNum; - _pwmFreqHz = PwmFreqHz; +BrushedMotor::BrushedMotor(uint32_t pwmAGpioNum, uint32_t pwmBGpioNum, uint32_t frequency, + uint8_t channel, uint8_t resolution, const char *name, bool forward_directional) { + _resolution = resolution; + _pwmAGpioNum = pwmAGpioNum; + _pwmBGpioNum = pwmBGpioNum; + _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::disable() {} - -void BrushedMotor::setDuty(uint16_t duty) {} - -uint16_t BrushedMotor::maxDuty() { - return 0; +void BrushedMotor::setDuty(uint32_t duty) const { + ledcWrite(_channel, duty); } -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() {} \ No newline at end of file +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); +} diff --git a/src/Motor/BrushedMotor.hpp b/src/Motor/BrushedMotor.hpp index 65ddd22..0de722a 100644 --- a/src/Motor/BrushedMotor.hpp +++ b/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 +#include +#include "Arduino.h" +#include "string.h" +#include "driver/ledc.h" -// TODO: implement class class BrushedMotor { - /* Driver for native MCPWM controller. */ - // TODO: define default values + // TODO: rewrite with MCPWM public: - BrushedMotor(uint32_t PwmAGpioNum, uint32_t PwmBGpioNum, uint32_t PwmFreqHz, - uint32_t McpwmResolutionHz, int McpwmGroupId = 0); + BrushedMotor(uint32_t pwmAGpioNum, uint32_t pwmBGpioNum, uint32_t frequency, + 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(); - - void disable(); + [[nodiscard]] uint32_t maxDuty() const; 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; }; \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 4823c9d..7510c1e 100644 --- a/src/main.cpp +++ b/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(1, 2, 3, 4, 1), + new BrushedMotor(23, 16, 30000, 0, 10), + 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))); }