Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 5bf4f99113 | |||
| 72a4e5b073 |
@ -71,13 +71,13 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
|||||||
User/component/user_math.c
|
User/component/user_math.c
|
||||||
|
|
||||||
# User/device sources
|
# User/device sources
|
||||||
User/device/ai.c
|
|
||||||
User/device/bmi088.c
|
User/device/bmi088.c
|
||||||
User/device/dr16.c
|
User/device/dr16.c
|
||||||
User/device/motor.c
|
User/device/motor.c
|
||||||
User/device/motor_dm.c
|
User/device/motor_dm.c
|
||||||
User/device/motor_rm.c
|
User/device/motor_rm.c
|
||||||
User/device/remote_control.c
|
User/device/remote_control.c
|
||||||
|
User/device/ai.c
|
||||||
|
|
||||||
# User/module sources
|
# User/module sources
|
||||||
User/module/chassis.c
|
User/module/chassis.c
|
||||||
@ -87,8 +87,6 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
|||||||
User/module/shoot.c
|
User/module/shoot.c
|
||||||
|
|
||||||
# User/task sources
|
# User/task sources
|
||||||
User/task/Task8.c
|
|
||||||
User/task/ai.c
|
|
||||||
User/task/atti_esti.c
|
User/task/atti_esti.c
|
||||||
User/task/chassis.c
|
User/task/chassis.c
|
||||||
User/task/cmd.c
|
User/task/cmd.c
|
||||||
@ -96,8 +94,9 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
|||||||
User/task/init.c
|
User/task/init.c
|
||||||
User/task/rc.c
|
User/task/rc.c
|
||||||
User/task/shoot.c
|
User/task/shoot.c
|
||||||
User/task/tempCodeRunnerFile.c
|
|
||||||
User/task/user_task.c
|
User/task/user_task.c
|
||||||
|
User/task/ai.c
|
||||||
|
User/task/Task8.c
|
||||||
)
|
)
|
||||||
|
|
||||||
# Add include paths
|
# Add include paths
|
||||||
|
|||||||
@ -287,7 +287,6 @@ void USART1_IRQHandler(void)
|
|||||||
/* USER CODE END USART1_IRQn 0 */
|
/* USER CODE END USART1_IRQn 0 */
|
||||||
HAL_UART_IRQHandler(&huart1);
|
HAL_UART_IRQHandler(&huart1);
|
||||||
/* USER CODE BEGIN USART1_IRQn 1 */
|
/* USER CODE BEGIN USART1_IRQn 1 */
|
||||||
BSP_UART_IRQHandler(&huart1);
|
|
||||||
|
|
||||||
/* USER CODE END USART1_IRQn 1 */
|
/* USER CODE END USART1_IRQn 1 */
|
||||||
}
|
}
|
||||||
|
|||||||
@ -72,6 +72,4 @@ uart:
|
|||||||
name: DR16
|
name: DR16
|
||||||
- instance: USART6
|
- instance: USART6
|
||||||
name: NUC
|
name: NUC
|
||||||
- instance: USART1
|
|
||||||
name: '1'
|
|
||||||
enabled: true
|
enabled: true
|
||||||
|
|||||||
@ -73,20 +73,14 @@ int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)) {
|
|||||||
int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) {
|
int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) {
|
||||||
switch (gpio) {
|
switch (gpio) {
|
||||||
case BSP_GPIO_KEY:
|
case BSP_GPIO_KEY:
|
||||||
#if defined(KEY_EXTI_IRQn)
|
|
||||||
HAL_NVIC_EnableIRQ(KEY_EXTI_IRQn);
|
HAL_NVIC_EnableIRQ(KEY_EXTI_IRQn);
|
||||||
#endif
|
break;
|
||||||
return BSP_OK;
|
|
||||||
case BSP_GPIO_ACCL_INT:
|
case BSP_GPIO_ACCL_INT:
|
||||||
#if defined(ACCL_INT_EXTI_IRQn)
|
|
||||||
HAL_NVIC_EnableIRQ(ACCL_INT_EXTI_IRQn);
|
HAL_NVIC_EnableIRQ(ACCL_INT_EXTI_IRQn);
|
||||||
#endif
|
break;
|
||||||
return BSP_OK;
|
|
||||||
case BSP_GPIO_GYRO_INT:
|
case BSP_GPIO_GYRO_INT:
|
||||||
#if defined(GYRO_INT_EXTI_IRQn)
|
|
||||||
HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn);
|
HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn);
|
||||||
#endif
|
break;
|
||||||
return BSP_OK;
|
|
||||||
default:
|
default:
|
||||||
return BSP_ERR;
|
return BSP_ERR;
|
||||||
}
|
}
|
||||||
@ -96,20 +90,14 @@ int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) {
|
|||||||
int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
|
int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
|
||||||
switch (gpio) {
|
switch (gpio) {
|
||||||
case BSP_GPIO_KEY:
|
case BSP_GPIO_KEY:
|
||||||
#if defined(KEY_EXTI_IRQn)
|
|
||||||
HAL_NVIC_DisableIRQ(KEY_EXTI_IRQn);
|
HAL_NVIC_DisableIRQ(KEY_EXTI_IRQn);
|
||||||
#endif
|
break;
|
||||||
return BSP_OK;
|
|
||||||
case BSP_GPIO_ACCL_INT:
|
case BSP_GPIO_ACCL_INT:
|
||||||
#if defined(ACCL_INT_EXTI_IRQn)
|
|
||||||
HAL_NVIC_DisableIRQ(ACCL_INT_EXTI_IRQn);
|
HAL_NVIC_DisableIRQ(ACCL_INT_EXTI_IRQn);
|
||||||
#endif
|
break;
|
||||||
return BSP_OK;
|
|
||||||
case BSP_GPIO_GYRO_INT:
|
case BSP_GPIO_GYRO_INT:
|
||||||
#if defined(GYRO_INT_EXTI_IRQn)
|
|
||||||
HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn);
|
HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn);
|
||||||
#endif
|
break;
|
||||||
return BSP_OK;
|
|
||||||
default:
|
default:
|
||||||
return BSP_ERR;
|
return BSP_ERR;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -27,8 +27,6 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
|
|||||||
return BSP_UART_DR16;
|
return BSP_UART_DR16;
|
||||||
else if (huart->Instance == USART6)
|
else if (huart->Instance == USART6)
|
||||||
return BSP_UART_NUC;
|
return BSP_UART_NUC;
|
||||||
else if (huart->Instance == USART1)
|
|
||||||
return BSP_UART_1;
|
|
||||||
else
|
else
|
||||||
return BSP_UART_ERR;
|
return BSP_UART_ERR;
|
||||||
}
|
}
|
||||||
@ -121,8 +119,6 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
|
|||||||
return &huart3;
|
return &huart3;
|
||||||
case BSP_UART_NUC:
|
case BSP_UART_NUC:
|
||||||
return &huart6;
|
return &huart6;
|
||||||
case BSP_UART_1:
|
|
||||||
return &huart1;
|
|
||||||
default:
|
default:
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -29,7 +29,6 @@ extern "C" {
|
|||||||
typedef enum {
|
typedef enum {
|
||||||
BSP_UART_DR16,
|
BSP_UART_DR16,
|
||||||
BSP_UART_NUC,
|
BSP_UART_NUC,
|
||||||
BSP_UART_1,
|
|
||||||
BSP_UART_NUM,
|
BSP_UART_NUM,
|
||||||
BSP_UART_ERR,
|
BSP_UART_ERR,
|
||||||
} BSP_UART_t;
|
} BSP_UART_t;
|
||||||
|
|||||||
@ -3,7 +3,7 @@
|
|||||||
// Example:
|
// Example:
|
||||||
|
|
||||||
// DR16_Init(&dr16);
|
// DR16_Init(&dr16);
|
||||||
|
//
|
||||||
// while (1) {
|
// while (1) {
|
||||||
// DR16_StartDmaRecv(&dr16);
|
// DR16_StartDmaRecv(&dr16);
|
||||||
// if (DR16_WaitDmaCplt(20)) {
|
// if (DR16_WaitDmaCplt(20)) {
|
||||||
@ -114,7 +114,7 @@
|
|||||||
|
|
||||||
// dr16->header.online = true;
|
// dr16->header.online = true;
|
||||||
// dr16->header.last_online_time = BSP_TIME_Get_us();
|
// dr16->header.last_online_time = BSP_TIME_Get_us();
|
||||||
|
//
|
||||||
// memset(&(dr16->data), 0, sizeof(dr16->data));
|
// memset(&(dr16->data), 0, sizeof(dr16->data));
|
||||||
|
|
||||||
// float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN);
|
// float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN);
|
||||||
@ -139,7 +139,7 @@
|
|||||||
|
|
||||||
// // 解析键盘按键 - 使用union简化代码
|
// // 解析键盘按键 - 使用union简化代码
|
||||||
// uint16_t key_value = dr16->raw_data.key;
|
// uint16_t key_value = dr16->raw_data.key;
|
||||||
|
//
|
||||||
// // 解析键盘位映射(W-B键,位0-15)
|
// // 解析键盘位映射(W-B键,位0-15)
|
||||||
// for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) {
|
// for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) {
|
||||||
// dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0;
|
// dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0;
|
||||||
@ -151,7 +151,7 @@
|
|||||||
|
|
||||||
// // 解析第五通道
|
// // 解析第五通道
|
||||||
// dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range;
|
// dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range;
|
||||||
|
//
|
||||||
// return DEVICE_OK;
|
// return DEVICE_OK;
|
||||||
//}
|
//}
|
||||||
|
|
||||||
|
|||||||
@ -12,11 +12,11 @@
|
|||||||
|
|
||||||
///* USER INCLUDE BEGIN */
|
///* USER INCLUDE BEGIN */
|
||||||
|
|
||||||
// ///* USER INCLUDE END */
|
///* USER INCLUDE END */
|
||||||
|
|
||||||
///* USER DEFINE BEGIN */
|
///* USER DEFINE BEGIN */
|
||||||
|
|
||||||
// ///* USER DEFINE END */
|
///* USER DEFINE END */
|
||||||
|
|
||||||
///* Exported constants ------------------------------------------------------- */
|
///* Exported constants ------------------------------------------------------- */
|
||||||
///* Exported macro ----------------------------------------------------------- */
|
///* Exported macro ----------------------------------------------------------- */
|
||||||
@ -110,7 +110,7 @@
|
|||||||
|
|
||||||
///* USER FUNCTION BEGIN */
|
///* USER FUNCTION BEGIN */
|
||||||
|
|
||||||
// ///* USER FUNCTION END */
|
///* USER FUNCTION END */
|
||||||
|
|
||||||
//#ifdef __cplusplus
|
//#ifdef __cplusplus
|
||||||
//}
|
//}
|
||||||
|
|||||||
@ -56,12 +56,12 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module
|
|||||||
switch (module) {
|
switch (module) {
|
||||||
case MOTOR_M2006:
|
case MOTOR_M2006:
|
||||||
case MOTOR_M3508:
|
case MOTOR_M3508:
|
||||||
if (can_id >= M3508_M2006_FB_ID_BASE && can_id <= M3508_M2006_FB_ID_BASE + 7) {
|
if (can_id >= M3508_M2006_FB_ID_BASE && can_id < M3508_M2006_FB_ID_BASE + 7) {
|
||||||
return can_id - M3508_M2006_FB_ID_BASE;
|
return can_id - M3508_M2006_FB_ID_BASE;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case MOTOR_GM6020:
|
case MOTOR_GM6020:
|
||||||
if (can_id >= GM6020_FB_ID_BASE && can_id <= GM6020_FB_ID_BASE + 6) {
|
if (can_id >= GM6020_FB_ID_BASE && can_id < GM6020_FB_ID_BASE + 6) {
|
||||||
return can_id - GM6020_FB_ID_BASE + 4;
|
return can_id - GM6020_FB_ID_BASE + 4;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
@ -74,7 +74,7 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module
|
|||||||
static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
|
static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
|
||||||
switch (module) {
|
switch (module) {
|
||||||
case MOTOR_M2006: return 36.0f;
|
case MOTOR_M2006: return 36.0f;
|
||||||
case MOTOR_M3508: return 3591.0f / 187.0f;
|
case MOTOR_M3508: return 268.0f/17.0f ; // 通用3508减速比3591.0f / 187.0f;前面的是舵轮减速箱减速比
|
||||||
case MOTOR_GM6020: return 1.0f;
|
case MOTOR_GM6020: return 1.0f;
|
||||||
default: return 1.0f;
|
default: return 1.0f;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -187,7 +187,7 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd
|
|||||||
s_cmd->mode = SHOOT_MODE_SINGLE;
|
s_cmd->mode = SHOOT_MODE_SINGLE;
|
||||||
break;
|
break;
|
||||||
case CMD_SW_DOWN:
|
case CMD_SW_DOWN:
|
||||||
s_cmd->mode = SHOOT_MODE_CONTINUE;
|
s_cmd->mode = SHOOT_MODE_BURST;
|
||||||
break;
|
break;
|
||||||
case CMD_SW_ERR:
|
case CMD_SW_ERR:
|
||||||
s_cmd->mode = SHOOT_MODE_SAFE;
|
s_cmd->mode = SHOOT_MODE_SAFE;
|
||||||
@ -195,6 +195,7 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd
|
|||||||
}
|
}
|
||||||
switch(rc->ET16s.key_D)
|
switch(rc->ET16s.key_D)
|
||||||
{
|
{
|
||||||
|
|
||||||
case CMD_SW_UP:
|
case CMD_SW_UP:
|
||||||
s_cmd->firecmd = false;
|
s_cmd->firecmd = false;
|
||||||
s_cmd->ready = false;
|
s_cmd->ready = false;
|
||||||
|
|||||||
@ -47,8 +47,6 @@ typedef struct
|
|||||||
|
|
||||||
struct
|
struct
|
||||||
{
|
{
|
||||||
|
|
||||||
uint8_t head;
|
|
||||||
float ch_l_x; /* 遥控器左侧摇杆横轴值,上为正 */
|
float ch_l_x; /* 遥控器左侧摇杆横轴值,上为正 */
|
||||||
float ch_l_y; /* 遥控器左侧摇杆纵轴值,右为正 */
|
float ch_l_y; /* 遥控器左侧摇杆纵轴值,右为正 */
|
||||||
float ch_r_x; /* 遥控器右侧摇杆横轴值,上为正 */
|
float ch_r_x; /* 遥控器右侧摇杆横轴值,上为正 */
|
||||||
@ -63,8 +61,6 @@ typedef struct
|
|||||||
CMD_SwitchPos_t key_G;
|
CMD_SwitchPos_t key_G;
|
||||||
CMD_SwitchPos_t key_H;
|
CMD_SwitchPos_t key_H;
|
||||||
|
|
||||||
|
|
||||||
uint8_t end;
|
|
||||||
int16_t knob_left; // 左旋钮
|
int16_t knob_left; // 左旋钮
|
||||||
int16_t knob_right; // 右旋钮
|
int16_t knob_right; // 右旋钮
|
||||||
} __attribute__((packed)) ET16s;
|
} __attribute__((packed)) ET16s;
|
||||||
@ -131,14 +127,13 @@ typedef enum {
|
|||||||
}Shoot_Mode_t;
|
}Shoot_Mode_t;
|
||||||
|
|
||||||
typedef enum{
|
typedef enum{
|
||||||
SHOOT_REMOTE = 0,/* 遥控器控制 */
|
SHOOT_REMOTE,
|
||||||
SHOOT_AI, /* AI控制 */
|
SHOOT_AI
|
||||||
} Shoot_Control_Mode_t;
|
}Shoot_CONTROL_Mode_t;
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
Shoot_Mode_t mode;/* 射击模式 */
|
Shoot_Mode_t mode;/* 射击模式 */
|
||||||
|
Shoot_CONTROL_Mode_t control_mode;
|
||||||
Shoot_Control_Mode_t control_mode;/* 控制模式 */
|
|
||||||
bool ready; /* 准备射击 */
|
bool ready; /* 准备射击 */
|
||||||
bool firecmd; /* 射击 */
|
bool firecmd; /* 射击 */
|
||||||
}Shoot_CMD_t;
|
}Shoot_CMD_t;
|
||||||
|
|||||||
@ -145,8 +145,8 @@ static const Config_Param_t config = {
|
|||||||
},
|
},
|
||||||
|
|
||||||
.pid.yaw_4310_motor_omega = {
|
.pid.yaw_4310_motor_omega = {
|
||||||
.k = 0.5f,
|
.k = 0.0f,
|
||||||
.p = 0.5f,
|
.p = 0.3f,
|
||||||
.i = 0.0f,
|
.i = 0.0f,
|
||||||
.d = 0.0f,
|
.d = 0.0f,
|
||||||
.i_limit = 0.0f,
|
.i_limit = 0.0f,
|
||||||
|
|||||||
@ -115,7 +115,6 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq)
|
|||||||
PID_Init(&g->pid.yaw_6020_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_6020_motor_omega );
|
PID_Init(&g->pid.yaw_6020_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_6020_motor_omega );
|
||||||
/*4310大yaw pid初始化 大yaw单环控*/
|
/*4310大yaw pid初始化 大yaw单环控*/
|
||||||
PID_Init(&g->pid.yaw_4310_angle,KPID_MODE_SET_D, target_freq,&g->param->pid.yaw_4310_motor_angle );
|
PID_Init(&g->pid.yaw_4310_angle,KPID_MODE_SET_D, target_freq,&g->param->pid.yaw_4310_motor_angle );
|
||||||
PID_Init(&g->pid.yaw_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_4310_motor_omega );
|
|
||||||
/*4310 pitch pid初始化 单环*/
|
/*4310 pitch pid初始化 单环*/
|
||||||
PID_Init(&g->pid.pitch_4310_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.pitch_4310_motor_angle );
|
PID_Init(&g->pid.pitch_4310_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.pitch_4310_motor_angle );
|
||||||
PID_Init(&g->pid.pitch_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.pitch_4310_motor_omega );
|
PID_Init(&g->pid.pitch_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.pitch_4310_motor_omega );
|
||||||
@ -247,7 +246,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
|
|||||||
Gimbal_SetMode(g, g_cmd->mode);
|
Gimbal_SetMode(g, g_cmd->mode);
|
||||||
|
|
||||||
/*处理小yaw轴控制命令*/
|
/*处理小yaw轴控制命令*/
|
||||||
float delta_yaw_6020 = 10.0f*g_cmd->delta_yaw_6020 * g->dt;
|
float delta_yaw_6020 = 5.0f*g_cmd->delta_yaw_6020 * g->dt;
|
||||||
|
|
||||||
if(g->param->travel.yaw_6020 > 0) // 有限位才处理
|
if(g->param->travel.yaw_6020 > 0) // 有限位才处理
|
||||||
{
|
{
|
||||||
@ -277,7 +276,7 @@ const float delta_min_6020 = CircleError(g->limit.yaw_6020.min,
|
|||||||
g->setpoint.yaw_4310=small_yaw_center_offset;
|
g->setpoint.yaw_4310=small_yaw_center_offset;
|
||||||
|
|
||||||
/*处理大pitch控制命令*/
|
/*处理大pitch控制命令*/
|
||||||
float delta_pitch_4310 = 8.0f*g_cmd->delta_pitch_4310*g->dt;
|
float delta_pitch_4310 = 5.0f*g_cmd->delta_pitch_4310*g->dt;
|
||||||
if(g->param->travel.pitch_4310 > 0) // 有限位才处理
|
if(g->param->travel.pitch_4310 > 0) // 有限位才处理
|
||||||
{
|
{
|
||||||
/* 计算当前电机角度与IMU角度的偏差 */
|
/* 计算当前电机角度与IMU角度的偏差 */
|
||||||
@ -331,15 +330,11 @@ g->setpoint.eulr.pit = g_cmd->set_pitch;
|
|||||||
/*4310大YAW控制
|
/*4310大YAW控制
|
||||||
这里是单环控制的,有需要加双环
|
这里是单环控制的,有需要加双环
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
|
yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
|
||||||
g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt);
|
g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt);
|
||||||
|
// g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
|
||||||
|
// g->feedback.imu.gyro.z,0.0f,g->dt);
|
||||||
g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
|
g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出
|
||||||
g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt);
|
|
||||||
// g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出
|
|
||||||
/*控制云台4310电机 也是单环,但是加了重力补偿函数,可以根据不一样的情况去拟合函数*/
|
/*控制云台4310电机 也是单环,但是加了重力补偿函数,可以根据不一样的情况去拟合函数*/
|
||||||
pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit,
|
pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit,
|
||||||
g->feedback.imu.eulr.rol,0.0f,g->dt);
|
g->feedback.imu.eulr.rol,0.0f,g->dt);
|
||||||
@ -381,9 +376,8 @@ void Gimbal_Output(Gimbal_t *g)
|
|||||||
MOTOR_RM_SetOutput(&g->param->yaw_6020_motor, g->out.yaw_6020);
|
MOTOR_RM_SetOutput(&g->param->yaw_6020_motor, g->out.yaw_6020);
|
||||||
|
|
||||||
MOTOR_MIT_Output_t output_yaw_4310 = {0};
|
MOTOR_MIT_Output_t output_yaw_4310 = {0};
|
||||||
output_yaw_4310.torque = g->out.yaw_4310 * 2.0f;
|
output_yaw_4310.torque = g->out.yaw_4310 * 1.0f;
|
||||||
// output_yaw_4310.kp = 0.2f;
|
output_yaw_4310.kd = 0.2f;
|
||||||
output_yaw_4310.kd = 0.1f;
|
|
||||||
// output_yaw_4310.kd=0.1f;
|
// output_yaw_4310.kd=0.1f;
|
||||||
|
|
||||||
MOTOR_MIT_Output_t output_pitch_4310 = {0};
|
MOTOR_MIT_Output_t output_pitch_4310 = {0};
|
||||||
|
|||||||
@ -6,6 +6,9 @@
|
|||||||
/* Includes ----------------------------------------------------------------- */
|
/* Includes ----------------------------------------------------------------- */
|
||||||
#include "task/user_task.h"
|
#include "task/user_task.h"
|
||||||
/* USER INCLUDE BEGIN */
|
/* USER INCLUDE BEGIN */
|
||||||
|
#include "bsp/can.h"
|
||||||
|
#include "device/motor_rm.h"
|
||||||
|
#include "component/pid.h"
|
||||||
|
|
||||||
/* USER INCLUDE END */
|
/* USER INCLUDE END */
|
||||||
|
|
||||||
@ -14,7 +17,57 @@
|
|||||||
/* Private macro ------------------------------------------------------------ */
|
/* Private macro ------------------------------------------------------------ */
|
||||||
/* Private variables -------------------------------------------------------- */
|
/* Private variables -------------------------------------------------------- */
|
||||||
/* USER STRUCT BEGIN */
|
/* USER STRUCT BEGIN */
|
||||||
|
MOTOR_RM_t motor_6020;
|
||||||
|
MOTOR_RM_t motor_3508;
|
||||||
|
MOTOR_RM_Param_t motor_6020_param = {
|
||||||
|
.can = BSP_CAN_1,
|
||||||
|
.id = 0x205,
|
||||||
|
.module = MOTOR_GM6020,
|
||||||
|
.reverse = false,
|
||||||
|
.gear = false,
|
||||||
|
};
|
||||||
|
MOTOR_RM_Param_t motor_3508_param = {
|
||||||
|
.can = BSP_CAN_1,
|
||||||
|
.id = 0x201,
|
||||||
|
.module = MOTOR_M3508,
|
||||||
|
.reverse = false,
|
||||||
|
.gear = false,
|
||||||
|
};
|
||||||
|
|
||||||
|
KPID_t motor_6020_speed;
|
||||||
|
KPID_t motor_3508_angle;
|
||||||
|
KPID_t motor_3508_omega;
|
||||||
|
|
||||||
|
KPID_Params_t motor_6020_speed_param = {
|
||||||
|
.k = 0.1f,
|
||||||
|
.p = 0.2f,
|
||||||
|
.i = 0.1f,
|
||||||
|
.d = 0.0f,
|
||||||
|
.i_limit = 1.0f,
|
||||||
|
.out_limit = 1.0f,
|
||||||
|
.d_cutoff_freq = 30.0f,
|
||||||
|
};
|
||||||
|
KPID_Params_t motor_3508_angle_param = {
|
||||||
|
.k = 1.0f,
|
||||||
|
.p = 5.0f,
|
||||||
|
.i = 0.1f,
|
||||||
|
.d = 0.0f,
|
||||||
|
.i_limit = 1.0f,
|
||||||
|
.out_limit = 1.0f,
|
||||||
|
.d_cutoff_freq = 30.0f,
|
||||||
|
};
|
||||||
|
KPID_Params_t motor_3508_omega_param = {
|
||||||
|
.k = 1.0f,
|
||||||
|
.p = 5.0f,
|
||||||
|
.i = 0.1f,
|
||||||
|
.d = 0.0f,
|
||||||
|
.i_limit = 1.0f,
|
||||||
|
.out_limit = 1.0f,
|
||||||
|
.d_cutoff_freq = 30.0f,
|
||||||
|
};
|
||||||
|
|
||||||
|
float motor_3508_target_angle = 5*M_2PI;
|
||||||
|
float motor_6020_target_speed = 33.0f;
|
||||||
/* USER STRUCT END */
|
/* USER STRUCT END */
|
||||||
|
|
||||||
/* Private function --------------------------------------------------------- */
|
/* Private function --------------------------------------------------------- */
|
||||||
@ -30,17 +83,34 @@ void Task_Task8(void *argument) {
|
|||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||||
/* USER CODE INIT BEGIN */
|
/* USER CODE INIT BEGIN */
|
||||||
|
BSP_CAN_Init();
|
||||||
|
MOTOR_RM_Register( &motor_6020_param);
|
||||||
|
MOTOR_RM_Register(&motor_3508_param);
|
||||||
|
|
||||||
|
PID_Init(&motor_6020_speed, KPID_MODE_CALC_D, TASK8_FREQ, &motor_6020_speed_param);
|
||||||
|
PID_Init(&motor_3508_angle, KPID_MODE_NO_D, TASK8_FREQ, &motor_3508_angle_param);
|
||||||
|
PID_Init(&motor_3508_omega, KPID_MODE_NO_D, TASK8_FREQ, &motor_3508_omega_param);
|
||||||
/* USER CODE INIT END */
|
/* USER CODE INIT END */
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||||
/* USER CODE BEGIN */
|
/* USER CODE BEGIN */
|
||||||
// osMessageQueueGet(task_runtime.msgq.RC_REMOTE, &remote, NULL, 0);
|
MOTOR_RM_UpdateAll();
|
||||||
|
|
||||||
|
motor_6020.feedback.rotor_speed= (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&motor_6020_param)->motor)) );
|
||||||
|
motor_3508.feedback.rotor_abs_angle= MOTOR_RM_GetMotor(&motor_3508_param)->gearbox_total_angle;
|
||||||
|
motor_3508.feedback.rotor_speed= (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&motor_3508_param)->motor)) );
|
||||||
|
|
||||||
|
|
||||||
|
float motor_6020out,motor_3508_ommega,motor_3508_out;
|
||||||
|
|
||||||
|
motor_6020out = PID_Calc(&motor_6020_speed, motor_6020_target_speed, motor_6020.feedback.rotor_speed, 0.0f,0.0f);
|
||||||
|
|
||||||
|
// motor_3508_ommega = PID_Calc(&motor_3508_omega, motor_3508_target_angle, motor_3508.feedback.rotor_abs_angle, 0.0f,0.0f);
|
||||||
|
// motor_3508_out = PID_Calc(&motor_3508_angle, motor_3508_ommega, motor_3508.feedback.rotor_speed, 0.0f,0.0f);
|
||||||
|
|
||||||
|
MOTOR_RM_SetOutput(&motor_6020_param, motor_6020out);
|
||||||
|
MOTOR_RM_Ctrl(&motor_6020_param);
|
||||||
/* USER CODE END */
|
/* USER CODE END */
|
||||||
osDelayUntil(tick); /* 运行结束,等待下一次唤醒 */
|
osDelayUntil(tick); /* 运行结束,等待下一次唤醒 */
|
||||||
}
|
}
|
||||||
|
|||||||
@ -49,7 +49,9 @@ void Task_ai(void *argument) {
|
|||||||
AI_Get_NUC(&ai,&ai_cmd);
|
AI_Get_NUC(&ai,&ai_cmd);
|
||||||
osMessageQueueReset(task_runtime.msgq.gimbal.ai.g_cmd);
|
osMessageQueueReset(task_runtime.msgq.gimbal.ai.g_cmd);
|
||||||
osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_cmd, 0, 0);
|
osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_cmd, 0, 0);
|
||||||
osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd, &ai_cmd, 0, 0);
|
|
||||||
|
osMessageQueueReset(task_runtime.msgq.gimbal.ai.s_cmd);
|
||||||
|
osMessageQueuePut(task_runtime.msgq.gimbal.ai.s_cmd, &ai_cmd, 0, 0);
|
||||||
/* USER CODE END */
|
/* USER CODE END */
|
||||||
osDelayUntil(tick); /* 运行结束,等待下一次唤醒 */
|
osDelayUntil(tick); /* 运行结束,等待下一次唤醒 */
|
||||||
}
|
}
|
||||||
|
|||||||
@ -38,7 +38,7 @@ void Task_Init(void *argument) {
|
|||||||
task_runtime.thread.gimbal = osThreadNew(Task_gimbal, NULL, &attr_gimbal);
|
task_runtime.thread.gimbal = osThreadNew(Task_gimbal, NULL, &attr_gimbal);
|
||||||
task_runtime.thread.shoot = osThreadNew(Task_shoot, NULL, &attr_shoot);
|
task_runtime.thread.shoot = osThreadNew(Task_shoot, NULL, &attr_shoot);
|
||||||
task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
|
task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
|
||||||
task_runtime.thread.Task8 = osThreadNew(Task_Task8, NULL, &attr_Task8);
|
// task_runtime.thread.Task8 = osThreadNew(Task_Task8, NULL, &attr_Task8);
|
||||||
|
|
||||||
// 创建消息队列
|
// 创建消息队列
|
||||||
/* USER MESSAGE BEGIN */
|
/* USER MESSAGE BEGIN */
|
||||||
@ -54,8 +54,7 @@ void Task_Init(void *argument) {
|
|||||||
task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t),NULL);
|
task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t),NULL);
|
||||||
task_runtime.msgq.gimbal.ai.feedback = osMessageQueueNew(2u, sizeof(Gimbal_feedback_t),NULL);
|
task_runtime.msgq.gimbal.ai.feedback = osMessageQueueNew(2u, sizeof(Gimbal_feedback_t),NULL);
|
||||||
task_runtime.msgq.gimbal.ai.g_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL);
|
task_runtime.msgq.gimbal.ai.g_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL);
|
||||||
task_runtime.msgq.shoot.ai.s_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL);
|
task_runtime.msgq.gimbal.ai.s_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL);
|
||||||
|
|
||||||
/* USER MESSAGE END */
|
/* USER MESSAGE END */
|
||||||
|
|
||||||
osKernelUnlock(); // 解锁内核
|
osKernelUnlock(); // 解锁内核
|
||||||
|
|||||||
@ -44,28 +44,28 @@ Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE);
|
|||||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||||
/* USER CODE BEGIN */
|
/* USER CODE BEGIN */
|
||||||
osMessageQueueGet(task_runtime.msgq.shoot.cmd, &shoot_cmd, NULL, 0);
|
osMessageQueueGet(task_runtime.msgq.shoot.cmd, &shoot_cmd, NULL, 0);
|
||||||
osMessageQueueGet(task_runtime.msgq.shoot.ai.s_cmd, &shoot_ai_cmd, NULL, 0);
|
osMessageQueueGet(task_runtime.msgq.gimbal.ai.s_cmd, &shoot_ai_cmd, NULL, 0);
|
||||||
|
|
||||||
// if(shoot_cmd.control_mode==SHOOT_REMOTE)
|
if(shoot_cmd.control_mode==SHOOT_REMOTE)
|
||||||
// {
|
{
|
||||||
// //do nothing,使用遥控器的指令
|
//do nothing,使用遥控器的指令
|
||||||
// }
|
}
|
||||||
// else if(shoot_cmd.control_mode==SHOOT_AI)
|
else if(shoot_cmd.control_mode==SHOOT_AI)
|
||||||
// {
|
{
|
||||||
// shoot_cmd.mode = SHOOT_MODE_SINGLE;
|
shoot_cmd.mode = SHOOT_MODE_SINGLE;
|
||||||
// if(shoot_ai_cmd.mode==0 || shoot_ai_cmd.mode==1)
|
if(shoot_ai_cmd.mode==0 || shoot_ai_cmd.mode==1)
|
||||||
// {
|
{
|
||||||
// shoot_cmd.firecmd=false;
|
shoot_cmd.firecmd=false;
|
||||||
// shoot_cmd.ready=true;
|
shoot_cmd.ready=true;
|
||||||
|
|
||||||
// }
|
}
|
||||||
// else if(shoot_ai_cmd.mode==2)
|
else if(shoot_ai_cmd.mode==2)
|
||||||
// {
|
{
|
||||||
|
|
||||||
// shoot_cmd.firecmd=true;
|
shoot_cmd.firecmd=true;
|
||||||
// shoot_cmd.ready=true;
|
shoot_cmd.ready=true;
|
||||||
// }
|
}
|
||||||
// }
|
}
|
||||||
|
|
||||||
Shoot_UpdateFeedback(&shoot);
|
Shoot_UpdateFeedback(&shoot);
|
||||||
Shoot_SetMode(&shoot,shoot_cmd.mode);
|
Shoot_SetMode(&shoot,shoot_cmd.mode);
|
||||||
|
|||||||
@ -0,0 +1 @@
|
|||||||
|
feedback
|
||||||
@ -91,7 +91,7 @@ typedef struct {
|
|||||||
osMessageQueueId_t quat;
|
osMessageQueueId_t quat;
|
||||||
osMessageQueueId_t feedback;
|
osMessageQueueId_t feedback;
|
||||||
osMessageQueueId_t g_cmd;
|
osMessageQueueId_t g_cmd;
|
||||||
|
osMessageQueueId_t s_cmd;
|
||||||
}ai;
|
}ai;
|
||||||
/* 新增的 ai 消息队列 主要是给自瞄*/
|
/* 新增的 ai 消息队列 主要是给自瞄*/
|
||||||
} gimbal;
|
} gimbal;
|
||||||
@ -99,13 +99,8 @@ typedef struct {
|
|||||||
struct
|
struct
|
||||||
{
|
{
|
||||||
osMessageQueueId_t cmd;
|
osMessageQueueId_t cmd;
|
||||||
struct{
|
|
||||||
|
|
||||||
osMessageQueueId_t s_cmd;
|
|
||||||
}ai;
|
|
||||||
}shoot;
|
}shoot;
|
||||||
|
|
||||||
|
|
||||||
} msgq;
|
} msgq;
|
||||||
/* USER MESSAGE END */
|
/* USER MESSAGE END */
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user