ArmDev/motor-controller
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[feat] project add deadzone

Browse Source
This commit is contained in:
robinson
2026-05-22 00:12:16 +08:00
parent bbe8bd06bc
commit 9171374490
10 changed files with 188 additions and 202 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
app/control_task.c
View File

@@ -12,6 +12,7 @@
#include "app.h"
#include "cmsis_os2.h"
#include "dead_zone.h"
#include "mf4010v2.h"
#include "pid.h"
#include <stdio.h>
@@ -27,8 +28,8 @@ static float g_target_pitch = 0.0f; /* Pitch 目标 (deg) */
static float g_target_yaw = 0.0f; /* Yaw 目标 (deg) */
/* 电机实例 */
static mf4010v2_t motor_yaw; /* Yaw 轴电机（外框，补偿 Roll） */
static mf4010v2_t motor_pitch; /* Pitch 轴电机（内框，补偿 Pitch） */
static mf4010v2_t motor_1; /* Yaw 轴电机（外框，补偿 Roll） */
static mf4010v2_t motor_2; /* Pitch 轴电机（内框，补偿 Pitch） */
/* PID 控制器 */
static pid_t g_roll_pid; /* Roll 轴 PID */
@@ -36,6 +37,7 @@ static pid_t g_pitch_pid; /* Pitch 轴 PID */
static lowpass_t g_roll_lowpass;
static lowpass_t g_pitch_lowpass;
static dead_zone_t g_yaw_deadzone;
/* 电机使能状态 — 由 callback_task 通过 UART 命令控制 */
static bool g_motor_enabled[2] = {false, false};
@@ -63,20 +65,19 @@ static bool g_motor_enabled[2] = {false, false};
* Kd: 抑制振荡的微分系数
* 例如 Kd=50, 则角速度变化率反馈阻尼
*/
#define ROLL_KP 200.0f
#define ROLL_KI 5.0f
#define ROLL_KD 50.0f
#define ROLL_KP 50.0f
#define ROLL_KI 0.0f
#define ROLL_KD 0.0f
#define ROLL_ALPHA 0.7f
#define PITCH_KP 200.0f
#define PITCH_KI 5.0f
#define PITCH_KD 50.0f
#define PITCH_KP 50.0f
#define PITCH_KI 0.0f
#define PITCH_KD 0.0f
#define PITCH_ALPHA 0.7f
/* ============================================================================
* 控制任务（速度控制模式）
* ============================================================================ */
void control_task(void)
{
imu_data_t imu;
@@ -102,29 +103,30 @@ void control_task(void)
lowpass_init(&g_roll_lowpass, ROLL_ALPHA);
lowpass_init(&g_pitch_lowpass, PITCH_ALPHA);
dead_zone_init(&g_yaw_deadzone, 0.2f);
log_printf("[control_task] PID: Roll(Kp=%.1f Ki=%.1f Kd=%.1f) Pitch(Kp=%.1f Ki=%.1f Kd=%.1f)\n",
ROLL_KP, ROLL_KI, ROLL_KD, PITCH_KP, PITCH_KI, PITCH_KD);
/* 2. 初始化电机 */
mf4010v2_init(&motor_yaw, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */
mf4010v2_init(&motor_pitch, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */
mf4010v2_init(&motor_1, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */
mf4010v2_init(&motor_2, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */
/* 3. 电机关机状态启动 */
mf4010v2_close(&motor_yaw);
mf4010v2_close(&motor_pitch);
mf4010v2_close(&motor_1);
mf4010v2_close(&motor_2);
log_printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_yaw));
log_printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_pitch));
log_printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_1));
log_printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_2));
/* 等待 IMU 稳定 */
osDelay(pdMS_TO_TICKS(5000));
/* 4. 使能电机（零速启动） */
mf4010v2_set_speed(&motor_yaw, 0);
mf4010v2_set_speed(&motor_pitch, 0);
mf4010v2_run(&motor_yaw);
mf4010v2_run(&motor_pitch);
mf4010v2_set_speed(&motor_1, 0);
mf4010v2_set_speed(&motor_2, 0);
mf4010v2_run(&motor_1);
mf4010v2_run(&motor_2);
log_printf("[control_task] stabilization enabled (speed mode)\n");
@@ -144,30 +146,40 @@ void control_task(void)
if (qstat == osOK) {
imu.angle[0] = lowpass_update(&g_roll_lowpass, imu.angle[0]);
imu.angle[1] = lowpass_update(&g_pitch_lowpass, imu.angle[1]);
imu.angle[0] = dead_zone_apply(&g_yaw_deadzone, imu.angle[0]);
/* ---- Roll 轴速度控制 ---- */
float roll_speed = pid_compute(&g_roll_pid,
g_target_roll,
imu.angle[0]); /* Roll */
int32_t yaw_speed_cmd = (int32_t)roll_speed;
int32_t motor_1_speed_cmd = (int32_t)roll_speed;
/* ---- Pitch 轴速度控制 ---- */
float pitch_speed = pid_compute(&g_pitch_pid,
g_target_pitch,
imu.angle[1]); /* Pitch */
int32_t pitch_speed_cmd = (int32_t)pitch_speed;
yaw_speed_cmd = yaw_speed_cmd > 36000 ? 36000 : (yaw_speed_cmd < -36000 ? -36000 : yaw_speed_cmd);
pitch_speed_cmd = pitch_speed_cmd > 36000 ? 36000 : (pitch_speed_cmd < -36000 ? -36000 : pitch_speed_cmd);
int32_t motor_2_speed_cmd = (int32_t)pitch_speed;
motor_1_speed_cmd = motor_1_speed_cmd > 36000 ? 36000 : (motor_1_speed_cmd < -36000 ? -36000 : motor_1_speed_cmd);
motor_2_speed_cmd = motor_2_speed_cmd > 36000 ? 36000 : (motor_2_speed_cmd < -36000 ? -36000 : motor_2_speed_cmd);
if (g_motor_enabled[GIMBAL_MOTOR_YAW]) {
mf4010v2_set_speed(&motor_yaw, yaw_speed_cmd);
if (imu.angle[0] < 1e-6f && imu.angle[0] > -1e-6f) {
mf4010v2_set_speed(&motor_1, 0);
log_printf("[control_task] motor 1: %d,", 0);
pid_reset(&g_roll_pid);
} else {
mf4010v2_set_speed(&motor_1, motor_1_speed_cmd);
log_printf("[control_task] motor 1: %ld,", motor_1_speed_cmd);
}
} else {
mf4010v2_close(&motor_yaw);
log_printf("[control_task] motor 1: %d,", 0);
mf4010v2_close(&motor_1);
}
if (g_motor_enabled[GIMBAL_MOTOR_PITCH]) {
mf4010v2_set_speed(&motor_pitch, pitch_speed_cmd);
mf4010v2_set_speed(&motor_2, motor_2_speed_cmd);
log_printf("motor 2: %ld\r\n", motor_2_speed_cmd);
} else {
mf4010v2_close(&motor_pitch);
mf4010v2_close(&motor_2);
log_printf(" motor 2: %d\r\n", 0);
}
}
@@ -212,7 +224,7 @@ void gimbal_motor_disable(void)
void gimbal_motor_enable_id(int id)
{
g_motor_enabled[id] = true;
mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_yaw : &motor_pitch;
mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_1 : &motor_2;
pid_t *pid = (id == GIMBAL_MOTOR_YAW) ? &g_roll_pid : &g_pitch_pid;
mf4010v2_set_speed(motor, 0);
mf4010v2_run(motor);
@@ -223,7 +235,7 @@ void gimbal_motor_enable_id(int id)
void gimbal_motor_disable_id(int id)
{
g_motor_enabled[id] = false;
mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_yaw : &motor_pitch;
mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_1 : &motor_2;
mf4010v2_close(motor);
}