163 lines
4.0 KiB
C
163 lines
4.0 KiB
C
#include "up_utils.h"
|
||
#include "up.h"
|
||
|
||
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
|
||
{
|
||
int8_t cnt=0;
|
||
fp32 angle ,delta;
|
||
|
||
angle = f->ecd;
|
||
if (f->init_cnt < 50) {
|
||
f->orig_angle= angle;
|
||
f->last_angle = angle;
|
||
f->init_cnt++;
|
||
return 0;
|
||
}
|
||
|
||
|
||
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度曲线（偏上）
|
||
// 计算66度曲线（偏上）
|
||
static float curve_66(float d) {
|
||
return 4.0310f * d * d + 8.9026f * d -139.5156;
|
||
}
|
||
|
||
// 计算58度曲线（偏下）
|
||
// 计算58度曲线（偏下）
|
||
static float curve_58(float d) {
|
||
return -1.9776f * d * d + 42.8499f * d - 204.2442f;
|
||
}
|
||
|
||
/*
|
||
曲线切换,用于距离和pos拟合
|
||
迟滞区x-y
|
||
曲线x重合区，根据当前函数和变化方向切换
|
||
*/
|
||
int abdddd=0;
|
||
float CurveChange(float d, float x, float y, CurveType *cs)
|
||
{
|
||
if (d<3.2) abdddd++;
|
||
if (*cs == CURVE_66) {
|
||
if (d > y) {
|
||
*cs = CURVE_58;
|
||
}
|
||
} else { // CURVE_58
|
||
if (d < x) {
|
||
*cs = CURVE_66;
|
||
}
|
||
}
|
||
|
||
// 根据当前曲线返回结果
|
||
if (*cs == CURVE_58) {
|
||
return curve_58(d);
|
||
} else {
|
||
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;
|
||
}
|
||
|
||
|