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

55
app/callback_task.c
View File

@@ -20,11 +20,11 @@ static struct {
/*
* 支持的命令（格式：@command\n）:
* "motor enable" — 使能全部电机（零速启动）
* "motor enable yaw" — 使能 Yaw 电机
* "motor enable pitch" — 使能 Pitch 电机
* "motor enable 1" — 使能电机 1
* "motor enable 2" — 使能电机 2
* "motor disable" — 关闭全部电机（惰行 coast）
* "motor disable yaw" — 关闭 Yaw 电机
* "motor disable pitch" — 关闭 Pitch 电机
* "motor disable 1" — 关闭电机 1
* "motor disable 2" — 关闭电机 2
* "sensor enable" — 恢复 IMU 读取
* "sensor disable" — 暂停 IMU 读取
* "sensorcali acc enable" — 开始加速度计标定
@@ -98,8 +98,8 @@ void callback_task(void)
if (strcmp(cmd_buf, "help") == 0) {
log_printf("[cmd] available commands:\r\n"
" help ==> show this message\r\n"
" motor enable [yaw|pitch] ==> enable motor(s), default both\r\n"
" motor disable [yaw|pitch] ==> disable motor(s), default both\r\n"
" motor enable [1|2] ==> enable motor(s), default both\r\n"
" motor disable [1|2] ==> disable motor(s), default both\r\n"
" sensor enable ==> resume IMU data publishing\r\n"
" sensor disable ==> pause IMU data publishing\r\n"
" sensorcali acc enable ==> start accelerometer calibration\r\n"
@@ -107,20 +107,20 @@ void callback_task(void)
" sensorcali mag enable ==> start magnetometer calibration\r\n"
" sensorcali mag disable ==> stop magnetometer calibration\r\n"
" pid ==> show all PID coefficients\r\n"
" pid <axis> <p> <v> ==> set PID param (v÷10=实际值, motor disabled)\r\n"
" axis: roll|pitch, p: kp|ki|kd, v: float\r\n");
" pid <motor> <p> <v> ==> set PID param (v÷10=实际值, motor disabled)\r\n"
" motor: 1|2, p: kp|ki|kd, v: float\r\n");
} else if (strncmp(cmd_buf, "motor enable", 12) == 0) {
const char *p = cmd_buf + 12;
while (*p == ' ') p++;
if (*p == '\0') {
gimbal_motor_enable();
log_printf("[cmd] both motors enabled\r\n");
} else if (strcmp(p, "yaw") == 0) {
} else if (strcmp(p, "1") == 0) {
gimbal_motor_enable_id(GIMBAL_MOTOR_YAW);
log_printf("[cmd] yaw motor enabled\r\n");
} else if (strcmp(p, "pitch") == 0) {
log_printf("[cmd] motor 1 enabled\r\n");
} else if (strcmp(p, "2") == 0) {
gimbal_motor_enable_id(GIMBAL_MOTOR_PITCH);
log_printf("[cmd] pitch motor enabled\r\n");
log_printf("[cmd] motor 2 enabled\r\n");
} else {
log_printf("[cmd] unknown motor: %s\r\n", p);
}
@@ -130,12 +130,12 @@ void callback_task(void)
if (*p == '\0') {
gimbal_motor_disable();
log_printf("[cmd] both motors disabled\r\n");
} else if (strcmp(p, "yaw") == 0) {
} else if (strcmp(p, "1") == 0) {
gimbal_motor_disable_id(GIMBAL_MOTOR_YAW);
log_printf("[cmd] yaw motor disabled\r\n");
} else if (strcmp(p, "pitch") == 0) {
log_printf("[cmd] motor 1 disabled\r\n");
} else if (strcmp(p, "2") == 0) {
gimbal_motor_disable_id(GIMBAL_MOTOR_PITCH);
log_printf("[cmd] pitch motor disabled\r\n");
log_printf("[cmd] motor 2 disabled\r\n");
} else {
log_printf("[cmd] unknown motor: %s\r\n", p);
}
@@ -161,28 +161,31 @@ void callback_task(void)
/* 查看当前 PID 系数（整数显示，值 = 实际系数 × 10） */
float kp, ki, kd;
gimbal_get_pid("roll", &kp, &ki, &kd);
log_printf("[cmd] Roll: Kp=%d Ki=%d Kd=%d\r\n",
log_printf("[cmd] Motor 1: Kp=%d Ki=%d Kd=%d\r\n",
(int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f));
gimbal_get_pid("pitch", &kp, &ki, &kd);
log_printf("[cmd] Pitch: Kp=%d Ki=%d Kd=%d\r\n",
log_printf("[cmd] Motor 2: Kp=%d Ki=%d Kd=%d\r\n",
(int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f));
} else if (strncmp(cmd_buf, "pid ", 4) == 0) {
/* 调参：pid <roll|pitch> <kp|ki|kd> <value> （value ÷ 10 = 实际系数） */
char axis[8], param[8];
int int_val;
if (sscanf(cmd_buf, "pid %7s %7s %d", axis, param, &int_val) == 3) {
if (gimbal_is_motor_enabled()) {
/* 调参：pid <1|2> <kp|ki|kd> <value> （value ÷ 10 = 实际系数） */
char param[8];
int motor_id, int_val;
if (sscanf(cmd_buf, "pid %d %7s %d", &motor_id, param, &int_val) == 3) {
if (motor_id < 1 || motor_id > 2) {
log_printf("[cmd] usage: pid <1|2> <kp|ki|kd> <value>\r\n");
} else if (gimbal_is_motor_enabled()) {
log_printf("[cmd] ERROR: disable motor first (motor disable)\r\n");
} else {
const char *axis = (motor_id == 1) ? "roll" : "pitch";
float value = (float)int_val / 10.0f;
if (gimbal_set_pid(axis, param, value) == 0) {
log_printf("[cmd] pid.%s.%s = %d\r\n", axis, param, int_val);
log_printf("[cmd] motor %d.%s = %d\r\n", motor_id, param, int_val);
} else {
log_printf("[cmd] usage: pid <roll|pitch> <kp|ki|kd> <value>\r\n");
log_printf("[cmd] usage: pid <1|2> <kp|ki|kd> <value>\r\n");
}
}
} else {
log_printf("[cmd] usage: pid <roll|pitch> <kp|ki|kd> <value>\r\n");
log_printf("[cmd] usage: pid <1|2> <kp|ki|kd> <value>\r\n");
}
} else {
log_printf("[cmd] unknown: %s\r\n", cmd_buf);

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

2
app/sensor_task.c
View File

@@ -116,7 +116,7 @@ void sensor_task(void)
if(g_sensor_enabled) {
WitReadReg(AX, 13);
}
last_tick += pdMS_TO_TICKS(2); /* 500Hz 轮询 */
last_tick += pdMS_TO_TICKS(5); /* 500Hz 轮询 */
if(g_sensor_enabled && s_cDataUpdate)
{
for(i = 0; i < 3; i++)

2
modules/control/pid/pid.c
View File

@@ -15,7 +15,6 @@ void pid_init(pid_t *pid)
float pid_compute(pid_t *pid, float target, float actual)
{
if (!pid) return 0.0f;
float error = target - actual;
/* 比例项 */
@@ -34,7 +33,6 @@ float pid_compute(pid_t *pid, float target, float actual)
pid->integral = -pid->integral_max;
}
float i_term = pid->Ki * pid->integral;
float output = p_term + i_term + d_term;
/* 输出限幅（0=不限幅） */

68
modules/device/motor/mf4010v2.c
View File

@@ -63,10 +63,8 @@ int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8])
int ret = can_if_send(motor->can_ch, &msg);
if (ret != 0) {
motor->error_count++;
// printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id);
return MF4010_ERR_CAN_SEND;
}
/* 轮询等待电机响应（带超时） */
for (int retry = 0; retry < MF4010_RECV_RETRIES; retry++) {
ret = can_if_recv(motor->can_ch, &msg);
@@ -81,8 +79,10 @@ int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8])
printf("Error: Timeout waiting for motor response (ID 0x%03X)\r\n", motor->id);
return MF4010_ERR_NO_RESPONSE;
}
memcpy(motor->last_response, msg.data, 8);
if (motor->last_response[0] != command[0]) {
return MF4010_ERR_BAD_RECV;
}
motor->success_count++;
return MF4010_OK;
}
@@ -168,6 +168,41 @@ void mf4010v2_close(mf4010v2_t* motor) {
}
}
// int32_t mf4010v2_get_speed(mf4010v2_t* motor) {
// can_message_t msg;
// if (!motor) return MF4010_ERR_INVALID_PARAM;
// msg.id = motor->id;
// uint8_t commands = [0x9c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00];
// mf4010v2_send_command(motor, commands);
// uint8_t = motor->last_response
// }
/* ============================================================================
* 非阻塞发送 — fire-and-forget，不轮询 CAN 响应
* ============================================================================ */
static int send_cmd_nb(mf4010v2_t* motor, const uint8_t command[8])
{
can_message_t msg;
if (!motor) return MF4010_ERR_INVALID_PARAM;
msg.id = motor->id;
memcpy(msg.data, command, 8);
msg.length = 8;
return can_if_send(motor->can_ch, &msg);
}
int mf4010v2_set_speed_nb(mf4010v2_t* motor, int32_t speed_0_01dps)
{
if (!motor) return MF4010_ERR_INVALID_PARAM;
uint8_t cmd[8];
cmd_speed_control(cmd, speed_0_01dps);
return send_cmd_nb(motor, cmd);
}
int mf4010v2_close_nb(mf4010v2_t* motor)
{
if (!motor) return MF4010_ERR_INVALID_PARAM;
uint8_t cmd[8] = COMMAND_MOTOR_CLOSE;
return send_cmd_nb(motor, cmd);
}
int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) {
if (!motor) return MF4010_ERR_INVALID_PARAM;
@@ -205,10 +240,10 @@ void cmd_speed_control(uint8_t* buf, int32_t speed)
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = (uint8_t)(speed & 0xFF);
buf[5] = (uint8_t)((speed >> 8) & 0xFF);
buf[6] = (uint8_t)((speed >> 16) & 0xFF);
buf[7] = (uint8_t)((speed >> 24) & 0xFF);
buf[4] = *(uint8_t *)(&speed);
buf[5] = *((uint8_t *)(&speed)+1);
buf[6] = *((uint8_t *)(&speed)+2);
buf[7] = *((uint8_t *)(&speed)+3);
}
@@ -337,3 +372,22 @@ uint8_t mf4010v2_get_error_code(mf4010v2_t* motor)
{
return (motor != NULL) ? (motor->last_response[0] & 0x7F) : 0;
}
int mf4010v2_get_motor_state(mf4010v2_t* motor, int8_t* tempture, int16_t* iq, int16_t* speed, int16_t* encoder) {
uint8_t command[] = {0x9c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
mf4010v2_send_command(motor, command);
if (motor->last_response[0] != 0x9c) {
motor->flags |= MF4010_FLAG_FAULT;
return -1;
}
motor->flags &= ~MF4010_FLAG_FAULT;
if (tempture != NULL)
*tempture = *(int8_t*)(motor->last_response + 1);
if (iq != NULL)
*iq = (int16_t)(motor->last_response[2] | (motor->last_response[3] << 8));
if(speed != NULL)
*speed = (int16_t)(motor->last_response[4] | (motor->last_response[5] << 8));
if (encoder != NULL)
*encoder = (int16_t)(motor->last_response[6] | (motor->last_response[7] << 8));
return 0;
}

9
modules/device/motor/mf4010v2.h
View File

@@ -41,7 +41,8 @@ typedef enum {
MF4010_ERR_CRC = -6, /**< CRC 校验错误 */
MF4010_ERR_HARDWARE = -7, /**< 硬件错误 */
MF4010_ERR_CAN_SEND = -8, /**< CAN 发送失败 */
MF4010_ERR_CAN_RECV = -9 /**< CAN 接收失败 */
MF4010_ERR_CAN_RECV = -9, /**< CAN 接收失败 */
MF4010_ERR_BAD_RECV = -10
} mf4010_status_t;
/**
@@ -115,7 +116,11 @@ void mf4010v2_close(mf4010v2_t* motor);
int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps);
int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg);
int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid);
int32_t mf4010v2_get_speed(mf4010v2_t* motor);
int mf4010v2_get_motor_state(mf4010v2_t* motor, int8_t* tempture, int16_t* iq, int16_t* speed, int16_t* encoder);
/* Non-blocking variants — fire-and-forget, no CAN response polling */
int mf4010v2_set_speed_nb(mf4010v2_t* motor, int32_t speed_0_01dps);
int mf4010v2_close_nb(mf4010v2_t* motor);
/* ============================================================================
* 命令宏定义（8 字节 CAN 命令帧）

2
release

Submodule release updated: 2483e67f90...86fdbcb85a