zyx
/
infantry_gimbal
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

弹舱盖，完善运动滤波，UI，识别自瞄UI联盟赛版本

This commit is contained in:
zyxxj 2024-04-02 13:33:35 +08:00
parent 5a320042a0
commit 1fd8961204
25 changed files with 6981 additions and 6877 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
application/chassis/chassis.c
View File

@ -47,8 +47,8 @@ static Subscriber_t *chassis_sub; // 用于订阅底盘的控
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
static Chassis_Ctrl_Cmd_s Chassis_Power_Mx;
const static float CHASSIS_ACCEL_X_NUM=0.1666666667f;
const static float CHASSIS_ACCEL_Y_NUM=0.3333333333f;
const static float CHASSIS_ACCEL_X_NUM=0.1f;
const static float CHASSIS_ACCEL_Y_NUM=0.1f;
@ -124,8 +124,8 @@ void ChassisInit() {
//用一阶滤波代替斜波函数生成
first_order_filter_init(&vx_filter, 0.002f, &CHASSIS_ACCEL_X_NUM);
first_order_filter_init(&vx_filter, 0.002f, &CHASSIS_ACCEL_Y_NUM);
// PowerMeter_Init_Config_s
first_order_filter_init(&vy_filter, 0.002f, &CHASSIS_ACCEL_Y_NUM);
// PowerMeter_Init_Config_s
// 发布订阅初始化,如果为双板,则需要can comm来传递消息
#ifdef CHASSIS_BOARD
@ -197,13 +197,22 @@ float vofa_send_data[6];
*/
static void LimitChassisOutput()
{
float Plimit = 1.0f;
float P_cmd = motor_rf->motor_controller.motor_power_predict +
motor_rb->motor_controller.motor_power_predict +
motor_lb->motor_controller.motor_power_predict +
motor_lf->motor_controller.motor_power_predict + 3.6f;
float K = (float)(chassis_cmd_recv.chassis_power_limit - 5) / P_cmd;
//vofa_send_data[2] = P_cmd;
if(chassis_cmd_recv.buffer_energy<50&&chassis_cmd_recv.buffer_energy>=40) Plimit=0.9f;
else if(chassis_cmd_recv.buffer_energy<40&&chassis_cmd_recv.buffer_energy>=35) Plimit=0.75f;
else if(chassis_cmd_recv.buffer_energy<35&&chassis_cmd_recv.buffer_energy>=30) Plimit=0.5f;
else if(chassis_cmd_recv.buffer_energy<30&&chassis_cmd_recv.buffer_energy>=20) Plimit=0.25f;
else if(chassis_cmd_recv.buffer_energy<20&&chassis_cmd_recv.buffer_energy>=10) Plimit=0.125f;
else if(chassis_cmd_recv.buffer_energy<10&&chassis_cmd_recv.buffer_energy>=0) Plimit=0.05f;
else if(chassis_cmd_recv.buffer_energy==60) Plimit=1.0f;
float K = ((float)(chassis_cmd_recv.chassis_power_limit - 3) * (Plimit + 0.50f) ) / P_cmd;
//float K = (float)(chassis_cmd_recv.chassis_power_limit - 5) / P_cmd;
motor_rf->motor_controller.motor_power_scale = K;
motor_rb->motor_controller.motor_power_scale = K;
@ -276,11 +285,21 @@ void ChassisTask() {
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
//一阶低通滤波计算
first_order_filter_cali(&vx_filter, chassis_cmd_recv.vx);
first_order_filter_cali(&vy_filter, chassis_cmd_recv.vy);
chassis_cmd_recv.vx = vx_filter.out;
chassis_cmd_recv.vy = vy_filter.out;
chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
//一阶低通滤波计算
first_order_filter_cali(&vx_filter, chassis_vx);
first_order_filter_cali(&vy_filter, chassis_vy);
// chassis_vx = (1.0f - 0.30f) * chassis_vx + 0.30f * (chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta);
// chassis_vy = (1.0f - 0.30f) * chassis_vy + 0.30f * (chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta);
// 根据控制模式进行正运动学解算,计算底盘输出
//MecanumCalculate();
OmniCalculate();

21
application/cmd/robot_cmd.c
View File

@ -51,6 +51,7 @@ static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息
static Publisher_t *shoot_cmd_pub; // 发射控制消息发布者
static Subscriber_t *shoot_feed_sub; // 发射反馈信息订阅者
static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息
static Shoot_Ctrl_Cmd_s shoot_cmd_recv;
static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
static Robot_Status_e robot_state; // 机器人整体工作状态
@ -124,6 +125,8 @@ static void update_ui_data() {
ui_data.gimbal_mode = gimbal_cmd_send.gimbal_mode;
ui_data.friction_mode = shoot_cmd_send.friction_mode;
ui_data.shoot_mode = shoot_cmd_send.shoot_mode;
ui_data.lid_mode = shoot_cmd_send.lid_mode;
ui_data.aim_fire = aim_select.suggest_fire;
ui_data.Chassis_Power_Data.chassis_power_mx = referee_data->GameRobotState.chassis_power_limit;
}
@ -249,6 +252,8 @@ static void RemoteControlSet() {
// 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
shoot_cmd_send.shoot_rate = 8;
if(shoot_fetch_data.stalled_flag ==1)
shoot_cmd_send.load_mode = LOAD_REVERSE;
}
static void hand_aim_mode() {
@ -268,7 +273,7 @@ static void MouseKeySet() {
gimbal_cmd_send.yaw -= (float) rc_data[TEMP].mouse.x / 660 * 4; // 系数待测
gimbal_cmd_send.pitch += (float) rc_data[TEMP].mouse.y / 660 * 6;
aim_select.suggest_fire = 0;
if (rc_data[TEMP].mouse.press_l && (!rc_data[TEMP].mouse.press_r)) // 左键发射模式
{
if (shoot_cmd_send.friction_mode == FRICTION_ON) {
@ -330,6 +335,15 @@ static void MouseKeySet() {
shoot_cmd_send.friction_mode = FRICTION_ON;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关弹舱盖
{
case 0:
shoot_cmd_send.lid_mode = LID_OPEN;
break;
default:
shoot_cmd_send.lid_mode = LID_CLOSE;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
{
case 0:
@ -354,6 +368,9 @@ static void MouseKeySet() {
}
shoot_cmd_send.shoot_rate = 8;
if(shoot_fetch_data.stalled_flag ==1)
shoot_cmd_send.load_mode = LOAD_REVERSE;
}
/**
@ -411,6 +428,8 @@ void RobotCMDTask() {
//根据裁判系统反馈确定底盘功率上限
chassis_cmd_send.chassis_power_limit = referee_data->GameRobotState.chassis_power_limit;
//根据裁判系统反馈确定缓冲功率
chassis_cmd_send.buffer_energy = referee_data->PowerHeatData.buffer_energy;
// 推送消息,双板通信,视觉通信等
// 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置

30
application/gimbal/gimbal.c
View File

@ -50,20 +50,20 @@ void GimbalInit() {
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 1.5f,//4
.Kp = 1.2f,//4
.Ki = 0.0f,
.Kd = 0.1f,
.DeadBand = 0.1f,
.Kd = 0.05f,
.DeadBand = 0.0f,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 500,
},
.speed_PID = {
.Kp = 3300, // 2480
.Ki = 0.0f, // 200
.Kd = 2.0f,
.Kp = 4000, // 2480
.Ki = 100.0f, // 200
.Kd = 0.0f,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 1200,
.IntegralLimit = 10000,
.MaxOut = 20000,
},
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
@ -83,24 +83,24 @@ void GimbalInit() {
// PITCH
Motor_Init_Config_s pitch_config = {
.can_init_config = {
.can_handle = &hcan2,
.tx_id = 1,
.can_handle = &hcan1,
.tx_id = 2,
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 2.5f,
.Kp = 0.9f,
.Ki = 0.0f,
.Kd = 0.1f,
.Kd = 0.0f,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 500,
},
.speed_PID = {
.Kp = 2900,
.Ki = 0.0f,
.Kd = 0.01f, // 0
.Kp = 6000.0f,
.Ki = 900.0f,
.Kd = 0.0f, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 2500,
.IntegralLimit = 10000,
.MaxOut = 20000,
},
.other_angle_feedback_ptr = &gimba_IMU_data->Roll,

3
application/robot_def.h
View File

@ -29,7 +29,7 @@
#define YAW_CHASSIS_ALIGN_ECD 6822 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define PITCH_HORIZON_ECD 4422 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE 20 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MAX_ANGLE 18 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MIN_ANGLE -30 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MAX_RELATIVE_ANGLE 123 // 云台相对底盘最大角度
@ -153,6 +153,7 @@ typedef struct
int chassis_speed_buff;
uint16_t chassis_power_limit;
uint16_t buffer_energy;
// UI部分
// ...

42
application/shoot/shoot.c
View File

@ -5,10 +5,11 @@
#include "message_center.h"
#include "bsp_dwt.h"
#include "general_def.h"
#include "servo_motor.h"
/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
// static servo_instance *lid; 需要增加弹舱盖
static ServoInstance *lid;
static Publisher_t *shoot_pub;
static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息
@ -46,7 +47,6 @@ void ShootInit()
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
@ -102,6 +102,15 @@ void ShootInit()
shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
Servo_Init_Config_s lid_config = {
.htim = &htim1,
.Channel = TIM_CHANNEL_1,
.Servo_type = Servo180,
.Servo_Angle_Type = Free_Angle_mode,
};
lid = ServoInit(&lid_config);
}
//卡弹检测
void stalled_detect()
@ -110,29 +119,33 @@ void stalled_detect()
static float last_total_angle = 0;
static uint8_t stalled_cnt = 0;
last_detect_time = detect_time;
detect_time = DWT_GetTimeline_ms();
//last_detect_time + 200 > detect_time
if(detect_time - last_detect_time < 200) // 200ms 检测一次
return;
// reference_angle = (detect_time - last_detect_time) * loader->motor_controller.speed_PID.Ref * 1e-3f;
// real_angle = loader->measure.total_angle - last_total_angle;
if(shoot_cmd_recv.load_mode == LOAD_BURSTFIRE)
{
float reference_angle = (detect_time - last_detect_time) * loader->motor_controller.pid_ref * 1e3f;
float real_angle = loader->measure.total_angle - last_total_angle;
if(real_angle < reference_angle * 0.2f)
//if(real_angle < reference_angle * 0.2f)
if(abs(loader->measure.real_current)>=9500)
{
//stalled_cnt ++;
shoot_feedback_data.stalled_flag = 1;
}
last_detect_time = DWT_GetTimeline_ms();
}
// last_detect_time = DWT_GetTimeline_ms();
// last_total_angle = loader->measure.total_angle;
}
/* 机器人发射机构控制核心任务 */
void ShootTask()
{
//从cmd获取控制数据
//从cmd获取控制数据
SubGetMessage(shoot_sub, &shoot_cmd_recv);
// 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
@ -187,9 +200,9 @@ void ShootTask()
// 也有可能需要从switch-case中独立出来
case LOAD_REVERSE:
DJIMotorOuterLoop(loader, SPEED_LOOP);
DJIMotorSetRef(loader, 8 * 360 * REDUCTION_RATIO_LOADER / 8); // 固定速度反转
DJIMotorSetRef(loader, -8 * 360 * REDUCTION_RATIO_LOADER / 8); // 固定速度反转
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 500; // 翻转500ms
dead_time = 100; // 翻转500ms
shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志
// ...
break;
@ -216,9 +229,9 @@ void ShootTask()
// DJIMotorSetRef(friction_l, 0);
// DJIMotorSetRef(friction_r, 0);
// break;
//default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
DJIMotorSetRef(friction_l, 34000);
DJIMotorSetRef(friction_r, 34000);
//default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
DJIMotorSetRef(friction_l, 39000);
DJIMotorSetRef(friction_r, 39000);
// break;
// }
}
@ -231,11 +244,12 @@ void ShootTask()
// 开关弹舱盖
if (shoot_cmd_recv.lid_mode == LID_CLOSE)
{
Servo_Motor_FreeAngle_Set(lid,95);
//...
}
else if (shoot_cmd_recv.lid_mode == LID_OPEN)
{
//...
Servo_Motor_FreeAngle_Set(lid,20);
}
//卡弹检测
stalled_detect();

BIN
cmake-build-debug/CMakeFiles/basic_framework.elf.dir/application/chassis/chassis.c.obj
View File

Binary file not shown.

BIN
cmake-build-debug/CMakeFiles/basic_framework.elf.dir/application/cmd/robot_cmd.c.obj
View File

Binary file not shown.

BIN
cmake-build-debug/CMakeFiles/basic_framework.elf.dir/application/gimbal/gimbal.c.obj
View File

Binary file not shown.

BIN
cmake-build-debug/CMakeFiles/basic_framework.elf.dir/application/shoot/shoot.c.obj
View File

Binary file not shown.

4
cmake-build-debug/CMakeFiles/basic_framework.elf.dir/application/shoot/shoot.c.obj.d
View File

@ -110,4 +110,6 @@ CMakeFiles/basic_framework.elf.dir/application/shoot/shoot.c.obj: \
D:\zhandui\cqdm\basic_framework\modules\motor/motor_def.h \
D:\zhandui\cqdm\basic_framework\modules\daemon/daemon.h \
D:\zhandui\cqdm\basic_framework\modules\message_center/message_center.h \
D:\zhandui\cqdm\basic_framework\modules/general_def.h
D:\zhandui\cqdm\basic_framework\modules/general_def.h \
D:\zhandui\cqdm\basic_framework\modules\motor\servo_motor/servo_motor.h \
D:\zhandui\cqdm\basic_framework\Inc/tim.h

2
cmake-build-debug/CMakeFiles/basic_framework.elf.dir/compiler_depend.internal
View File

@ -4686,6 +4686,8 @@ CMakeFiles/basic_framework.elf.dir/application/shoot/shoot.c.obj
D:/zhandui/cqdm/basic_framework/modules/daemon/daemon.h
D:/zhandui/cqdm/basic_framework/modules/message_center/message_center.h
D:/zhandui/cqdm/basic_framework/modules/general_def.h
D:/zhandui/cqdm/basic_framework/modules/motor/servo_motor/servo_motor.h
D:/zhandui/cqdm/basic_framework/Inc/tim.h
CMakeFiles/basic_framework.elf.dir/bsp/adc/bsp_adc.c.obj
D:/zhandui/cqdm/basic_framework/bsp/adc/bsp_adc.c

8
cmake-build-debug/CMakeFiles/basic_framework.elf.dir/compiler_depend.make
View File

@ -4608,7 +4608,9 @@ CMakeFiles/basic_framework.elf.dir/application/shoot/shoot.c.obj: D:/zhandui/cqd
D:/zhandui/cqdm/basic_framework/modules/motor/motor_def.h \
D:/zhandui/cqdm/basic_framework/modules/daemon/daemon.h \
D:/zhandui/cqdm/basic_framework/modules/message_center/message_center.h \
D:/zhandui/cqdm/basic_framework/modules/general_def.h
D:/zhandui/cqdm/basic_framework/modules/general_def.h \
D:/zhandui/cqdm/basic_framework/modules/motor/servo_motor/servo_motor.h \
D:/zhandui/cqdm/basic_framework/Inc/tim.h
CMakeFiles/basic_framework.elf.dir/bsp/adc/bsp_adc.c.obj: D:/zhandui/cqdm/basic_framework/bsp/adc/bsp_adc.c
@ -9004,6 +9006,8 @@ D:/zhandui/cqdm/basic_framework/modules/algorithm/QuaternionEKF.c:
D:/zhandui/cqdm/basic_framework/application/shoot/shoot.c:
D:/zhandui/cqdm/basic_framework/modules/motor/servo_motor/servo_motor.h:
D:/zhandui/cqdm/basic_framework/bsp/bsp_tools.h:
D:/zhandui/cqdm/basic_framework/bsp/can/bsp_can.c:
@ -9066,8 +9070,6 @@ D:/zhandui/cqdm/basic_framework/modules/motor/DJImotor/dji_motor.c:
D:/zhandui/cqdm/basic_framework/modules/motor/LKmotor/LK9025.c:
D:/zhandui/cqdm/basic_framework/modules/motor/servo_motor/servo_motor.h:
D:/zhandui/cqdm/basic_framework/modules/motor/servo_motor/servo_motor.c:
D:/zhandui/cqdm/basic_framework/modules/referee/crc_ref.c:

BIN
cmake-build-debug/CMakeFiles/basic_framework.elf.dir/modules/auto_aim/auto_aim.c.obj
View File

Binary file not shown.

BIN
cmake-build-debug/CMakeFiles/basic_framework.elf.dir/modules/motor/servo_motor/servo_motor.c.obj
View File

Binary file not shown.

BIN
cmake-build-debug/CMakeFiles/basic_framework.elf.dir/modules/referee/referee_UI.c.obj
View File

Binary file not shown.

BIN
cmake-build-debug/CMakeFiles/basic_framework.elf.dir/modules/referee/referee_task.c.obj
View File

Binary file not shown.

BIN
cmake-build-debug/basic_framework.bin
View File

Binary file not shown.

BIN
cmake-build-debug/basic_framework.elf
View File

Binary file not shown.

10808
cmake-build-debug/basic_framework.hex
View File

File diff suppressed because it is too large Load Diff

2718
cmake-build-debug/basic_framework.map
View File

File diff suppressed because it is too large Load Diff

37
modules/auto_aim/auto_aim.c
View File

@ -104,9 +104,9 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
if (distance[i] < distance[label_first]) {
label_second = label_first;
label_first = i;
} else if (distance[i] < distance[label_second]) {
} else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
label_second = i;
} else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
}else if (distance[i] < distance[label_second]) {
label_second = i;
}
}
@ -158,19 +158,29 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
}
int label_first = 0;
int label_second = 0;
int label_second = 1;
for (i = 1; i < 4; i++) {
if (distance[i] < distance[label_first]) {
label_second = label_first;
label_first = i;
} else if (distance[i] < distance[label_second]) {
label_second = i;
} else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
label_second = i;
}
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));
@ -188,6 +198,9 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
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;

78
modules/motor/servo_motor/servo_motor.c
View File

@ -33,20 +33,20 @@ void Servo_Motor_FreeAngle_Set(ServoInstance *Servo_Motor, int16_t S_angle)
{
switch (Servo_Motor->Servo_type)
{
case Servo180:
if (S_angle > 180)
S_angle = 180;
break;
case Servo270:
if (S_angle > 270)
S_angle = 270;
break;
case Servo360:
if (S_angle > 100)
S_angle = 100;
break;
default:
break;
case Servo180:
if (S_angle > 180)
S_angle = 180;
break;
case Servo270:
if (S_angle > 270)
S_angle = 270;
break;
case Servo360:
if (S_angle > 100)
S_angle = 100;
break;
default:
break;
}
if (S_angle < 0)
S_angle = 0;
@ -92,31 +92,31 @@ void ServeoMotorControl()
switch (Servo_Motor->Servo_type)
{
case Servo180:
if (Servo_Motor->Servo_Angle_Type == Start_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Init_angle * 20000 / 20 / 90;
if (Servo_Motor->Servo_Angle_Type == Final_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Final_angle * 20000 / 20 / 90;
if (Servo_Motor->Servo_Angle_Type == Free_Angle_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.free_angle * 20000 / 20 / 90;
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
break;
case Servo270:
if (Servo_Motor->Servo_Angle_Type == Start_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Init_angle * 20000 / 20 / 135;
if (Servo_Motor->Servo_Angle_Type == Final_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Final_angle * 20000 / 20 / 135;
if (Servo_Motor->Servo_Angle_Type == Free_Angle_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.free_angle * 20000 / 20 / 135;
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
break;
case Servo360:
/*500-2500的占空比 500-1500对应正向转速 1500-2500对于反向转速*/
compare_value[i] = 500 + 20 * Servo_Motor->Servo_Angle.servo360speed;
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
break;
default:
break;
case Servo180:
if (Servo_Motor->Servo_Angle_Type == Start_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Init_angle * 20000 / 20 / 90;
if (Servo_Motor->Servo_Angle_Type == Final_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Final_angle * 20000 / 20 / 90;
if (Servo_Motor->Servo_Angle_Type == Free_Angle_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.free_angle * 20000 / 20 / 90;
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
break;
case Servo270:
if (Servo_Motor->Servo_Angle_Type == Start_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Init_angle * 20000 / 20 / 135;
if (Servo_Motor->Servo_Angle_Type == Final_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Final_angle * 20000 / 20 / 135;
if (Servo_Motor->Servo_Angle_Type == Free_Angle_mode)
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.free_angle * 20000 / 20 / 135;
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
break;
case Servo360:
/*500-2500的占空比 500-1500对应正向转速 1500-2500对于反向转速*/
compare_value[i] = 500 + 20 * Servo_Motor->Servo_Angle.servo360speed;
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
break;
default:
break;
}
}
}

38
modules/referee/referee_task.c
View File

@ -55,6 +55,7 @@ void UITask()
}
static Graph_Data_t UI_shoot_line[10]; // 射击准线
static Graph_Data_t UI_shoot_yuan[2]; // 射击圆心
static Graph_Data_t UI_Energy[4]; // 电容能量条
static String_Data_t UI_State_sta[7]; // 机器人状态,静态只需画一次
static String_Data_t UI_State_dyn[7]; // 机器人状态,动态先add才能change
@ -70,13 +71,13 @@ void MyUIInit()
DeterminRobotID(); // 确定ui要发送到的目标客户端
UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI
// 绘制发射基准线
// 绘制发射基准线和基准圆
UILineDraw(&UI_shoot_line[0], "sl0", UI_Graph_ADD, 7, UI_Color_White, 3, 710, shoot_line_location[0], 1210, shoot_line_location[0]);
UILineDraw(&UI_shoot_line[1], "sl1", UI_Graph_ADD, 7, UI_Color_White, 3, shoot_line_location[1], 340, shoot_line_location[1], 740);
UILineDraw(&UI_shoot_line[2], "sl2", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[2], 1110, shoot_line_location[2]);
UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[3], 1110, shoot_line_location[3]);
UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3], UI_shoot_line[4]);
UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Orange, 2, 960, 200, 960, 600);
UICircleDraw(&UI_shoot_yuan[0],"sy0",UI_Graph_ADD,7,UI_Color_Yellow,2,960,540,10);
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3],UI_shoot_yuan[0]);
// 绘制车辆状态标志指示
UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:");
@ -85,9 +86,9 @@ void MyUIInit()
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 150, 650, "shoot:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 600, "frict:");
UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 600, "F.frict:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[3]);
UICharDraw(&UI_State_sta[4], "ss4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 550, "lid:");
UICharDraw(&UI_State_sta[4], "ss4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 550, "Q.lid:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[4]);
// 绘制车辆状态标志，动态
@ -107,23 +108,18 @@ void MyUIInit()
UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "Power_MAX:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
//底盘功率上限
// UICharDraw(&UI_State_sta[6], "ss7", UI_Graph_ADD, 8, UI_Color_Pink, 18, 2, 620, 300, "Power_MAX:");
// UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[6]);
// 能量条框
UIRectangleDraw(&UI_Energy[0], "ss6", UI_Graph_ADD, 7, UI_Color_Green, 2, 720, 140, 1220, 180);
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[0]);
// UIRectangleDraw(&UI_Energy[0], "ss6", UI_Graph_ADD, 7, UI_Color_Green, 2, 720, 140, 1220, 180);
// UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[0]);
// 底盘功率显示,动态
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 850, 230, referee_recv_info->GameRobotState.chassis_power_limit*1000);
// 能量条初始状态
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160);
//UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160);
//UIIntDraw(&UI_Energy[3],"sd7",UI_Graph_ADD,8,UI_Color_Pink,18,2,1000,300,referee_recv_info->GameRobotState.chassis_power_limit);
//UIFloatDraw(&UI_Energy[3], "sd7", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 1000, 1000, referee_recv_info->GameRobotState.chassis_power_limit);
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[1]);
}
// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
@ -261,6 +257,12 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
_Interactive_data->Referee_Interactive_Flag.Power_flag = 0;
}
//绘制开火建议
if (_Interactive_data->Referee_Interactive_Flag.aim_flag == 1) {
UICircleDraw(&UI_shoot_yuan[0], "sy0", UI_Graph_Change, 7, _Interactive_data->aim_fire == 0 ? UI_Color_Yellow : UI_Color_Main, 2, 960, 540, 10);
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_yuan[0]);
_Interactive_data->Referee_Interactive_Flag.lid_flag = 0;
}
}
/**
@ -306,4 +308,10 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
_Interactive_data->Referee_Interactive_Flag.Power_flag = 1;
_Interactive_data->Chassis_last_Power_Data.last_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx;
}
if (_Interactive_data->aim_fire != _Interactive_data->aim_last_fire)
{
_Interactive_data->Referee_Interactive_Flag.aim_flag = 1;
_Interactive_data->aim_last_fire = _Interactive_data->aim_fire;
}
}

3
modules/referee/rm_referee.h
View File

@ -54,6 +54,7 @@ typedef struct
uint32_t lid_flag : 1;
uint32_t friction_flag : 1;
uint32_t Power_flag : 1;
uint32_t aim_flag : 1;
} Referee_Interactive_Flag_t;
// 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据
@ -67,6 +68,7 @@ typedef struct
friction_mode_e friction_mode; // 摩擦轮关闭
lid_mode_e lid_mode; // 弹舱盖打开
Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
uint8_t aim_fire; // 开火建议
// 上一次的模式，用于flag判断
chassis_mode_e chassis_last_mode;
@ -75,6 +77,7 @@ typedef struct
friction_mode_e friction_last_mode;
lid_mode_e lid_last_mode;
Chassis_Power_Data_s Chassis_last_Power_Data;
uint8_t aim_last_fire;
} Referee_Interactive_info_t;

29
xs.jdebug.user
View File

@ -1,9 +1,10 @@
OpenDocument="xs.jdebug", FilePath="D:/zhandui/cqdm/basic_framework/xs.jdebug", Line=0
OpenDocument="shoot.c", FilePath="D:/zhandui/cqdm/basic_framework/application/shoot/shoot.c", Line=190
OpenDocument="main.c", FilePath="D:/zhandui/cqdm/basic_framework/Src/main.c", Line=64
OpenDocument="ins_task.c", FilePath="D:/zhandui/cqdm/basic_framework/modules/imu/ins_task.c", Line=192
OpenDocument="referee_task.c", FilePath="D:/zhandui/cqdm/basic_framework/modules/referee/referee_task.c", Line=75
OpenDocument="main.c", FilePath="D:/zhandui/cqdm/basic_framework/Src/main.c", Line=63
OpenDocument="xs.jdebug", FilePath="D:/zhandui/cqdm/basic_framework/xs.jdebug", Line=0
OpenDocument="bsp_dwt.c", FilePath="D:/zhandui/cqdm/basic_framework/bsp/dwt/bsp_dwt.c", Line=75
OpenDocument="startup_stm32f407ighx.s", FilePath="D:/zhandui/cqdm/basic_framework/Startup/startup_stm32f407ighx.s", Line=46
OpenDocument="stm32f4xx_it.c", FilePath="D:/zhandui/cqdm/basic_framework/Src/stm32f4xx_it.c", Line=102
@ -11,28 +12,30 @@ OpenDocument="controller.c", FilePath="D:/zhandui/cqdm/basic_framework/modules/a
OpenDocument="controller.h", FilePath="D:/zhandui/cqdm/basic_framework/modules/algorithm/controller.h", Line=42
OpenDocument="SEGGER_RTT.c", FilePath="D:/zhandui/cqdm/basic_framework/Middlewares/Third_Party/SEGGER/RTT/SEGGER_RTT.c", Line=1181
OpenDocument="gimbal.c", FilePath="D:/zhandui/cqdm/basic_framework/application/gimbal/gimbal.c", Line=105
OpenDocument="chassis.c", FilePath="D:/zhandui/cqdm/basic_framework/application/chassis/chassis.c", Line=180
OpenDocument="tasks.c", FilePath="D:/zhandui/cqdm/basic_framework/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3394
OpenDocument="chassis.c", FilePath="D:/zhandui/cqdm/basic_framework/application/chassis/chassis.c", Line=103
OpenDocument="tasks.c", FilePath="D:/zhandui/cqdm/basic_framework/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3635
OpenToolbar="Debug", Floating=0, x=0, y=0
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=651, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=1045, h=328, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=651, h=568, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=725, h=328, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=426, h=328, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=681, h=328, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=520, h=559, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=994, h=328, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=520, h=533, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=379, h=328, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=680, h=328, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=935, h=1093, 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="708;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="724;0"
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=504, h=328, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;950]
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;100;100;100;100;110;126;126]
TableHeader="Power Sampling", SortCol="None", 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=[238;100;104;209]
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[238;100;104;100]
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[27;27;27;27]
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[27;27;27;26]
WatchedExpression="yaw_motor", RefreshRate=2, Window=Watched Data 1
WatchedExpression="pitch_motor", RefreshRate=2, Window=Watched Data 1
WatchedExpression="motor_rf", RefreshRate=2, Window=Watched Data 1
WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1
WatchedExpression="referee_recv_info", Window=Watched Data 1
WatchedExpression="chassis_power_limit", Window=Watched Data 1
WatchedExpression="chassis_cmd_recv", RefreshRate=2, Window=Watched Data 1
WatchedExpression="chassis_cmd_recv", RefreshRate=2, Window=Watched Data 1
WatchedExpression="vx_filter", RefreshRate=2, Window=Watched Data 1