Helicopter/firmware
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Brushed motors drivers were implemented

Browse Source
This commit is contained in:
Goga Coder
2024-03-19 19:10:52 +07:00
parent b58379ac75
commit 47f91eeae5
7 changed files with 111 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/Logic/FlightController.cpp
View File

@@ -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);
}
}

7
src/Logic/FlightController.hpp
View File

@@ -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();

6
src/Logic/FlightDispatcher.cpp
View File

@@ -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();

18
src/Logic/PIDController.hpp
View File

@@ -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;
};

79
src/Motor/BrushedMotor.cpp
View File

@@ -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() {}
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);
}

39
src/Motor/BrushedMotor.hpp
View File

@@ -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: 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;
};

6
src/main.cpp
View File

@@ -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)));
}