robofish/move_xrobot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加了SEARCH模式

Browse Source
This commit is contained in:
Robofish 2025-01-15 23:23:01 +08:00
parent ee7ed0b056
commit 9e763bae27
8 changed files with 9322 additions and 9251 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
MDK-ARM/DevC/DevC.axf
View File

Binary file not shown.

18506
MDK-ARM/DevC/DevC.hex
View File

File diff suppressed because it is too large Load Diff

1
Middlewares/Third_Party/Protocol/protocol.h vendored
View File

@ -59,6 +59,7 @@ typedef struct __attribute__((packed)) {
float rol; /* 翻滚角（Roll angle） */
} gimbal; /* 欧拉角 */
float distance; /* 距离 */
uint8_t notice; /* 控制命令 */
struct __attribute__((packed)) {

2
User/component/cmd.c
View File

@ -221,7 +221,7 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
break;
case CMD_SW_MID:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->gimbal.mode = GIMBAL_MODE_SEARCH;
cmd->shoot.fire = false;
cmd->shoot.mode = SHOOT_MODE_LOADED;
break;

1
User/component/cmd.h
View File

@ -43,6 +43,7 @@ typedef enum {
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
GIMBAL_MODE_SEARCH, /* 搜索模式，通过云台旋转搜索目标 */
} CMD_GimbalMode_t;
/* 射击运行模式 */

22
User/device/ai.c
View File

@ -67,7 +67,7 @@ int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
ai->ai_online = true;
if (!CRC16_Verify(ai->form_host.rx_buffer, sizeof(ai->form_host.rx_buffer)))
goto error;
memcpy(&(ai->form_host.data), ai->form_host.rx_buffer, sizeof(ai->form_host.data));
memcpy(&(ai->form_host.DownPackage), ai->form_host.rx_buffer, sizeof(ai->form_host.DownPackage));
// cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
// cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
// cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
@ -75,13 +75,17 @@ int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
// cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
// cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
// cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
cmd_host->gimbal_delta.pit = ai->form_host.data.data.gimbal.pit;
cmd_host->gimbal_delta.yaw = ai->form_host.data.data.gimbal.yaw;
cmd_host->gimbal_delta.rol = ai->form_host.data.data.gimbal.rol;
cmd_host->fire = (ai->form_host.data.data.notice & AI_NOTICE_FIRE);
cmd_host->chassis_move_vec.vx = ai->form_host.data.data.chassis_move_vec.vx;
cmd_host->chassis_move_vec.vy = ai->form_host.data.data.chassis_move_vec.vy;
cmd_host->chassis_move_vec.wz = ai->form_host.data.data.chassis_move_vec.wz;
// float yaw_out = PID_Calc(&ai->pid[0], 0.0f, ai->form_host.DownPackage.data.gimbal.yaw, 0.0f , 1.0/10.0f);
// float pit_out = PID_Calc(&ai->pid[1], 0.0f, ai->form_host.DownPackage.data.gimbal.pit, 0.0f , 1.0/10.0f);
cmd_host->gimbal_delta.pit = ai->form_host.DownPackage.data.gimbal.pit;
cmd_host->gimbal_delta.yaw = ai->form_host.DownPackage.data.gimbal.yaw;
// cmd_host->gimbal_delta.pit = pit_out;
// cmd_host->gimbal_delta.yaw = yaw_out;
cmd_host->gimbal_delta.rol = ai->form_host.DownPackage.data.gimbal.rol;
cmd_host->fire = (ai->form_host.DownPackage.data.notice & AI_NOTICE_FIRE);
cmd_host->chassis_move_vec.vx = ai->form_host.DownPackage.data.chassis_move_vec.vx;
cmd_host->chassis_move_vec.vy = ai->form_host.DownPackage.data.chassis_move_vec.vy;
cmd_host->chassis_move_vec.wz = ai->form_host.DownPackage.data.chassis_move_vec.wz;
return DEVICE_OK;
@ -95,7 +99,7 @@ int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
if (cmd_host == NULL) return DEVICE_ERR_NULL;
ai->ai_online = false;
memset(ai->form_host.rx_buffer, 0, sizeof(ai->form_host.rx_buffer));
memset(&(ai->form_host.data), 0, sizeof(ai->form_host.data));
memset(&(ai->form_host.DownPackage), 0, sizeof(ai->form_host.DownPackage));
memset(cmd_host, 0, sizeof(*cmd_host));
return 0;
}

2
User/device/ai.h
View File

@ -38,7 +38,7 @@ typedef struct __packed {
osThreadId_t thread_alert;
struct {
uint8_t rx_buffer[sizeof(Protocol_DownPackage_t)];
Protocol_DownPackage_t data;
Protocol_DownPackage_t DownPackage;
}form_host;
struct {

37
User/module/gimbal.c
View File

@ -9,6 +9,9 @@
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define YAW_DELTA_MAX (5.7f + M_PI)
#define YAW_DELTA_MIN (1.8f + M_PI)
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
@ -39,6 +42,8 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
g->setpoint.eulr.yaw = g->feedback.eulr.imu.yaw;
} else if (mode == GIMBAL_MODE_RELATIVE) {
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
} else if (mode == GIMBAL_MODE_SEARCH) {
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
}
}
@ -138,6 +143,18 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
}
/* 处理yaw控制命令 */
/* 处理yaw控制命令，添加yaw限位 */
const float yaw_delta_max =
CircleError(YAW_DELTA_MAX,
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
g->feedback.eulr.imu.yaw),
M_2PI);
const float yaw_delta_min =
CircleError(YAW_DELTA_MIN,
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
g->feedback.eulr.imu.yaw),
M_2PI);
Clip(&(g_cmd->delta_eulr.yaw), yaw_delta_min, yaw_delta_max);
CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
/* 处理pitch控制命令，软件限位 */
@ -197,6 +214,26 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
case GIMBAL_MODE_RELATIVE:
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
break;
case GIMBAL_MODE_SEARCH:
// g->setpoint.eulr.yaw = YAW_DELTA_MIN + (YAW_DELTA_MAX - YAW_DELTA_MIN) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
g->setpoint.eulr.yaw = YAW_DELTA_MIN + (YAW_DELTA_MAX - YAW_DELTA_MIN) * (1.0f - sinf(now / 500.0f)) / 2.0f; // 在min和max之间摆动
g->setpoint.eulr.pit = g->limit.min + (g->limit.max - g->limit.min) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
yaw_omega_set_point =
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
g->feedback.eulr.encoder.yaw, 0.0f, g->dt);
g->out[GIMBAL_ACTR_YAW_IDX] =
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
g->feedback.gyro.z, 0.f, g->dt);
pit_omega_set_point =
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
g->feedback.eulr.encoder.pit, 0.0f, g->dt);
g->out[GIMBAL_ACTR_PIT_IDX] =
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
0.0, 0.f, g->dt);
break;
}
/* 输出滤波 */