xiaocheng/Er_sentry
1
1
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

确认了导航方向，添加了自瞄

Browse Source
This commit is contained in:
Xiaocheng 2026-03-10 14:21:21 +08:00
parent 87ff7ff05e
commit 5f06fe477b
20 changed files with 380 additions and 251 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Core/Inc/stm32f4xx_it.h
View File

@ -68,6 +68,7 @@ void CAN2_RX0_IRQHandler(void);
void CAN2_RX1_IRQHandler(void);
void DMA2_Stream5_IRQHandler(void);
void DMA2_Stream6_IRQHandler(void);
void DMA2_Stream7_IRQHandler(void);
void USART6_IRQHandler(void);
/* USER CODE BEGIN EFP */

3
Core/Src/dma.c
View File

@ -62,6 +62,9 @@ void MX_DMA_Init(void)
/* DMA2_Stream6_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream6_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream6_IRQn);
/* DMA2_Stream7_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream7_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream7_IRQn);
}

15
Core/Src/stm32f4xx_it.c
View File

@ -63,6 +63,7 @@ extern CAN_HandleTypeDef hcan2;
extern DMA_HandleTypeDef hdma_spi1_rx;
extern DMA_HandleTypeDef hdma_spi1_tx;
extern DMA_HandleTypeDef hdma_usart1_rx;
extern DMA_HandleTypeDef hdma_usart1_tx;
extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart6_rx;
extern DMA_HandleTypeDef hdma_usart6_tx;
@ -405,6 +406,20 @@ void DMA2_Stream6_IRQHandler(void)
/* USER CODE END DMA2_Stream6_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream7 global interrupt.
*/
void DMA2_Stream7_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream7_IRQn 0 */
/* USER CODE END DMA2_Stream7_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart1_tx);
/* USER CODE BEGIN DMA2_Stream7_IRQn 1 */
/* USER CODE END DMA2_Stream7_IRQn 1 */
}
/**
* @brief This function handles USART6 global interrupt.
*/

20
Core/Src/usart.c
View File

