5.14提交
This commit is contained in:
parent
59d43f4c7b
commit
f09de54055
|
|
@ -17,7 +17,7 @@
|
|||
|
||||
// 私有宏,自动将编码器转换成角度值
|
||||
#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
|
||||
#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
|
||||
#define PITCH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
|
||||
|
||||
/* cmd应用包含的模块实例指针和交互信息存储*/
|
||||
#ifdef GIMBAL_BOARD // 对双板的兼容,条件编译
|
||||
|
|
@ -122,6 +122,60 @@ static void CalcOffsetAngle()
|
|||
#endif
|
||||
}
|
||||
|
||||
#define PITCH_ZERO_ANGLE 195.0f //电机旋转导致的云台相对底盘角度
|
||||
static void pitch_limit()
|
||||
{
|
||||
static float PitchMotorAngle,DeltaPitchAngle;
|
||||
PitchMotorAngle = gimbal_fetch_data.pitch_motor_ecd - PITCH_ZERO_ANGLE;//电机旋转导致的云台相对底盘角度
|
||||
DeltaPitchAngle = gimbal_fetch_data.gimbal_imu_data.Pitch - PitchMotorAngle;
|
||||
gimbal_cmd_send.pitch = LIMIT_MIN_MAX(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE-DeltaPitchAngle, PITCH_MAX_ANGLE-DeltaPitchAngle);
|
||||
}
|
||||
|
||||
static void auto_aim_mode() {
|
||||
trajectory_cal.v0 = 25; //弹速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;
|
||||
|
||||
auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
|
||||
|
||||
VisionSetAim(aim_select.aim_point[0], aim_select.aim_point[1], aim_select.aim_point[2]);
|
||||
|
||||
float single_angle_yaw_now = gimbal_fetch_data.gimbal_imu_data.Yaw;
|
||||
float diff_yaw = trajectory_cal.cmd_yaw * 180 / PI - single_angle_yaw_now;
|
||||
float yaw_err = diff_yaw;
|
||||
|
||||
if(diff_yaw>180)
|
||||
diff_yaw -= 360;
|
||||
else if(diff_yaw<-180)
|
||||
diff_yaw += 360;
|
||||
|
||||
gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle + diff_yaw;
|
||||
|
||||
gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI;
|
||||
|
||||
if (yaw_err <= 3) //3度
|
||||
{
|
||||
aim_select.suggest_fire = 1;
|
||||
}
|
||||
else
|
||||
aim_select.suggest_fire = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 控制输入为遥控器(调试时)的模式和控制量设置
|
||||
*
|
||||
|
|
@ -137,7 +191,7 @@ static void RemoteControlSet()
|
|||
else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘跟随模式
|
||||
{
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
}
|
||||
|
||||
// 云台参数,确定云台控制数据
|
||||
|
|
@ -162,36 +216,7 @@ 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
|
||||
//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_mode();
|
||||
}
|
||||
// 云台软件限位
|
||||
|
||||
|
|
@ -235,18 +260,37 @@ static void MouseKeySet()
|
|||
gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测
|
||||
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10;
|
||||
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速
|
||||
aim_select.suggest_fire = 0;
|
||||
if (rc_data[TEMP].mouse.press_l && (!rc_data[TEMP].mouse.press_r)) // 左键发射模式
|
||||
{
|
||||
case 0:
|
||||
shoot_cmd_send.bullet_speed = 15;
|
||||
break;
|
||||
case 1:
|
||||
shoot_cmd_send.bullet_speed = 18;
|
||||
break;
|
||||
default:
|
||||
shoot_cmd_send.bullet_speed = 30;
|
||||
break;
|
||||
if (shoot_cmd_send.friction_mode == FRICTION_ON) {
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON;
|
||||
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
|
||||
}
|
||||
} else if ((!rc_data[TEMP].mouse.press_l) && (!rc_data[TEMP].mouse.press_r))
|
||||
{
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
|
||||
}
|
||||
if (rc_data[TEMP].mouse.press_r) // 右键自瞄模式
|
||||
{
|
||||
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))
|
||||
{
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
} else {
|
||||
auto_aim_mode();
|
||||
if (aim_select.suggest_fire == 1 && rc_data[TEMP].mouse.press_l &&
|
||||
shoot_cmd_send.friction_mode == FRICTION_ON) {
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON;
|
||||
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
|
||||
} else {
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式
|
||||
{
|
||||
case 0:
|
||||
|
|
@ -355,11 +399,11 @@ static void VTMouseKeySet()
|
|||
|
||||
memcpy(&vt_data[LAST], &vt_data[TEMP], sizeof(VT_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断
|
||||
|
||||
chassis_cmd_send.vx = vt_data[TEMP].key[KEY_PRESS].w * 1500 - vt_data[TEMP].key[KEY_PRESS].s * 1500; // 系数待测
|
||||
chassis_cmd_send.vx = vt_data[TEMP].key[KEY_PRESS].w * 2000 - vt_data[TEMP].key[KEY_PRESS].s * 2000; // 系数待测
|
||||
//chassis_cmd_send.vy = vt_data[TEMP].key[KEY_PRESS].s * 800 - vt_data[TEMP].key[KEY_PRESS].d * 800;
|
||||
|
||||
gimbal_cmd_send.yaw -= (float)vt_data[TEMP].mouse.x / 660 * 5; // 系数待测
|
||||
gimbal_cmd_send.pitch += (float)vt_data[TEMP].mouse.y / 660 * 10;
|
||||
gimbal_cmd_send.yaw -= (float)vt_data[TEMP].mouse.x / 660 * 3; // 系数待测
|
||||
gimbal_cmd_send.pitch -= (float)vt_data[TEMP].mouse.y / 660 * 3;
|
||||
|
||||
|
||||
switch (vt_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
|
||||
|
|
@ -383,6 +427,8 @@ static void VTMouseKeySet()
|
|||
chassis_cmd_send.chassis_mode = CHASSIS_LATERAL;
|
||||
break;
|
||||
}
|
||||
//R键手动刷新UI 发到底盘板自己处理
|
||||
chassis_cmd_send.UI_refresh = vt_data[TEMP].key_count[KEY_PRESS][Key_R];
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -395,7 +441,8 @@ static void VTMouseKeySet()
|
|||
static void EmergencyHandler()
|
||||
{
|
||||
// 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正
|
||||
if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
|
||||
if ((rc_data[TEMP].rc.dial > 300 || chassis_fetch_data.power_management_chassis_output == 0) //死亡状态 急停
|
||||
|| robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
|
||||
{
|
||||
robot_state = ROBOT_STOP;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
|
||||
|
|
@ -440,13 +487,16 @@ void RobotCMDTask()
|
|||
MouseKeySet();
|
||||
}
|
||||
|
||||
else if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
|
||||
else if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[中],图传链路
|
||||
VTMouseKeySet();
|
||||
|
||||
|
||||
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
|
||||
|
||||
// 设置视觉发送数据,还需增加加速度和角速度数据
|
||||
// VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed)
|
||||
pitch_limit();
|
||||
//设置视觉识别颜色
|
||||
VisionSetFlag(chassis_fetch_data.enemy_color);
|
||||
gimbal_cmd_send.chassis_rotate_wz = chassis_fetch_data.real_wz;
|
||||
|
||||
// 推送消息,双板通信,视觉通信等
|
||||
// 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
|
||||
|
|
@ -458,5 +508,4 @@ void RobotCMDTask()
|
|||
#endif // GIMBAL_BOARD
|
||||
PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
|
||||
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
|
||||
VisionSend(&vision_send_data);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -50,8 +50,8 @@ void GimbalInit()
|
|||
.MaxOut = 500,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 1e4, // 50
|
||||
.Ki = 5000, // 200
|
||||
.Kp = 8000,//1e4, // 50 @todo:有疯转现象 先降低试试看
|
||||
.Ki = 0,//5000, // 200
|
||||
.Kd = 0,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 10000,
|
||||
|
|
@ -201,7 +201,7 @@ void GimbalTask()
|
|||
// 设置反馈数据,主要是imu和yaw的ecd
|
||||
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
|
||||
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
|
||||
gimbal_feedback_data.pitch_motor_ecd = pitch_motor->measure.angle_single_round + pitch_motor->motor_controller.angle_PID.Err;
|
||||
gimbal_feedback_data.pitch_motor_ecd = pitch_motor->measure.angle_single_round;
|
||||
|
||||
|
||||
// 推送消息
|
||||
|
|
|
|||
|
|
@ -29,8 +29,8 @@
|
|||
#define YAW_CHASSIS_ALIGN_ECD 5046 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
||||
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
|
||||
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
||||
#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
#define PITCH_MAX_ANGLE 23.0f // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
#define PITCH_MIN_ANGLE -30.0f // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
|
||||
#define PITCH_MAX_ECD 5100 * ECD_ANGLE_COEF_DJI
|
||||
#define PITCH_MIN_ECD 3900 * ECD_ANGLE_COEF_DJI
|
||||
|
|
@ -146,10 +146,15 @@ typedef struct
|
|||
float vy; // 横移方向速度
|
||||
float wz; // 旋转速度
|
||||
float offset_angle; // 底盘和归中位置的夹角
|
||||
float leg_length; // 腿长
|
||||
chassis_mode_e chassis_mode;
|
||||
int chassis_speed_buff;
|
||||
// UI部分
|
||||
// ...
|
||||
shoot_mode_e shoot_mode; // 发射模式设置
|
||||
friction_mode_e friction_mode; // 摩擦轮关闭
|
||||
|
||||
uint8_t UI_refresh;
|
||||
|
||||
} Chassis_Ctrl_Cmd_s;
|
||||
|
||||
|
|
@ -186,14 +191,14 @@ typedef struct
|
|||
#if defined(CHASSIS_BOARD) || defined(GIMBAL_BOARD) // 非单板的时候底盘还将imu数据回传(若有必要)
|
||||
// attitude_t chassis_imu_data;
|
||||
#endif
|
||||
// 后续增加底盘的真实速度
|
||||
// 后续增加底盘的真实速度
|
||||
// float real_vx;
|
||||
// float real_vy;
|
||||
// float real_wz;
|
||||
// 底盘旋转速度
|
||||
float real_wz;
|
||||
|
||||
uint8_t rest_heat; // 剩余枪口热量
|
||||
Bullet_Speed_e bullet_speed; // 弹速限制
|
||||
Enemy_Color_e enemy_color; // 0 for blue, 1 for red
|
||||
uint8_t power_management_chassis_output;
|
||||
|
||||
} Chassis_Upload_Data_s;
|
||||
|
||||
|
|
|
|||
|
|
@ -83,9 +83,9 @@ __attribute__((noreturn)) void StartMOTORTASK(void const *argument)
|
|||
motor_start = DWT_GetTimeline_ms();
|
||||
MotorControlTask();
|
||||
motor_dt = DWT_GetTimeline_ms() - motor_start;
|
||||
if (motor_dt > 1)
|
||||
if (motor_dt > 5)
|
||||
LOGERROR("[freeRTOS] MOTOR Task is being DELAY! dt = [%f]", &motor_dt);
|
||||
osDelay(1);
|
||||
osDelay(5);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -119,9 +119,9 @@ __attribute__((noreturn)) void StartROBOTTASK(void const *argument)
|
|||
robot_start = DWT_GetTimeline_ms();
|
||||
RobotTask();
|
||||
robot_dt = DWT_GetTimeline_ms() - robot_start;
|
||||
if (robot_dt > 1)
|
||||
if (robot_dt > 2)
|
||||
LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt);
|
||||
osDelay(1);
|
||||
osDelay(2);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -52,10 +52,10 @@ void ShootInit()
|
|||
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||
},
|
||||
.motor_type = M3508};
|
||||
friction_config.can_init_config.tx_id = 1,
|
||||
friction_config.can_init_config.tx_id = 2,
|
||||
friction_l = DJIMotorInit(&friction_config);
|
||||
|
||||
friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
|
||||
friction_config.can_init_config.tx_id = 1; // 右摩擦轮,改txid和方向就行
|
||||
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
friction_r = DJIMotorInit(&friction_config);
|
||||
|
||||
|
|
@ -221,8 +221,8 @@ void ShootTask()
|
|||
// DJIMotorSetRef(friction_r, 0);
|
||||
// break;
|
||||
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
|
||||
DJIMotorSetRef(friction_l, 30000);
|
||||
DJIMotorSetRef(friction_r, 30000);
|
||||
DJIMotorSetRef(friction_l, 45000);
|
||||
DJIMotorSetRef(friction_r, 45000);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -31,6 +31,7 @@ void OnProjectLoad (void) {
|
|||
//
|
||||
// User settings
|
||||
//
|
||||
Edit.SysVar (VAR_HSS_SPEED, FREQ_500_HZ);
|
||||
Project.SetOSPlugin ("FreeRTOSPlugin_CM4");
|
||||
File.Open ("$(ProjectDir)/cmake-build-debug/wheel_legged_gimbal.elf");
|
||||
|
||||
|
|
|
|||
|
|
@ -1,34 +1,33 @@
|
|||
|
||||
|
||||
GraphedExpression="((vt_data[0]).mouse).x", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#e56a6f
|
||||
OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/cmd/robot_cmd.c", Line=89
|
||||
OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/cmd/robot_cmd.c", Line=6
|
||||
OpenDocument="main.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Src/main.c", Line=70
|
||||
OpenDocument="dji_motor.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/motor/DJImotor/dji_motor.c", Line=137
|
||||
OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/bsp/dwt/bsp_dwt.c", Line=101
|
||||
OpenDocument="tasks.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3414
|
||||
OpenDocument="tasks.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3641
|
||||
OpenDocument="SEGGER_RTT_printf.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Middlewares/Third_Party/SEGGER/RTT/SEGGER_RTT_printf.c", Line=108
|
||||
OpenDocument="shoot.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/shoot/shoot.c", Line=99
|
||||
OpenDocument="robot.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/robot.c", Line=8
|
||||
OpenDocument="master_process.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/master_machine/master_process.c", Line=155
|
||||
OpenDocument="master_process.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/master_machine/master_process.c", Line=154
|
||||
OpenDocument="stm32f4xx_hal_spi.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c", Line=1274
|
||||
OpenDocument="gimbal.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/gimbal/gimbal.c", Line=125
|
||||
OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/wheel_legged_gimbal/Startup/startup_stm32f407ighx.s", Line=52
|
||||
OpenDocument="gimbal.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/gimbal/gimbal.c", Line=192
|
||||
OpenDocument="controller.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/algorithm/controller.c", Line=57
|
||||
OpenToolbar="Debug", Floating=0, x=0, y=0
|
||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=695, h=394, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=569, h=278, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=695, h=226, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=128, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x0
|
||||
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=569, h=358, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=986, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=695, h=367, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=569, h=291, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=695, h=253, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=184, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x0
|
||||
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=569, h=345, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=950, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=264, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=695, h=394, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="313;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="326;0"
|
||||
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=539, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=695, h=367, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="329;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="343;0"
|
||||
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=519, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
||||
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;456]
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[233;100;100;100;784]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((vt_data[0]).mouse).x"], ColWidths=[100;100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Type", SortOrder="DESCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[359;100;100;100;100;102;100;107;107]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Type", SortOrder="DESCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[359;100;100;100;100;100;100;107;107]
|
||||
TableHeader="Power Sampling", SortCol="Index", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||
TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[166;134;100;148]
|
||||
|
|
@ -49,4 +48,7 @@ WatchedExpression="GRAVITY_FEED", Window=Watched Data 1
|
|||
WatchedExpression="rc_data", Window=Watched Data 1
|
||||
WatchedExpression="gimbal_cmd_send", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="vt_data", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="chassis_cmd_send", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="chassis_cmd_send", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="gimbal_feedback_data", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="(((yaw_motor)->motor_controller).speed_PID).Measure", Window=Watched Data 1
|
||||
WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1
|
||||
|
|
@ -1,9 +1,4 @@
|
|||
//
|
||||
// Created by SJQ on 2024/1/26.
|
||||
//
|
||||
|
||||
#include "auto_aim.h"
|
||||
//
|
||||
// Created by sph on 2024/1/21.
|
||||
//
|
||||
#include "auto_aim.h"
|
||||
|
|
@ -15,7 +10,7 @@
|
|||
* @param[in] trajectory_cal:弹道解算结构体
|
||||
* @retval 返回空
|
||||
*/
|
||||
void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
|
||||
int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
|
||||
aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time;
|
||||
aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time;
|
||||
|
||||
|
|
@ -35,14 +30,22 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
|
|||
aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * PI;
|
||||
}
|
||||
|
||||
float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
|
||||
|
||||
//因为是平衡步兵 只需判断两块装甲板即可
|
||||
float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw);
|
||||
if (temp_yaw_diff < yaw_diff_min) {
|
||||
yaw_diff_min = temp_yaw_diff;
|
||||
// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
|
||||
//
|
||||
// //因为是平衡步兵 只需判断两块装甲板即可
|
||||
// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw);
|
||||
// if (temp_yaw_diff < yaw_diff_min) {
|
||||
// yaw_diff_min = temp_yaw_diff;
|
||||
// idx = 1;
|
||||
// }
|
||||
// 平衡步兵选择两块装甲板中较近的装甲板
|
||||
float distance_min = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2);
|
||||
float distance_temp = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2);
|
||||
if (distance_temp < distance_min) {
|
||||
distance_min = distance_temp;
|
||||
idx = 1;
|
||||
}
|
||||
|
||||
} else if (aim_sel->target_state.armor_num == 3)//前哨站
|
||||
{
|
||||
for (i = 0; i < 3; i++) {
|
||||
|
|
@ -60,15 +63,71 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
|
|||
use_1 = !use_1;
|
||||
}
|
||||
|
||||
//计算枪管到目标装甲板yaw最小的那个装甲板
|
||||
float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
|
||||
// //计算枪管到目标装甲板yaw最小的那个装甲板
|
||||
// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
|
||||
// for (i = 1; i < 3; i++) {
|
||||
// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
|
||||
// if (temp_yaw_diff < yaw_diff_min) {
|
||||
// yaw_diff_min = temp_yaw_diff;
|
||||
// idx = i;
|
||||
// }
|
||||
// }
|
||||
// 选择两块较近的装甲板，再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
|
||||
// float distance_temp0 = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2);
|
||||
// float distance_temp1 = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2);
|
||||
// float distance_temp2 = powf(aim_sel->armor_pose[2].x, 2) + powf(aim_sel->armor_pose[2].y, 2);
|
||||
|
||||
// int label_first = 0;
|
||||
// int label_second = 1;
|
||||
// if (distance_temp0 > distance_temp1) {
|
||||
// label_first = 1;
|
||||
// if (distance_temp0 > distance_temp2) {
|
||||
// label_second = 2;
|
||||
// } else label_second = 0;
|
||||
// } else {
|
||||
// label_first = 0;
|
||||
// if (distance_temp1 > distance_temp2) {
|
||||
// label_second = 2;
|
||||
// } else label_second = 1;
|
||||
// }
|
||||
|
||||
// 选择两块较近的装甲板
|
||||
float distance[3];
|
||||
for (i = 0; i < 3; i++) {
|
||||
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
|
||||
}
|
||||
|
||||
int label_first = 0;
|
||||
int label_second = 0;
|
||||
|
||||
for (i = 1; i < 3; i++) {
|
||||
float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
|
||||
if (temp_yaw_diff < yaw_diff_min) {
|
||||
yaw_diff_min = temp_yaw_diff;
|
||||
idx = i;
|
||||
if (distance[i] < distance[label_first]) {
|
||||
label_second = label_first;
|
||||
label_first = i;
|
||||
} else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
|
||||
label_second = i;
|
||||
}else if (distance[i] < distance[label_second]) {
|
||||
label_second = i;
|
||||
}
|
||||
}
|
||||
|
||||
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
|
||||
float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
|
||||
|
||||
float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
|
||||
aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) /
|
||||
(sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) +
|
||||
powf(aim_sel->armor_pose[label_first].y, 2)) * center_length);
|
||||
float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x +
|
||||
aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) /
|
||||
(sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) +
|
||||
powf(aim_sel->armor_pose[label_second].y, 2)) * center_length);
|
||||
|
||||
if (cos_theta_first > cos_theta_second)
|
||||
idx = label_first;
|
||||
else
|
||||
idx = label_second;
|
||||
|
||||
} else {
|
||||
for (i = 0; i < 4; i++) {
|
||||
float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0;
|
||||
|
|
@ -82,19 +141,70 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
|
|||
use_1 = !use_1;
|
||||
}
|
||||
|
||||
//计算枪管到目标装甲板yaw最小的那个装甲板
|
||||
float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
|
||||
for (i = 1; i < 4; i++) {
|
||||
float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
|
||||
if (temp_yaw_diff < yaw_diff_min) {
|
||||
yaw_diff_min = temp_yaw_diff;
|
||||
idx = i;
|
||||
}
|
||||
// //计算枪管到目标装甲板yaw最小的那个装甲板
|
||||
// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
|
||||
// for (i = 1; i < 4; i++) {
|
||||
// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
|
||||
// if (temp_yaw_diff < yaw_diff_min) {
|
||||
// yaw_diff_min = temp_yaw_diff;
|
||||
// idx = i;
|
||||
// }
|
||||
// }
|
||||
|
||||
// 选择两块较近的装甲板
|
||||
float distance[4];
|
||||
for (i = 0; i < 4; i++) {
|
||||
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
|
||||
}
|
||||
|
||||
int label_first = 0;
|
||||
int label_second = 1;
|
||||
|
||||
if(distance[label_first] > distance[label_second])
|
||||
{
|
||||
label_first = 1;
|
||||
label_second = 0;
|
||||
}
|
||||
|
||||
if(distance[2]<distance[label_first]){
|
||||
label_second = label_first;
|
||||
label_first = 2;
|
||||
}
|
||||
else if(distance[2]<distance[label_second])
|
||||
label_second = 2;
|
||||
|
||||
|
||||
if(distance[3]<distance[label_first]){
|
||||
label_second = label_first;
|
||||
label_first = 3;
|
||||
}
|
||||
else if(distance[3]<distance[label_second])
|
||||
label_second = 3;
|
||||
|
||||
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
|
||||
float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
|
||||
|
||||
float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
|
||||
aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) /
|
||||
(sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) +
|
||||
powf(aim_sel->armor_pose[label_first].y, 2)) * center_length);
|
||||
float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x +
|
||||
aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) /
|
||||
(sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) +
|
||||
powf(aim_sel->armor_pose[label_second].y, 2)) * center_length);
|
||||
|
||||
if (cos_theta_first > cos_theta_second)
|
||||
idx = label_first;
|
||||
else
|
||||
idx = label_second;
|
||||
}
|
||||
|
||||
idx = 0;
|
||||
|
||||
aim_sel->aim_point[0] = aim_sel->armor_pose[idx].x + aim_sel->target_state.vx * aim_sel->delay_time;
|
||||
aim_sel->aim_point[1] = aim_sel->armor_pose[idx].y + aim_sel->target_state.vy * aim_sel->delay_time;
|
||||
aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + aim_sel->target_state.vz * aim_sel->delay_time;
|
||||
return idx;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -105,7 +215,7 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
|
|||
* @param[in] v_x0:弹速水平分量
|
||||
* @retval 返回空
|
||||
*/
|
||||
const float k1 = 0.0761; //标准大气压25度下空气阻力系数（小弹丸）
|
||||
const float k1 = 0.0761; //标准大气压25度下空气阻力系数
|
||||
float get_fly_time(float x, float vx, float v_x0) {
|
||||
float t = 0;
|
||||
float f_ti = 0, df_ti = 0;
|
||||
|
|
@ -162,8 +272,8 @@ void get_cmd_angle(Trajectory_Type_t *trajectory_cal) {
|
|||
trajectory_cal->cmd_pitch = trajectory_cal->theta_k;
|
||||
}
|
||||
|
||||
void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) {
|
||||
trajectory_cal->extra_delay_time = 0.025;//0.025
|
||||
int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) {
|
||||
trajectory_cal->extra_delay_time = 0.035;//0.025
|
||||
|
||||
aim_sel->target_state.armor_type = receive_packet->id;
|
||||
aim_sel->target_state.armor_num = receive_packet->armors_num;
|
||||
|
|
@ -179,7 +289,7 @@ void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, Rec
|
|||
aim_sel->target_state.r2 = receive_packet->r2;
|
||||
aim_sel->target_state.dz = receive_packet->dz;
|
||||
|
||||
aim_armor_select(aim_sel, trajectory_cal);
|
||||
int idx = aim_armor_select(aim_sel, trajectory_cal);
|
||||
|
||||
trajectory_cal->position_xy[0] = aim_sel->aim_point[0];
|
||||
trajectory_cal->position_xy[1] = aim_sel->aim_point[1];
|
||||
|
|
@ -189,5 +299,5 @@ void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, Rec
|
|||
trajectory_cal->velocity[2] = aim_sel->target_state.vz;
|
||||
|
||||
get_cmd_angle(trajectory_cal);
|
||||
|
||||
return idx;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,14 +1,10 @@
|
|||
//
|
||||
// Created by SJQ on 2024/1/26.
|
||||
//
|
||||
|
||||
#ifndef WHEEL_LEGGED_GIMBAL_AUTO_AIM_H
|
||||
#define WHEEL_LEGGED_GIMBAL_AUTO_AIM_H
|
||||
|
||||
//
|
||||
// Created by sph on 2024/1/21.
|
||||
//
|
||||
|
||||
#ifndef BASIC_FRAMEWORK_AUTO_AIM_H
|
||||
#define BASIC_FRAMEWORK_AUTO_AIM_H
|
||||
|
||||
#include "master_process.h"
|
||||
//弹道解算
|
||||
typedef struct
|
||||
|
|
@ -73,11 +69,9 @@ typedef struct
|
|||
}Aim_Select_Type_t;
|
||||
|
||||
|
||||
void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal);
|
||||
int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal);
|
||||
float get_fly_time(float x, float vx, float v_x0);
|
||||
void get_cmd_angle(Trajectory_Type_t *trajectory_cal);
|
||||
void auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet);
|
||||
int auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet);
|
||||
|
||||
|
||||
|
||||
#endif //WHEEL_LEGGED_GIMBAL_AUTO_AIM_H
|
||||
#endif //BASIC_FRAMEWORK_AUTO_AIM_H
|
||||
|
|
|
|||
|
|
@ -177,8 +177,7 @@ void VisionSend()
|
|||
static uint8_t send_buffer[24]={0};
|
||||
|
||||
send_data.header = 0x5A;
|
||||
//VisionSetFlag(1);
|
||||
VisionSetAim(recv_data.x,recv_data.y,recv_data.z);
|
||||
|
||||
send_data.checksum = crc_16(&send_data.header,sizeof(send_data)-2);
|
||||
|
||||
memcpy(send_buffer,&send_data,sizeof(send_data));
|
||||
|
|
|
|||
Loading…
Reference in New Issue