Hshine
/
sentry_left
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

新增导航通信测试

This commit is contained in:
宋家齐 2024-03-16 21:12:28 +08:00
parent 9244bea55e
commit da855f7e01
5 changed files with 66 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
application/cmd/robot_cmd.c
View File

@ -129,10 +129,10 @@ static void RemoteControlSet()
}
// 云台参数,确定云台控制数据
if (switch_is_mid(rc_data[TEMP].rc.switch_left) ||
(vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
&& vision_recv_data->vx == 0 && vision_recv_data->vy == 0 &&
vision_recv_data->vz == 0)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制
if (switch_is_mid(rc_data[TEMP].rc.switch_left) )
// (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
// && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 &&
// vision_recv_data->vz == 0)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制
{
// 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制
// ...
@ -145,44 +145,51 @@ static void RemoteControlSet()
}
// 左侧开关状态为[下],视觉模式
if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
trajectory_cal.v0 = 30; //弹速30
if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
&& vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) {
aim_select.suggest_fire = 0;
//未发现目标
no_find_cnt++;
if (no_find_cnt >= 2000) {
//gimbal_scan_flag = 1;
//auto_aim_flag = 0;
}
//else
// if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
// trajectory_cal.v0 = 30; //弹速30
// if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
// && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) {
// aim_select.suggest_fire = 0;
// //未发现目标
// no_find_cnt++;
//
// if (no_find_cnt >= 2000) {
// //gimbal_scan_flag = 1;
// //auto_aim_flag = 0;
// }
// //else
// //auto_aim_flag = 1;
// } else {
// //弹道解算
// no_find_cnt = 0;
// auto_aim_flag = 1;
} else {
//弹道解算
no_find_cnt = 0;
auto_aim_flag = 1;
//
// auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
//
// gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI;
//
// gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
//
// float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
// if (yaw_err <= 0.008) //3度
// aim_select.suggest_fire = 1;
// else
// aim_select.suggest_fire = 0;
// }
// }
auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI;
gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
if (yaw_err <= 0.008) //3度
aim_select.suggest_fire = 1;
else
aim_select.suggest_fire = 0;
}
}
// 云台软件限位
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
chassis_cmd_send.vx = 0.001f * (float)rc_data[TEMP].rc.rocker_r1; // _水平方向
chassis_cmd_send.vy = -0.001f * (float)rc_data[TEMP].rc.rocker_r_; // 1数值方向
if (vision_recv_data->nav_vx != 0 || vision_recv_data->nav_vy != 0 || vision_recv_data->nav_wz != 0) {
chassis_cmd_send.vx = vision_recv_data->nav_vx;
chassis_cmd_send.vy = vision_recv_data->nav_vy;
chassis_cmd_send.wz = vision_recv_data->nav_wz;
}
// 发射参数
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开

12
application/gimbal/gimbal.c
View File

@ -82,17 +82,17 @@ void GimbalInit()
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 2.0f,
.Ki = 0,
.Kd = 0.0f,
.Kp = 2.0f,//4.0f,//2.0f
.Ki = 0.0f,//1,//0
.Kd = 0.0f,//0.0f
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 500,
},
.speed_PID = {
.Kp = 800,
.Ki = 100,
.Kd = 0,
.Kp = 800,//6000,//800
.Ki = 100,//500,//100
.Kd = 0,//0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 5000,
.MaxOut = 10000,

3
modules/master_machine/master_process.c
View File

@ -62,6 +62,8 @@ static void VisionOfflineCallback(void *id)
USARTServiceInit(vision_usart_instance);
#endif // !VISION_USE_UART
LOGWARNING("[vision] vision offline, restart communication.");
memset(&recv_data,0,sizeof(recv_data));
}
#ifdef VISION_USE_UART
@ -141,6 +143,7 @@ static void DecodeVision(uint16_t recv_len)
{
if(VerifyCRC16CheckSum(vis_recv_buff,sizeof(recv_data)))
{
DaemonReload(vision_daemon_instance);
memcpy(&recv_data,vis_recv_buff,sizeof(recv_data));
}
}

25
modules/master_machine/master_process.h
View File

@ -92,17 +92,20 @@ typedef struct {
uint8_t id: 3; // 0-outpost 6-guard 7-base
uint8_t armors_num: 3; // 2-balance 3-outpost 4-normal
uint8_t reserved: 1;
float x;
float y;
float z;
float yaw;
float vx;
float vy;
float vz;
float v_yaw;
float r1;
float r2;
float dz;
// float x;
// float y;
// float z;
// float yaw;
// float vx;
// float vy;
// float vz;
// float v_yaw;
// float r1;
// float r2;
// float dz;
float nav_vx;
float nav_vy;
float nav_wz;
uint16_t checksum;
} RecievePacket_t;
#pragma pack()

4
sentry_left.jdebug
View File

@ -22,12 +22,12 @@ void OnProjectLoad (void) {
//
// Dialog-generated settings
//
Project.AddPathSubstitute ("D:/CLion/Project/sentry_left", "$(ProjectDir)");
Project.AddPathSubstitute ("d:/clion/project/sentry_left", "$(ProjectDir)");
Project.SetDevice ("STM32F407IG");
Project.SetHostIF ("USB", "17935099");
Project.SetTargetIF ("SWD");
Project.SetTIFSpeed ("4 MHz");
Project.AddPathSubstitute ("D:/CLion/Project/sentry_left", "$(ProjectDir)");
Project.AddPathSubstitute ("d:/clion/project/sentry_left", "$(ProjectDir)");
Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd");
Project.AddSvdFile ("$(InstallDir)/Config/Peripherals/STM32F407IG.svd");
//