@ -28,6 +28,7 @@ UART_HandleTypeDef huart1;
UART_HandleTypeDef huart3;
UART_HandleTypeDef huart6;
DMA_HandleTypeDef hdma_usart1_rx;
DMA_HandleTypeDef hdma_usart1_tx;
DMA_HandleTypeDef hdma_usart3_rx;
DMA_HandleTypeDef hdma_usart6_rx;
DMA_HandleTypeDef hdma_usart6_tx;
@ -171,6 +172,24 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
__HAL_LINKDMA(uartHandle,hdmarx,hdma_usart1_rx);
/* USART1_TX Init */
hdma_usart1_tx.Instance = DMA2_Stream7;
hdma_usart1_tx.Init.Channel = DMA_CHANNEL_4;
hdma_usart1_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_usart1_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_usart1_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_usart1_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_usart1_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_usart1_tx.Init.Mode = DMA_NORMAL;
hdma_usart1_tx.Init.Priority = DMA_PRIORITY_LOW;
hdma_usart1_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_usart1_tx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(uartHandle,hdmatx,hdma_usart1_tx);
/* USART1 interrupt Init */
HAL_NVIC_SetPriority(USART1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(USART1_IRQn);
@ -311,6 +330,7 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
/* USART1 DMA DeInit */
HAL_DMA_DeInit(uartHandle->hdmarx);
HAL_DMA_DeInit(uartHandle->hdmatx);
/* USART1 interrupt Deinit */
HAL_NVIC_DisableIRQ(USART1_IRQn);

177
User/device/ai.c
View File

@ -4,162 +4,63 @@
#include "component/crc16.h"
int8_t AI_Init(AI_t *ai) {
if (ai == NULL) return DEVICE_ERR_NULL;
// if (inited) return DEVICE_ERR_INITED;
// if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_NUC,
BSP_UART_RX_CPLT_CB);
// inited = true;
return 0;
}
int8_t AI_StartReceiving(AI_t *ai) {
if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), true)==HAL_OK) {
int8_t AI_StartReceiving(PackageAI_t *ai) {
if (BSP_UART_Receive(BSP_UART_1,(uint8_t *)ai,sizeof(*ai), true)==HAL_OK) {
return DEVICE_OK;}
return DEVICE_ERR;
}
int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) {
if(ai->RX.head[0]!='M'&&ai->RX.head[1]!='R'){
return DEVICE_ERR;
}
// CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16);
if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX))!=true){
return DEVICE_ERR;
}
ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch;
ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw;
ai_cmd->mode=ai->RX.mode;
ai_cmd->gimbal_t.accl.pit=ai->RX.pitch_acc;
ai_cmd->gimbal_t.vel.pit=ai->RX.pitch_vel;
ai_cmd->gimbal_t.accl.yaw=ai->RX.yaw_acc;
ai_cmd->gimbal_t.vel.yaw=ai->RX.yaw_vel;
int8_t AI_Get_NUC(PackageAI_t *ai,AI_result_t* result) {
if(ai->id==ID_AI){
// if(CRC16_Verify((const uint8_t*)&(ai), sizeof(&ai))==true)
// {
result->mode=ai->data.mode;
result->gimbal_t.setpoint.yaw=ai->data.yaw;
result->gimbal_t.vel.yaw=ai->data.yaw_vel;
result->gimbal_t.accl.yaw=ai->data.yaw_acc;
result->gimbal_t.setpoint.pit=ai->data.pitch;
result->gimbal_t.vel.pit=ai->data.pitch_vel;
result->gimbal_t.accl.pit=ai->data.pitch_acc;
result->chassis_t.Vx=ai->data.vx;
result->chassis_t.Vy=ai->data.vy;
result->chassis_t.Vw=ai->data.wz;
// }
return DEVICE_OK;
}
}
int8_t AI_ParseHost(AI_t* ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
ai->TX.head[0]='M';
ai->TX.head[1]='R';
// ai->TX.mode=2;
int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle;
ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle;
ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed;
ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed;
ai->TX.q[0]=motor->imu.quat.q0;
ai->TX.q[1]=motor->imu.quat.q1;
ai->TX.q[2]=motor->imu.quat.q2;
ai->TX.q[3]=motor->imu.quat.q3;
mcu->id=ID_MCU;
mcu->data.mode=0;
mcu->data.q[0]=motor->imu.quat.q0;
mcu->data.q[1]=motor->imu.quat.q1;
mcu->data.q[2]=motor->imu.quat.q2;
mcu->data.q[3]=motor->imu.quat.q3;
mcu->data.yaw=motor->imu.eulr.yaw;
mcu->data.yaw_vel=motor->imu.gyro.z;
mcu->data.pitch=motor->imu.eulr.rol;
mcu->data.pitch_vel=motor->imu.gyro.x;
mcu->data.bullet_count=0;
mcu->data.bullet_speed=22;
ai->TX.bullet_count=10;
ai->TX.bullet_speed=22;
ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t), CRC16_INIT );
if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){
mcu->crc16=CRC16_Calc(((const uint8_t*)&(mcu)),sizeof(&mcu)-sizeof(uint16_t), CRC16_INIT );
if(CRC16_Verify(((const uint8_t*)&(mcu)), sizeof(&mcu))!=true){
return DEVICE_ERR;
}
return DEVICE_OK;
}
int8_t AI_StartSend(AI_t *ai) {
if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), true)==HAL_OK)
int8_t MCU_StartSend(PackageMCU_t *mcu) {
if(BSP_UART_Transmit(BSP_UART_1, (uint8_t *)mcu, sizeof(*mcu), true)==HAL_OK)
return DEVICE_OK;
// else
// return DEVICE_ERR;
// } else {
// if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
// (uint8_t *)&(ai->TX),
// sizeof(ai->TX)) == HAL_OK)
// return DEVICE_OK;
// else
// return DEVICE_ERR;
// }
}
// #include "ai.h"
// #include "bsp/uart.h"
// #include "component/crc16.h"
// // void ai_init(AI_t* ai){
// // ai->RX.head[0]="M";
// // ai->RX.head[0]="R";
// // ai->TX.head[0]="M";
// // ai->TX.head[0]="R";
// // }
// int8_t AI_StartReceiving(AI_t *ai) {
// if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), false))
// return DEVICE_OK;
// return DEVICE_ERR;
// }
// int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) {
// if(ai->RX.head[0]!='M'&&ai->RX.head[1]!='R'){
// return DEVICE_ERR;
// }
// // CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16);
// // if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX.crc16))!=true){
// // return DEVICE_ERR;
// // }
// ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch;
// ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw;
// ai_cmd->mode=ai->RX.mode;
// ai_cmd->gimbal_t.accl.pit=ai->RX.pitch_acc;
// ai_cmd->gimbal_t.vel.pit=ai->RX.pitch_vel;
// ai_cmd->gimbal_t.accl.yaw=ai->RX.yaw_acc;
// ai_cmd->gimbal_t.vel.yaw=ai->RX.yaw_vel;
// return DEVICE_OK;
// }
// int8_t AI_ParseHost(AI_t *ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
// ai->TX.head[0]='M';
// ai->TX.head[1]='R';
// // ai->TX.mode=2;
// ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle;
// ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle;
// ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed;
// ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed;
// ai->TX.q[0]=motor->imu.quat.q1;
// ai->TX.q[1]=motor->imu.quat.q2;
// ai->TX.q[2]=motor->imu.quat.q3;
// ai->TX.q[3]=motor->imu.quat.q0;
// ai->TX.bullet_count=10;
// ai->TX.bullet_speed=10;
// ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t),ai->TX.crc16);
// if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){
// return DEVICE_ERR;
// }
// return DEVICE_OK;
// }
// int8_t AI_StartSend(AI_t *ai) {
// // if (ref_update) {
// if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), false))
// return DEVICE_OK;
// // else
// // return DEVICE_ERR;
// // } else {
// // if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
// // (uint8_t *)&(ai->TX),
// // sizeof(ai->TX)) == HAL_OK)
// // return DEVICE_OK;
// // else
// // return DEVICE_ERR;
// // }
// }

