diff --git a/assets/User_code/bsp/gpio/gpio.c b/assets/User_code/bsp/gpio/gpio.c index 2950620..5c3113c 100644 --- a/assets/User_code/bsp/gpio/gpio.c +++ b/assets/User_code/bsp/gpio/gpio.c @@ -69,6 +69,7 @@ int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) { default: return BSP_ERR; } + return BSP_OK; } int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) { @@ -77,6 +78,7 @@ int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) { default: return BSP_ERR; } + return BSP_OK; } int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){ if (gpio >= BSP_GPIO_NUM) return BSP_ERR; diff --git a/assets/User_code/device/motor_dm/motor_dm.c b/assets/User_code/device/motor_dm/motor_dm.c index a77656f..703c936 100644 --- a/assets/User_code/device/motor_dm/motor_dm.c +++ b/assets/User_code/device/motor_dm/motor_dm.c @@ -111,11 +111,21 @@ static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output) uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp; uint16_t id = motor->param.can_id; - pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16); - vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12); + /* 根据 reverse 参数调整控制值 */ + float angle = output->angle; + float velocity = output->velocity; + float torque = output->torque; + if (motor->param.reverse) { + angle = -angle; + velocity = -velocity; + torque = -torque; + } + + pos_tmp = float_to_uint(angle, DM_P_MIN , DM_P_MAX, 16); + vel_tmp = float_to_uint(velocity, DM_V_MIN , DM_V_MAX, 12); kp_tmp = float_to_uint(output->kp, DM_KP_MIN, DM_KP_MAX, 12); kd_tmp = float_to_uint(output->kd, DM_KD_MIN, DM_KD_MAX, 12); - tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12); + tor_tmp = float_to_uint(torque, DM_T_MIN , DM_T_MAX, 12); /* 打包数据 */ data[0] = (pos_tmp >> 8); @@ -151,6 +161,11 @@ static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) { uint8_t data[8] = {0}; + /* 根据 reverse 参数调整控制值 */ + if (motor->param.reverse) { + pos = -pos; + vel = -vel; + } /* 直接发送浮点数数据 */ memcpy(&data[0], &pos, 4); // 位置,低位在前 @@ -179,6 +194,11 @@ static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) { uint8_t data[4] = {0}; + /* 根据 reverse 参数调整控制值 */ + if (motor->param.reverse) { + vel = -vel; + } + /* 直接发送浮点数数据 */ memcpy(&data[0], &vel, 4); // 速度,低位在前 diff --git a/assets/User_code/module/cmd/cmd.c b/assets/User_code/module/cmd/cmd.c index 6dcca41..13f09ac 100644 --- a/assets/User_code/module/cmd/cmd.c +++ b/assets/User_code/module/cmd/cmd.c @@ -197,7 +197,7 @@ int8_t CMD_Arbitrate(CMD_t *ctx) { } /* 自动仲裁:优先级 PC > RC > NUC */ - CMD_InputSource_t candidates[] = {CMD_SRC_PC, CMD_SRC_RC, CMD_SRC_NUC}; + CMD_InputSource_t candidates[] = {CMD_SRC_RC, CMD_SRC_PC, CMD_SRC_NUC}; const int num_candidates = sizeof(candidates) / sizeof(candidates[0]); /* 如果当前输入源仍然在线且有效,保持使用 */ diff --git a/assets/User_code/module/cmd/cmd_adapter.c b/assets/User_code/module/cmd/cmd_adapter.c index d8023f6..6246469 100644 --- a/assets/User_code/module/cmd/cmd_adapter.c +++ b/assets/User_code/module/cmd/cmd_adapter.c @@ -10,7 +10,7 @@ // static CMD_InputAdapter_t *g_adapters[CMD_SRC_NUM] = {0}; CMD_InputAdapter_t *g_adapters[CMD_SRC_NUM] = {0}; /* ========================================================================== */ -/* DR16 抽象实现 */ +/* DR16 适配器实现 */ /* ========================================================================== */ #if CMD_RC_DEVICE_TYPE == 0 @@ -27,19 +27,19 @@ int8_t CMD_DR16_RC_GetInput(void *data, CMD_RawInput_t *output) { output->online[CMD_SRC_RC] = dr16->header.online; /* 遥控器摇杆映射 */ - output->rc.joy_left.x = dr16->data.rc.ch_l_x; - output->rc.joy_left.y = dr16->data.rc.ch_l_y; - output->rc.joy_right.x = dr16->data.rc.ch_r_x; - output->rc.joy_right.y = dr16->data.rc.ch_r_y; + output->rc.joy_left.x = dr16->data.ch_l_x; + output->rc.joy_left.y = dr16->data.ch_l_y; + output->rc.joy_right.x = dr16->data.ch_r_x; + output->rc.joy_right.y = dr16->data.ch_r_y; /* 拨杆映射 */ - switch (dr16->data.rc.sw_l) { + switch (dr16->data.sw_l) { case DR16_SW_UP: output->rc.sw[0] = CMD_SW_UP; break; case DR16_SW_MID: output->rc.sw[0] = CMD_SW_MID; break; case DR16_SW_DOWN: output->rc.sw[0] = CMD_SW_DOWN; break; default: output->rc.sw[0] = CMD_SW_ERR; break; } - switch (dr16->data.rc.sw_r) { + switch (dr16->data.sw_r) { case DR16_SW_UP: output->rc.sw[1] = CMD_SW_UP; break; case DR16_SW_MID: output->rc.sw[1] = CMD_SW_MID; break; case DR16_SW_DOWN: output->rc.sw[1] = CMD_SW_DOWN; break; @@ -47,7 +47,7 @@ int8_t CMD_DR16_RC_GetInput(void *data, CMD_RawInput_t *output) { } /* 拨轮映射 */ - output->rc.dial = dr16->data.rc.ch_res; + output->rc.dial = dr16->data.ch_res; return CMD_OK; } @@ -60,10 +60,10 @@ int8_t CMD_DR16_PC_GetInput(void *data, CMD_RawInput_t *output) { output->online[CMD_SRC_PC] = dr16->header.online; /* PC端鼠标映射 */ - output->pc.mouse.x = dr16->data.pc.mouse.x; - output->pc.mouse.y = dr16->data.pc.mouse.y; - output->pc.mouse.l_click = dr16->data.pc.mouse.l_click; - output->pc.mouse.r_click = dr16->data.pc.mouse.r_click; + output->pc.mouse.x = dr16->data.mouse.x; + output->pc.mouse.y = dr16->data.mouse.y; + output->pc.mouse.l_click = dr16->data.mouse.l_click; + output->pc.mouse.r_click = dr16->data.mouse.r_click; /* 键盘映射 */ output->pc.keyboard.bitmap = dr16->raw_data.key; @@ -83,7 +83,7 @@ CMD_DEFINE_ADAPTER(DR16_PC, cmd_dr16, CMD_SRC_PC, CMD_DR16_Init, CMD_DR16_PC_Get #endif /* CMD_RC_DEVICE_TYPE == 0 */ /* ========================================================================== */ -/* AT9S 抽象实现 (示例框架) */ +/* AT9S 适配器实现 (示例框架) */ /* ========================================================================== */ #if CMD_RC_DEVICE_TYPE == 1 @@ -100,10 +100,10 @@ int8_t CMD_AT9S_GetInput(void *data, CMD_RawInput_t *output) { output->online[CMD_SRC_RC] = at9s->header.online; /* TODO: 按照AT9S的数据格式进行映射 */ - output->joy_left.x = at9s->data.rc.ch_l_x; - output->joy_left.y = at9s->data.rc.ch_l_y; - output->joy_right.x = at9s->data.rc.ch_r_x; - output->joy_right.y = at9s->data.rc.ch_r_y; + output->joy_left.x = at9s->data.ch_l_x; + output->joy_left.y = at9s->data.ch_l_y; + output->joy_right.x = at9s->data.ch_r_x; + output->joy_right.y = at9s->data.ch_r_y; /* 拨杆映射需要根据AT9S的实际定义 */ @@ -142,10 +142,6 @@ int8_t CMD_Adapter_InitAll(void) { CMD_Adapter_Register(&g_adapter_AT9S); #endif - /* 注册NUC适配器 */ - - /* 注册REF适配器 */ - /* 初始化所有已注册的适配器 */ for (int i = 0; i < CMD_SRC_NUM; i++) { if (g_adapters[i] != NULL && g_adapters[i]->init != NULL) { diff --git a/assets/User_code/module/cmd/cmd_example.c b/assets/User_code/module/cmd/cmd_example.c index dbea4b0..476c32e 100644 --- a/assets/User_code/module/cmd/cmd_example.c +++ b/assets/User_code/module/cmd/cmd_example.c @@ -6,7 +6,7 @@ #include "cmd.h" /* ========================================================================== */ -/* 配置示例 */ +/* config示例 */ /* ========================================================================== */ /* 默认配置 */ @@ -37,35 +37,58 @@ // /* CMD上下文 */ // static CMD_t g_cmd_ctx; +/* ========================================================================== */ +/* 队列创建示例 */ +/* ========================================================================== */ +// #if CMD_RCTypeTable_Index == 0 +// task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL); +// #elif CMD_RCTypeTable_Index == 1 +// task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(AT9S_t), NULL); +// #endif + /* ========================================================================== */ /* 任务示例 */ /* ========================================================================== */ -/* - * 初始化示例 - */ -// void Example_CMD_Init(void) { -// CMD_Init(&g_cmd_ctx, &g_cmd_config); -// } +// #if CMD_RCTypeTable_Index == 0 +// DR16_t cmd_dr16; +// #elif CMD_RCTypeTable_Index == 1 +// AT9S_t cmd_at9s; +// #endif +// Shoot_CMD_t *cmd_for_shoot; +// Chassis_CMD_t *cmd_for_chassis; +// Gimbal_CMD_t *cmd_for_gimbal; -// /* -// * 任务循环示例 -// */ -// void Example_CMD_Task(void) { -// /* 一键更新 */ -// CMD_Update(&g_cmd_ctx); +// static CMD_t cmd; + +// void Task_cmd() { + +// CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param); + +// while (1) { +// #if CMD_RCTypeTable_Index == 0 +// osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_dr16, NULL, 0); +// #elif CMD_RCTypeTable_Index == 1 +// osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0); +// #endif +// CMD_Update(&cmd); // /* 获取命令发送到各模块 */ -// Chassis_CMD_t *chassis_cmd = CMD_GetChassisCmd(&g_cmd_ctx); -// Gimbal_CMD_t *gimbal_cmd = CMD_GetGimbalCmd(&g_cmd_ctx); -// Shoot_CMD_t *shoot_cmd = CMD_GetShootCmd(&g_cmd_ctx); - -// /* 使用命令... */ -// (void)chassis_cmd; -// (void)gimbal_cmd; -// (void)shoot_cmd; +// cmd_for_chassis = CMD_GetChassisCmd(&cmd); +// cmd_for_gimbal = CMD_GetGimbalCmd(&cmd); +// cmd_for_shoot = CMD_GetShootCmd(&cmd); +// osMessageQueueReset(task_runtime.msgq.gimbal.cmd); +// osMessageQueuePut(task_runtime.msgq.gimbal.cmd, cmd_for_gimbal, 0, 0); +// osMessageQueueReset(task_runtime.msgq.shoot.cmd); +// osMessageQueuePut(task_runtime.msgq.shoot.cmd, cmd_for_shoot, 0, 0); +// osMessageQueueReset(task_runtime.msgq.chassis.cmd); +// osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0); +// } + // } + + /* ========================================================================== */ /* 架构说明 */ /* ========================================================================== */