LYL/R2_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

参数

Browse Source
This commit is contained in:
ZHAISHUI04 2025-07-11 04:47:49 +08:00
parent 04a3906b2c
commit 6cf2fb649b
5 changed files with 9 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

9
User/Module/Chassis.c
View File

@ -131,7 +131,14 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break;
case Chassis_Adjust_Sick:
sick_calibration(c, ctrl, out);
if(abs(ctrl->cmd_MID360.angle)<1.0f){
c->move_vec.Vx = ctrl->cmd_MID360.dis * 3000;
}
if(-ctrl->cmd_MID360.dis>0.02f){
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 1000;
}
// sick_calibration(c, ctrl, out);
// c->to_nuc.send = (c->sick_cali.is_reach == 1) ? 1 : 0;
break;

2
User/Module/config.c
View File

@ -75,7 +75,7 @@ static const ConfigParam_t param ={
/*投球*/
.LaunchCfg = {
.m2006_init = -125.0f, // M2006初始角度
.m2006_init = -130.0f, // M2006初始角度
.m2006_trig = 0.0f, // M2006触发角度
.go_pull_pos = -210.0f, // go上升去合并扳机，扳机位置
.go_up_speed = 12.0f, // 上升速度

59
User/Module/up_utils.c
View File

@ -171,62 +171,3 @@ int8_t Pass_Sequence_Check(UP_t *u, CMD_t *c) //按键顺序检测，传球，
#define BIN_SCALE 1000 // 精度：0.001米
#define BIN_OFFSET 3000 // 最小支持3.000米 → bin 3000
#define BIN_COUNT 5001 // 支持3.000 ~ 8.000米，(8000-3000+1)
#define WINDOW_SIZE 50
static float window[WINDOW_SIZE] = {0};
static int index = 0;
static int filled = 0;
static uint16_t freq_map[BIN_COUNT] = {0}; // 用16位计数防止溢出
static float last_output = 0.0f;
static uint8_t first_run = 1;
// 四舍五入保留3位小数
static inline float round3f(float x) {
return (int)(x * BIN_SCALE + 0.5f) / (float)BIN_SCALE;
}
float stable_distance_filter(float new_value) {
// 1. 四舍五入到0.001
float rounded = round3f(new_value);
int new_bin = (int)(rounded * BIN_SCALE + 0.5f);
// 2. 移除旧值计数
int old_bin = (int)(round3f(window[index]) * BIN_SCALE + 0.5f);
if (filled && old_bin >= BIN_OFFSET && old_bin < BIN_OFFSET + BIN_COUNT) {
freq_map[old_bin - BIN_OFFSET]--;
}
// 3. 加入新值到窗口和计数
window[index++] = new_value;
if (index >= WINDOW_SIZE) {
index = 0;
filled = 1;
}
if (new_bin >= BIN_OFFSET && new_bin < BIN_OFFSET + BIN_COUNT) {
freq_map[new_bin - BIN_OFFSET]++;
}
// 4. 找出现频率最高的桶
int max_bin = -1;
uint16_t max_freq = 0;
for (int i = 0; i < BIN_COUNT; ++i) {
if (freq_map[i] > max_freq) {
max_freq = freq_map[i];
max_bin = i + BIN_OFFSET;
}
}
float current_fixed = max_bin >= 0 ? max_bin / (float)BIN_SCALE : rounded;
// 5. 判断是否更新输出，差值超过0.1才更新
if (first_run || fabsf(current_fixed - last_output) > 0.1f) {
last_output = current_fixed;
first_run = 0;
}
return last_output;
}

3
User/Module/up_utils.h
View File

@ -53,9 +53,6 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
static inline float round3f(float x) ;
float stable_distance_filter(float new_value) ;
#endif