179
User/device/ai.h
View File

@ -9,73 +9,162 @@ extern "C" {
#include "component\user_math.h"
#include "remote_control.h"
#include "module/gimbal.h"
#include "stdint.h"
// struct __attribute__((packed)) GimbalToVision
// {
// uint8_t head[2];
// uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
// float q[4]; // wxyz顺序 /q4,q0,q1,q2/
// float yaw;
// float yaw_vel;
// float pitch;
// float pitch_vel;
// float bullet_speed;
// uint16_t bullet_count; // 子弹累计发送次数
// uint16_t crc16;
// };
struct __attribute__((packed)) GimbalToVision
// struct __attribute__((packed)) VisionToGimbal
// {
// uint8_t head[2];
// uint8_t mode; // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火
// float yaw;
// float yaw_vel;
// float yaw_acc;
// float pitch;
// float pitch_vel;
// float pitch_acc;
// uint16_t crc16;
// };
// typedef struct __attribute__((packed)) {
// uint8_t mode;
// struct{
// // Gimbal_CMD_t g_cmd;
// struct{
// float yaw;
// float pit;
// }setpoint;
// struct{
// float pit;
// float yaw;
// }accl;
// struct{
// float pit;
// float yaw;
// }vel;
// }gimbal_t;
// }AI_cmd_t;
// typedef struct __attribute__((packed)) {
// struct GimbalToVision TX;
// struct VisionToGimbal RX;
// }AI_t;
// 数据包 ID 定义
#define ID_MCU (0xC4) // MCU 数据包
#define ID_REF (0xA8) // 裁判系统数据包
#define ID_AI (0xB5) // AI 控制数据包
// MCU 数据结构（MCU -> 上位机）
typedef struct __attribute__((packed))
{
uint8_t head[2];
uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
float q[4]; // wxyz顺序 /q4,q0,q1,q2/
float yaw;
float yaw_vel;
float pitch;
float pitch_vel;
float bullet_speed;
float q[4]; // 四元数 wxyz 顺序
float yaw; // 偏航角
float yaw_vel; // 偏航角速度
float pitch; // 俯仰角
float pitch_vel; // 俯仰角速度
float bullet_speed; // 弹速
uint16_t bullet_count; // 子弹累计发送次数
uint16_t crc16;
};
} DataMCU_t;
struct __attribute__((packed)) VisionToGimbal
typedef struct __attribute__((packed))
{
uint8_t head[2];
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火
float yaw;
float yaw_vel;
float yaw_acc;
float pitch;
float pitch_vel;
float pitch_acc;
uint8_t id; // 数据包 ID: 0xC4
DataMCU_t data;
uint16_t crc16;
};
} PackageMCU_t;
// 裁判系统数据结构
typedef struct __attribute__((packed))
{
uint16_t remain_hp; // 剩余血量
uint8_t game_progress : 4; // 比赛进度
uint16_t stage_remain_time; // 比赛剩余时间
} DataReferee_t;
typedef struct __attribute__((packed))
{
uint8_t id; // 数据包 ID: 0xA8
DataReferee_t data;
uint16_t crc16;
} PackageReferee_t;
// AI 控制数据结构（上位机 -> MCU）
typedef struct __attribute__((packed))
{
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
float yaw; // 目标偏航角
float yaw_vel; // 偏航角速度
float yaw_acc; // 偏航角加速度
float pitch; // 目标俯仰角
float pitch_vel; // 俯仰角速度
float pitch_acc; // 俯仰角加速度
float vx; // X 方向速度
float vy; // Y 方向速度
float wz; // Z 方向角速度
uint8_t reserved; // 预留字段
} DataAI_t;
typedef struct __attribute__((packed))
{
uint8_t id; // 数据包 ID: 0xB5
DataAI_t data;
uint16_t crc16;
} PackageAI_t;
typedef struct __attribute__((packed)) {
uint8_t mode;
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
struct{
// Gimbal_CMD_t g_cmd;
struct{
float yaw;
float pit;
float yaw; // 目标偏航角
float pit; // 目标俯仰角
}setpoint;
struct{
float pit;
float yaw;
float pit; // 俯仰角加速度
float yaw; // 偏航角加速度
}accl;
struct{
float pit;
float yaw;
float pit; // 俯仰角速度
float yaw; // 偏航角速度
}vel;
}gimbal_t;
}AI_cmd_t;
struct{
float Vx; // X 方向速度
float Vy; // Y 方向速度
float Vw; // Z 方向角速度
}chassis_t;
uint8_t reserved; // 预留字段
}AI_result_t; //接收到的所有数据，来自NUC
typedef struct __attribute__((packed)) {
struct GimbalToVision TX;
struct VisionToGimbal RX;
}AI_t;
int8_t AI_StartReceiving(AI_t *ai) ;
int8_t AI_Get_NUC(AI_t * ai,AI_cmd_t* ai_cmd) ;
int8_t AI_ParseHost(AI_t * ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) ;
int8_t AI_StartSend(AI_t *ai) ;
int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat);
int8_t MCU_StartSend(PackageMCU_t *mcu);
int8_t AI_Get_NUC(PackageAI_t *ai,AI_result_t* result);
int8_t AI_StartReceiving(PackageAI_t *ai);
#ifdef __cplusplus
}
#endif

