102 lines
2.3 KiB
C
102 lines
2.3 KiB
C
#include "up_utils.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;
|
||
}
|
||
|
||
|
||
// 计算58度曲线
|
||
static float curve_58(float d) {
|
||
return 0.448f * d * d + 24.8604f * d - 177.99552f;
|
||
}
|
||
|
||
// 计算66度曲线
|
||
static float curve_66(float d) {
|
||
return 8.84935f * d * d - 10.5424f * d - 126.791f;
|
||
}
|
||
|
||
|
||
/*
|
||
曲线切换,用于距离和pos拟合
|
||
迟滞区x-y
|
||
曲线x重合区，根据当前函数和变化方向切换
|
||
*/
|
||
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) {
|
||
return curve_58(d);
|
||
} else {
|
||
return curve_66(d);
|
||
}
|
||
}
|
||
|