R2_NEW/User/Module/up_utils.c

#include "up_utils.h"
#include "up.h"

int8_t DJ_processdata(DJmotor_feedback_t *f, fp32 ecd_to_angle)
{
    fp32 angle, delta;

    angle = f->ecd;

    // 初始化阶段，记录 offset
    if (f->init_cnt < 50) {
        f->offset_ecd = (uint16_t)angle;   // 记录初始偏移
        f->orig_angle = angle - f->offset_ecd; // orig_angle 归零
        f->last_angle = angle - f->offset_ecd;
        f->init_cnt++;
        return 0;
    }

    // 使用 offset 修正
    angle = angle - f->offset_ecd;

    delta = angle - f->last_angle;
    if (delta > 4096) {
        f->round_cnt--;           
    } else if (delta < -4096) {
        f->round_cnt++;          
    }
    f->last_angle = angle;
    f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
}
/*go电机控制*/	 

int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, float limit)
{ 
 
	  *go_data = *get_GO_measure_point() ;


    // 读取参数
    float tff = go_cmd->T;          // 前馈力矩
    float kp = go_cmd->K_P;         // 位置刚度
    float kd = go_cmd->K_W;         // 速度阻尼
    float q_desired = go_cmd->Pos;  // 期望位置（rad）
    float q_current = go_data->Pos; // 当前角度位置（rad）
    float dq_desired = go_cmd->W;  // 期望角速度（rad/s）
    float dq_current = go_data->W;  // 当前角速度（rad/s）

    // 计算输出力矩 tau
    float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);

	  /*限制最大输入来限制最大输出*/
	if (pos - q_current > limit) {
		go_cmd->Pos = q_current + limit; // 限制位置
	}else if (pos - q_current < -limit) {
		go_cmd->Pos = q_current - limit; // 限制位置
	}else {
		go_cmd->Pos = pos; // 允许位置
	}

    // 发送数据
    GO_M8010_send_data(go_cmd);


    return 0;
}



// 计算66度曲线（偏上）
	static float curve_66(float d) {
    return 2.9851104*pow(d,4) -36.164382*pow(d,3) + 159.54844*pow(d,2)  -273.62856*d + 43.092452;
	}
// 计算58度曲线（偏下）
static float curve_58(float d) {
//    return 0.9242f * d * d + 19.4246f * d - 154.9055f;
//	    return 2.638517*pow(d,4)  -47.996138*pow(d,3) + 325.38515*pow(d,2)  -950.18155*d + 919.86543;
    return 2.7775f * d * d - 0.5798f * d - 113.1488;

}

/*
   曲线切换,用于距离和pos拟合
   迟滞区x-y
   曲线x重合区，根据当前函数和变化方向切换
*/
	fp32 ang_58;
fp32 ang_66;


float CurveChange(float d, float x, float y, CurveType *cs)
{
	
   
    if (*cs == CURVE_66) {
        if (d > y) {
            *cs = CURVE_58;
        }
    } else { // CURVE_58
        if (d < x) {
            *cs = CURVE_66;
        }
    }

    // 根据当前曲线返回结果
    if (*cs == CURVE_58) {
			ang_58=d;
        return curve_58(d);
    } else {
			ang_66=d;
        return curve_66(d);
    }
}

int8_t Pass_Sequence_Check(UP_t *u, CMD_t *c) //按键顺序检测，传球，按需求改
{
    PassState_t *state = &u->PassContext.PassState;
    static enum {
        SEQ_IDLE,
        SEQ_MID1,
        SEQ_UP,
        SEQ_MID2,
        SEQ_DOWN
    } seq = SEQ_IDLE;

    switch (seq) {
        case SEQ_IDLE:
            if (c->CMD_mode == PB_MID) {
                seq = SEQ_MID1;
                *state = PASS_IDLE;
            }
            break;
        case SEQ_MID1:
            if (c->CMD_mode == PB_UP) {
                seq = SEQ_UP;
                *state = PASS_PREPARE;
            } else if (c->CMD_mode != PB_MID) {
                seq = SEQ_IDLE;
                *state = PASS_STOP;
            }
            break;
        case SEQ_UP:
            if (c->CMD_mode == PB_MID) {
                seq = SEQ_MID2;
                *state = PASS_START;
            } else if (c->CMD_mode != PB_UP) {
                seq = SEQ_IDLE;
                *state = PASS_STOP;
            }
            break;
        case SEQ_MID2:
            if (c->CMD_mode == PB_DOWN) {
                seq = SEQ_DOWN;
                *state = PASS_POS_PREPARE;
            } else if (c->CMD_mode != PB_MID) {
                seq = SEQ_IDLE;
                *state = PASS_STOP;
            }
            break;
        case SEQ_DOWN:
            if (c->CMD_mode == PB_MID) {
                seq = SEQ_IDLE;
                *state = PASS_IDLE; // 或 PASS_COMPLETE，看你需求
            } else if (c->CMD_mode != PB_DOWN) {
                seq = SEQ_IDLE;
                *state = PASS_STOP;
            }
            break;
    }
    return 0;
}



#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;
}