6
User/device/dr16.h
View File

@ -12,11 +12,11 @@
// /* USER INCLUDE BEGIN */
// ///* USER INCLUDE END */
// // ///* USER INCLUDE END */
// /* USER DEFINE BEGIN */
// ///* USER DEFINE END */
// // ///* USER DEFINE END */
// /* Exported constants ------------------------------------------------------- */
// /* Exported macro ----------------------------------------------------------- */
@ -110,7 +110,7 @@
// /* USER FUNCTION BEGIN */
// ///* USER FUNCTION END */
// // ///* USER FUNCTION END */
// #ifdef __cplusplus
// }

13
User/module/chassis.c
View File

@ -73,7 +73,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_mode_t mode ,uint32_t now)
if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
srand(now);
c->wz_multi =1;
c->wz_multi =0.1;
// c->wz_multi = (rand() % 2) ? -1 : 1;
}
for (int i = 0; i < 4; i++)
@ -203,7 +203,7 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
case CHASSIS_MODE_ROTOR:
case CHASSIS_MODE_FOLLOW_GIMBAL:
case CHASSIS_MODE_DAOHANG:
// const double radians = atan(1.0f * 330 / 330);
@ -397,12 +397,17 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
// 跟随云台模式
c->move_vec.Vx =-c_cmd->y_l;
c->move_vec.Vy =-c_cmd->x_l;
c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 0.0627f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
// c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
break;
case CHASSIS_MODE_DAOHANG:
// 导航模式
c->move_vec.Vx = -c_cmd->Vx/10;
c->move_vec.Vy = c_cmd->Vy/10;
c->move_vec.Vw = c_cmd->Vw/10;
break;
default:
return CHASSIS_ERR_MODE; /* 未知模式 */

4
User/module/cmd.c
View File

@ -147,18 +147,22 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd
case CMD_SW_UP:
g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
s_cmd->control_mode = SHOOT_REMOTE;
// c_cmd->mode = CHASSIS_MODE_FOLLOW_GIMBAL;
break;
case CMD_SW_MID:
g_cmd->ctrl_mode = GIMBAL_MODE_AI;
s_cmd->control_mode = SHOOT_AI;
// c_cmd->mode = CHASSIS_MODE_FOLLOW_GIMBAL;
break;
case CMD_SW_DOWN:
g_cmd->ctrl_mode = GIMBAL_MODE_AI;
s_cmd->control_mode = SHOOT_AI;
c_cmd->mode = CHASSIS_MODE_DAOHANG;
break;
case CMD_SW_ERR:
g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
s_cmd->control_mode = SHOOT_REMOTE;
c_cmd->mode = CHASSIS_MODE_FOLLOW_GIMBAL;
break;
}

