Mirrors_Framework/RBF-PID-Balbot-firmware
1
0
Fork 0
mirror of https://github.com/Dictor/RBF-PID-Balbot-firmware.git synced 2025-12-11 12:57:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tuning parameters

Browse Source
This commit is contained in:
Dictor
2023-11-17 02:04:39 +09:00
parent d615202b0b
commit b3774846b4
4 changed files with 13 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
app.overlay
View File

@@ -5,10 +5,10 @@
status = "okay";
irq-gpios = <&gpiob 1 GPIO_ACTIVE_HIGH>;
gyro-sr-div = <0>;
gyro-dlpf = <250>;
gyro-dlpf = <5>;
gyro-fs = <250>;
accel-fs = <2>;
accel-dlpf = "99";
accel-dlpf = "5.05";
};
};

8
src/app_main.cpp
View File

@@ -34,12 +34,12 @@ void AppMain(void) {
LOG_INF("RBF-PID Balbot");
const int dt_ms = 2;
const double u_motor_factor = 2500;
const double u_motor_factor = 500;
std::array<double, 3> d_accel, d_gyro, d_magn, euler;
std::array<float, 3> f_accel, f_gyro, f_magn;
std::array<float, 4> quad;
posture::MahonyAHRS mahony((float)dt_ms / 1000, 100, 500);
control::RBFPID pid(3, 8, pow(10, 6), 0.01, 10000, 0.01, 100);
posture::MahonyAHRS mahony((float)dt_ms / 1000, 100, 100);
control::RBFPID pid(3, 8, pow(10, 4), 100, 300, 10, u_motor_factor * 0.9);
long i = 0;
double u, uf;
char telemetry[100];
@@ -68,7 +68,7 @@ void AppMain(void) {
u = pid.Update(-euler[0], euler[0]);
uf = (u / u_motor_factor); //+ (u >= 0 ? 0.3 : -0.3);
hardware::SetMotor(false, uf);
if (i % 10 == 0) {
if (i % 50 == 0) {
LOG_INF("e %6f u %6f uf %6f", -euler[0], u, uf);
gpio_pin_toggle_dt(&hardware::run_led);
gain = pid.ReadGain();

2
src/hardware.cpp
View File

@@ -58,7 +58,7 @@ int hardware::InitHardware() {
gpio_pin_set_dt(&hardware::m_mode, 1); // m_mode is active low
struct uart_config uart_cfg = {
.baudrate = 115200,
.baudrate = 9600,
.parity = UART_CFG_PARITY_NONE,
.stop_bits = UART_CFG_STOP_BITS_1,
.data_bits = UART_CFG_DATA_BITS_8,

8
src/rbfpid.cpp
View File

@@ -98,7 +98,11 @@ double RBFPID::Update(double e, double y) {
/* update PID gain */
kp_ = kp_ + gain_mg_ * mg_ * ek_ * J_ * xc1_;
ki_ = ki_ + gain_mg_ * mg_ * ek_ * J_ * xc2_;
kd_ = kd_ + gain_mg_ * ek_ * J_ * xc3_;
kd_ = kd_ + gain_mg_ * mg_ * ek_ * J_ * xc3_;
if (kp_ < 0) kp_ = 0;
if (ki_ < 0) ki_ = 0;
if (kd_ < 0) ki_ = 0;
/* calculate pid output */
du_ = kp_ * xc1_ + ki_ * xc2_ + kd_ * xc3_;
@@ -122,5 +126,5 @@ double RBFPID::Update(double e, double y) {
}
std::tuple<double, double, double> RBFPID::ReadGain() {
return std::make_tuple(kp_, kd_, ki_);
return std::make_tuple(kp_, ki_, kd_);
}