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

[release] version 1.0.0

Browse Source
This commit is contained in:
robinson
2026-05-23 00:16:40 +08:00
parent 9171374490
commit 361caca427
7 changed files with 164 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
app/control_task.c
View File

@@ -15,6 +15,7 @@
#include "dead_zone.h"
#include "mf4010v2.h"
#include "pid.h"
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include "lowpass.h"
@@ -51,7 +52,7 @@ static bool g_motor_enabled[2] = {false, false};
* 电机 MF4010V2 速度范围：±204800 (即 ±2048 DPS)
* 稳像场景使用较低速度：建议 ±36000 (即 ±360 DPS) 作为输出上限
*/
#define SPEED_OUTPUT_MAX 36000.0f /* 最大角速度 360 DPS (0.01 DPS) */
#define SPEED_OUTPUT_MAX 204800.0f /* 最大角速度 360 DPS (0.01 DPS) */
/*
* PID 参数说明（速度控制模式）:
@@ -65,19 +66,66 @@ static bool g_motor_enabled[2] = {false, false};
* Kd: 抑制振荡的微分系数
* 例如 Kd=50, 则角速度变化率反馈阻尼
*/
#define ROLL_KP 50.0f
#define ROLL_KP 1020.0f
#define ROLL_KI 0.0f
#define ROLL_KD 0.0f
#define ROLL_KD 20.0f
#define ROLL_ALPHA 0.7f
#define PITCH_KP 50.0f
#define PITCH_KP 1020.0f
#define PITCH_KI 0.0f
#define PITCH_KD 0.0f
#define PITCH_KD 20.0f
#define PITCH_ALPHA 0.7f
/* ============================================================================
* 控制任务（速度控制模式）
* ============================================================================ */
// void control_task(void) {
// log_printf("[control_task] starting stabilization control (speed mode)\n");
// // mf4010v2_init(&motor_1, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */
// mf4010v2_init(&motor_2, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */
// // mf4010v2_close(&motor_1);
// mf4010v2_close(&motor_2);
// osDelay(pdMS_TO_TICKS(5000));
// // mf4010v2_run(&motor_1);
// mf4010v2_run(&motor_2);
// // mf4010v2_set_speed(&motor_1, 0);
// mf4010v2_set_speed(&motor_2, 0);
// // int8_t tempture;
// int16_t speed;
// int ret;
// int aim_speed;
// while(true) {
// aim_speed = 500;
// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL);
// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret);
// ret = mf4010v2_set_speed(&motor_2, aim_speed);
// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret);
// osDelay(pdMS_TO_TICKS(2000));
// aim_speed = 1000;
// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL);
// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret);
// ret = mf4010v2_set_speed(&motor_2, aim_speed);
// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret);
// osDelay(pdMS_TO_TICKS(2000));
// aim_speed = -1000;
// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL);
// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret);
// ret = mf4010v2_set_speed(&motor_2, aim_speed);
// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret);
// osDelay(pdMS_TO_TICKS(2000));
// aim_speed = -500;
// ret = mf4010v2_get_motor_state(&motor_2, NULL, NULL, &speed, NULL);
// log_printf("[control_task] speed: %d, ret = %d\r\n", speed, ret);
// ret = mf4010v2_set_speed(&motor_2, aim_speed);
// log_printf("[control_task] aim_speed: %d, ret = %d\r\n", aim_speed, ret);
// osDelay(pdMS_TO_TICKS(2000));
// }
// }
void control_task(void)
{
imu_data_t imu;
@@ -139,7 +187,7 @@ void control_task(void)
* 3. 速度指令发送到电机（电机内部完成速度闭环）
* ======================================================================== */
while (1) {
last_tick += pdMS_TO_TICKS(20); /* 100Hz 周期 */
last_tick += pdMS_TO_TICKS(20); /* 50Hz 周期 */
/* 从 sensor_task 获取最新 IMU 数据（不等待，由定时保证节奏） */
qstat = osMessageQueueGet(g_imu_queue, &imu, NULL, 0);
@@ -158,28 +206,26 @@ void control_task(void)
g_target_pitch,
imu.angle[1]); /* Pitch */
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);
log_printf("[control_task:motor] %d, %d", (int)(imu.angle[0]*10), (int)(imu.angle[1]*10));
if (g_motor_enabled[GIMBAL_MOTOR_YAW]) {
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);
mf4010v2_set_speed_nb(&motor_1, 0);
log_printf(" 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);
mf4010v2_set_speed_nb(&motor_1, motor_1_speed_cmd);
log_printf(" 1: %ld,", motor_1_speed_cmd);
}
} else {
log_printf("[control_task] motor 1: %d,", 0);
mf4010v2_close(&motor_1);
log_printf(" 1: %d,", 0);
mf4010v2_close_nb(&motor_1);
}
if (g_motor_enabled[GIMBAL_MOTOR_PITCH]) {
mf4010v2_set_speed(&motor_2, motor_2_speed_cmd);
log_printf("motor 2: %ld\r\n", motor_2_speed_cmd);
mf4010v2_set_speed_nb(&motor_2, motor_2_speed_cmd);
log_printf(" 2: %ld\r\n", motor_2_speed_cmd);
} else {
mf4010v2_close(&motor_2);
log_printf(" motor 2: %d\r\n", 0);
mf4010v2_close_nb(&motor_2);
log_printf(" 2: %d\r\n", 0);
}
}