9
User/module/cmd.h
View File

@ -81,6 +81,7 @@ typedef enum
CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
CHASSIS_MODE_DAOHANG, /* 导航模式，底盘根据导航数据进行移动 */
} Chassis_mode_t;
typedef struct
@ -118,8 +119,12 @@ typedef struct {
float delta_pitch_4310;
float delta_yaw_6020;
GIMBAL_Ctrl_mode_t ctrl_mode;
float set_yaw;
float set_pitch;
float set_yaw; /*自瞄YAW目标值*/
float set_pitch;/*自瞄PITCH目标值*/
float yaw_vel; /*自瞄YAW角速度*/
float yaw_accl; /*自瞄YAW加速度*/
float pit_vel; /*自瞄PITCH角速度*/
float pit_accl; /*自瞄PITCH加速度*/
} Gimbal_CMD_t;
typedef enum {

24
User/module/config.c
View File

@ -174,12 +174,22 @@ static const Config_Param_t config = {
// .d_cutoff_freq = 0.0f,
// .range = M_2PI
.k = 2.0f,
.p = 7.0f,
.i = 0.5f,
.d = 1.0f,
/*双环pid参数*/
// .k = 2.0f,
// .p = 7.0f,
// .i = 0.5f,
// .d = 1.0f,
// .i_limit = 1.0f,
// .out_limit = 1.0f,
// .d_cutoff_freq = 0.0f,
// .range = M_2PI
.k = 1.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 10.0f,
.out_limit = 1.0f,
.d_cutoff_freq = 0.0f,
.range = M_2PI
},
@ -227,6 +237,10 @@ static const Config_Param_t config = {
.low_pass_cutoff_freq={
.out = -1.0f,
.gyro = 20.0f,
},
.K_forward_pid={
.K_vel=0.2f,
.K_accl=0.1f,
}
},

21
User/module/gimbal.c
View File

@ -246,7 +246,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
Gimbal_SetMode(g, g_cmd->mode);
/*处理小yaw轴控制命令*/
float delta_yaw_6020 = 10.0f*g_cmd->delta_yaw_6020 * g->dt;
float delta_yaw_6020 = 20.0f*g_cmd->delta_yaw_6020 * g->dt;
if(g->param->travel.yaw_6020 > 0) // 有限位才处理
{
@ -324,12 +324,18 @@ if(g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE){
else if (g_cmd->ctrl_mode == GIMBAL_MODE_AI) {
g->setpoint.eulr.yaw = g_cmd->set_yaw;
g->setpoint.eulr.pit = g_cmd->set_pitch;
g->setpoint.yaw_vel = g_cmd->yaw_vel;
g->setpoint.yaw_accl = g_cmd->yaw_accl;
g->setpoint.pit_vel = g_cmd->pit_vel;
g->setpoint.pit_accl = g_cmd->pit_accl;
}
yaw_6020_omega_set_point =PID_Calc(&g->pid.yaw_6020_angle,g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw,0.0f,g->dt);
g->out.yaw_6020 = PID_Calc(&g->pid.yaw_6020_omega,yaw_6020_omega_set_point,
g->feedback.imu.gyro.z,0.0f,g->dt);
// g->out.yaw_6020 = g->pid.yaw_6020_omega,yaw_6020_omega_set_point,
// g-PID_Calc(&>feedback.imu.gyro.z,0.0f,g->dt);
g->out.yaw_6020 = yaw_6020_omega_set_point+g->param->K_forward_pid.K_vel*g->setpoint.yaw_vel+g->param->K_forward_pid.K_accl*g->setpoint.yaw_accl;
/*4310大YAW控制
这里是单环控制的，有需要加双环
*/
@ -343,8 +349,9 @@ g->setpoint.eulr.pit = g_cmd->set_pitch;
/*控制云台4310电机 也是单环，但是加了重力补偿函数，可以根据不一样的情况去拟合函数*/
pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol,-g->feedback.imu.gyro.y,g->dt);
g->out.pitch_4310 = PID_Calc(&g->pid.pitch_4310_omega,8*pitch_omega_set_point,
-g->feedback.imu.gyro.y,0.0f,g->dt)+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
g->out.pitch_4310 = pitch_omega_set_point+g->param->K_forward_pid.K_vel*g->setpoint.pit_vel+g->param->K_forward_pid.K_accl*g->setpoint.pit_accl+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
// g->out.pitch_4310 = PID_Calc(&g->pid.pitch_4310_omega,8*pitch_omega_set_point,
// -g->feedback.imu.gyro.y,0.0f,g->dt)+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
// g->out.pitch_4310 = pitch_omega_set_point+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
@ -384,9 +391,9 @@ void Gimbal_Output(Gimbal_t *g)
MOTOR_RM_SetOutput(&g->param->yaw_6020_motor, g->out.yaw_6020);
MOTOR_MIT_Output_t output_yaw_4310 = {0};
output_yaw_4310.torque = g->out.yaw_4310 * 2.5f;
output_yaw_4310.torque = g->out.yaw_4310 * 3.0f;
// output_yaw_4310.kp = 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};

12
User/module/gimbal.h
View File

@ -65,6 +65,12 @@ extern "C"
float pitch_4310; /* pitch轴4310机械限位行程 -1表示无限位*/
} travel;
struct {
float K_vel; /*前馈速度*/
float K_accl; /*前馈加速度*/
}K_forward_pid;
} Gimbal_Param_t;
typedef struct
@ -110,6 +116,12 @@ extern "C"
AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
float yaw_4310; /* 大yaw电机目标角度 */
float yaw_vel; /*yaw自瞄角速度*/
float yaw_accl; /*yaw自瞄加速度*/
float pit_vel; /*pitch自瞄角速度*/
float pit_accl; /*pitch自瞄加速度*/
float NUC_Pitch; /* 自瞄用pitch目标角度 */
float NUC_Yaw; /* 自瞄用yaw目标角度 */
} setpoint;

19
User/task/ai.c
View File

@ -15,10 +15,12 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
AI_t ai;
AI_cmd_t ai_cmd;
PackageAI_t ai;
PackageMCU_t mcu;
AI_result_t ai_result;
AHRS_Quaternion_t quat;
Gimbal_feedback_t gimbal_motor;
PackageMCU_t shoot_mcu_package; /* 新增的 ai 数据包 主要是给自瞄发送射击数量*/
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -42,14 +44,17 @@ void Task_ai(void *argument) {
/* USER CODE BEGIN */
osMessageQueueGet(task_runtime.msgq.gimbal.ai.quat, &quat, NULL, 0);
osMessageQueueGet(task_runtime.msgq.gimbal.ai.feedback, &gimbal_motor, NULL, 0);
AI_ParseHost(&ai,&gimbal_motor,&quat);
AI_StartSend(&ai);
osMessageQueueGet(task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count, &shoot_mcu_package, NULL, 0);
MCU_Send(&mcu,&gimbal_motor,&quat);
mcu.data.bullet_count=shoot_mcu_package.data.bullet_count; /* 从 shoot 任务获取射击数量并放入 mcu 数据包中 */
MCU_StartSend(&mcu);
AI_StartReceiving(&ai);
AI_Get_NUC(&ai,&ai_cmd);
AI_Get_NUC(&ai,&ai_result);
osMessageQueueReset(task_runtime.msgq.gimbal.ai.g_cmd);
osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_cmd, 0, 0);
osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd, &ai_cmd, 0, 0);
osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_result, 0, 0);
osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd, &ai_result, 0, 0);
osMessageQueuePut(task_runtime.msgq.navi.c_cmd, &ai_result, 0, 0); /* 将 ai_result 中的射击命令布尔值发送给 shoot 任务 */
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}

9
User/task/chassis.c
View File

@ -4,6 +4,8 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "device/ai.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "module/chassis.h"
@ -17,6 +19,7 @@
/* USER STRUCT BEGIN */
Chassis_t chassis;
Chassis_CMD_t cmd_chassis;
AI_result_t c_cmd_ai_result; /* 新增的 ai 结果变量 主要是给底盘接收自瞄相关的命令*/
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -41,11 +44,17 @@ chassis_init(&chassis,&Config_GetRobotParam()->chassis,CHASSIS_FREQ);
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
osMessageQueueGet(task_runtime.msgq.navi.c_cmd, &c_cmd_ai_result, NULL, 0);
osMessageQueueGet(task_runtime.msgq.gimbal.yaw6020,&chassis.motorfeedback.gimbal_yaw_encoder,NULL,0);
/*接受cmd任务数据*/
if(osMessageQueueGet(task_runtime.msgq.chassis.cmd, &cmd_chassis, NULL, 0)==osOK)
{
if(cmd_chassis.mode == CHASSIS_MODE_DAOHANG){
cmd_chassis.Vx = c_cmd_ai_result.chassis_t.Vx;
cmd_chassis.Vy = c_cmd_ai_result.chassis_t.Vy;
cmd_chassis.Vw = c_cmd_ai_result.chassis_t.Vw;
}
Chassis_update(&chassis);
Chassis_Control(&chassis, &cmd_chassis,tick);
}else

32
User/task/gimbal.c
View File

@ -16,10 +16,10 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
Gimbal_CMD_t cmd_gimbal; //遥控器
Gimbal_CMD_t cmd_gimbal; //cmd命令关于云台
Gimbal_IMU_t gimbal_imu;
Gimbal_t gimbal;
AI_cmd_t ai_gimbal_cmd;
AI_result_t ai_gimbal_result_cmd; /*ai发送自瞄数据和 导航数据*/
Gimbal_CMD_t final_gimbal_cmd; //最终命令
@ -47,10 +47,10 @@ void Task_gimbal(void *argument) {
osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu,NULL, 0);
Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_cmd,NULL, 0);
osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_result_cmd,NULL, 0);
/* ai指令 */
// if(osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd, &ai_gimbal_cmd, NULL, 0)==osOK){
// if(ai_gimbal_cmd.mode==0){
// if(osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd, &ai_gimbal_result_cmd, NULL, 0)==osOK){
// if(ai_gimbal_result_cmd.mode==0){
// final_gimbal_cmd.set_pitch =gimbal_imu.eulr.rol;
// final_gimbal_cmd.set_yaw=gimbal_imu.eulr.yaw;
// }
@ -64,18 +64,26 @@ osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_cmd,NULL, 0);
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0);
if(ai_gimbal_cmd.mode==0){
if(ai_gimbal_result_cmd.mode==0){
cmd_gimbal.set_pitch =gimbal_imu.eulr.pit;
cmd_gimbal.set_yaw=gimbal_imu.eulr.yaw;
}
if(ai_gimbal_cmd.mode==1)
if(ai_gimbal_result_cmd.mode==1)
{
cmd_gimbal.set_pitch=ai_gimbal_cmd.gimbal_t.setpoint.pit;
cmd_gimbal.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw;
cmd_gimbal.set_pitch=ai_gimbal_result_cmd.gimbal_t.setpoint.pit;
cmd_gimbal.set_yaw=ai_gimbal_result_cmd.gimbal_t.setpoint.yaw;
cmd_gimbal.yaw_vel=ai_gimbal_result_cmd.gimbal_t.vel.yaw;
cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
}
if(ai_gimbal_cmd.mode==2){
cmd_gimbal.set_pitch=ai_gimbal_cmd.gimbal_t.setpoint.pit;
cmd_gimbal.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw;
if(ai_gimbal_result_cmd.mode==2){
cmd_gimbal.set_pitch=ai_gimbal_result_cmd.gimbal_t.setpoint.pit;
cmd_gimbal.set_yaw=ai_gimbal_result_cmd.gimbal_t.setpoint.yaw;
cmd_gimbal.yaw_vel=ai_gimbal_result_cmd.gimbal_t.vel.yaw;
cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
}
Gimbal_UpdateFeedback(&gimbal);

9
User/task/init.c
View File

@ -37,7 +37,7 @@ void Task_Init(void *argument) {
task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
task_runtime.thread.gimbal = osThreadNew(Task_gimbal, NULL, &attr_gimbal);
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);
// 创建消息队列
@ -53,9 +53,10 @@ void Task_Init(void *argument) {
task_runtime.msgq.gimbal.yaw6020 = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_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.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.g_cmd = osMessageQueueNew(2u, sizeof(AI_result_t),NULL);
task_runtime.msgq.shoot.ai.s_cmd = osMessageQueueNew(2u, sizeof(AI_result_t),NULL);
task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count = osMessageQueueNew(2u, sizeof(PackageMCU_t),NULL);
task_runtime.msgq.navi.c_cmd = osMessageQueueNew(2u, sizeof(AI_result_t),NULL); /* 新增的 ai 消息队列 主要是给底盘发送导航相关的命令*/
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核

41
User/task/shoot.c
View File

@ -4,6 +4,7 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "module/cmd.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
@ -19,7 +20,9 @@
/* USER STRUCT BEGIN */
Shoot_t shoot;
Shoot_CMD_t shoot_cmd;
AI_cmd_t shoot_ai_cmd;
AI_result_t shoot_ai_result_cmd;
PackageMCU_t shoot_ai_mcu_package;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -37,40 +40,52 @@ void Task_shoot(void *argument) {
/* USER CODE INIT BEGIN */
Shoot_Init(&shoot,&Config_GetRobotParam()->shoot,SHOOT_FREQ);
Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE);
shoot_ai_mcu_package.data.bullet_count = 0;
static bool last_fire_state = false;
bool current_fire_state = false; // 当前是否需要发射
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
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.shoot.ai.s_cmd, &shoot_ai_result_cmd, NULL, 0);
// if(shoot_cmd.control_mode==SHOOT_REMOTE)
// {
// //do nothing，使用遥控器的指令
// }
// else if(shoot_cmd.control_mode==SHOOT_AI)
// {
if(shoot_cmd.control_mode==SHOOT_REMOTE)
{
//do nothing，使用遥控器的指令
current_fire_state = shoot_cmd.firecmd;
}
else if(shoot_cmd.control_mode==SHOOT_AI)
{
// shoot_cmd.mode = SHOOT_MODE_SINGLE;
// if(shoot_ai_cmd.mode==0 || shoot_ai_cmd.mode==1)
// if(shoot_ai_result_cmd.mode==0 || shoot_ai_result_cmd.mode==1)
// {
// shoot_cmd.firecmd=false;
// shoot_cmd.ready=true;
// }
// else if(shoot_ai_cmd.mode==2)
// else if(shoot_ai_result_cmd.mode==2)
// {
// shoot_cmd.firecmd=true;
// shoot_cmd.ready=true;
// current_fire_state = true;
// }
// }
current_fire_state = shoot_cmd.firecmd;
}
if(current_fire_state == true && last_fire_state == false)
{
shoot_ai_mcu_package.data.bullet_count++; /* 每次射击时增加射击数量 */
}
last_fire_state = current_fire_state;
Shoot_UpdateFeedback(&shoot);
Shoot_SetMode(&shoot,shoot_cmd.mode);
Shoot_Control(&shoot,&shoot_cmd);
osMessageQueueReset(task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count);
osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count, &shoot_ai_mcu_package, 0, 0);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}

5
User/task/user_task.h
View File

@ -102,9 +102,12 @@ typedef struct {
struct{
osMessageQueueId_t s_cmd;
osMessageQueueId_t s_cmd_ai_bool_count; /* 新增的 ai 消息队列 主要是给自瞄发送射击数量*/
}ai;
}shoot;
struct{
osMessageQueueId_t c_cmd; /* 新增的 ai 消息队列 主要是给底盘发送自瞄相关的命令*/
}navi;
} msgq;
/* USER MESSAGE END */

14
lll0121.ioc
View File

@ -24,7 +24,8 @@ Dma.Request2=USART3_RX
Dma.Request3=USART6_RX
Dma.Request4=USART6_TX
Dma.Request5=USART1_RX
Dma.RequestsNb=6
Dma.Request6=USART1_TX
Dma.RequestsNb=7
Dma.SPI1_RX.0.Direction=DMA_PERIPH_TO_MEMORY
Dma.SPI1_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.SPI1_RX.0.Instance=DMA2_Stream2
@ -55,6 +56,16 @@ Dma.USART1_RX.5.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.USART1_RX.5.PeriphInc=DMA_PINC_DISABLE
Dma.USART1_RX.5.Priority=DMA_PRIORITY_VERY_HIGH
Dma.USART1_RX.5.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
Dma.USART1_TX.6.Direction=DMA_MEMORY_TO_PERIPH
Dma.USART1_TX.6.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.USART1_TX.6.Instance=DMA2_Stream7
Dma.USART1_TX.6.MemDataAlignment=DMA_MDATAALIGN_BYTE
Dma.USART1_TX.6.MemInc=DMA_MINC_ENABLE
Dma.USART1_TX.6.Mode=DMA_NORMAL
Dma.USART1_TX.6.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.USART1_TX.6.PeriphInc=DMA_PINC_DISABLE
Dma.USART1_TX.6.Priority=DMA_PRIORITY_LOW
Dma.USART1_TX.6.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
Dma.USART3_RX.2.Direction=DMA_PERIPH_TO_MEMORY
Dma.USART3_RX.2.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.USART3_RX.2.Instance=DMA1_Stream1
@ -154,6 +165,7 @@ NVIC.DMA2_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream6_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream7_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.EXTI0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.EXTI4_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true