228 lines
7.1 KiB
C
228 lines
7.1 KiB
C
//
|
|
// Created by SJQ on 2023/12/25.
|
|
//
|
|
|
|
#include "leg.h"
|
|
#include "math.h"
|
|
#include "main.h"
|
|
#include "arm_math.h"
|
|
|
|
#include "cal_phi0_dot.h"
|
|
|
|
|
|
static const float l[5]={0.15f,0.27f,0.27f,0.15f,0.15f};
|
|
static void calculate_leg_pos(Leg_State_t* leg_state);
|
|
static void VMC_jacobian(float L0, float phi0, float phi1, float phi2, float phi3,
|
|
float phi4, float F, float Tp, float T12[2]);
|
|
void VMC_getT(LegInstance* legInstance);
|
|
|
|
void Leg_Init(LegInstance* legInstance, Leg_init_config_s* legInitConfig)
|
|
{
|
|
PIDInit(&legInstance->Length_PID,&legInitConfig->length_PID_conf);
|
|
PIDInit(&legInstance->Phi0_PID,&legInitConfig->phi0_PID_conf);
|
|
legInstance->target_L0 = legInitConfig->init_target_L0;
|
|
legInstance->F_feedforward = legInitConfig->F_feedforward;
|
|
|
|
// set rest of memory to 0
|
|
DWT_GetDeltaT(&legInstance->legState.DWT_CNT);
|
|
}
|
|
|
|
void Leg_length_control_loop(LegInstance* legInstance)
|
|
{
|
|
float F_pid_out = PIDCalculate(&legInstance->Length_PID,legInstance->legState.L0,legInstance->target_L0);
|
|
float Tp_pid_out = PIDCalculate(&legInstance->Phi0_PID,legInstance->legState.phi0,legInstance->target_phi0);
|
|
Leg_Input_update(&legInstance->legInput,F_pid_out-2.73f,Tp_pid_out);
|
|
//Leg_calc_output(&legInstance->legState,&legInstance->legInput,&legInstance->legOutput);
|
|
VMC_getT(legInstance);
|
|
}
|
|
void Leg_State_update(LegInstance* legInstance, float phi1,float phi4 ,float phi1_dot,float phi4_dot)
|
|
{
|
|
Leg_State_t* leg_state = &legInstance->legState;
|
|
if(phi1<=PI/2)
|
|
phi1 = PI/2;
|
|
if(phi4>=PI/2)
|
|
phi4 = PI/2;
|
|
|
|
leg_state->phi1 = phi1;
|
|
leg_state->phi4 = phi4;
|
|
float leg_pos[4];
|
|
|
|
//calculate_leg_pos(leg_state->phi1,leg_state->phi4,leg_pos);
|
|
calculate_leg_pos(leg_state);
|
|
|
|
|
|
//对phi0求导
|
|
// leg_state->dt = DWT_GetDeltaT(&leg_state->DWT_CNT);
|
|
// if(leg_state->last_phi0>=0)
|
|
// leg_state->phi0_dot = (leg_state->phi0-leg_state->last_phi0)/leg_state->dt;
|
|
// else
|
|
// leg_state->phi0_dot = 0;
|
|
// leg_state->last_phi0 = leg_state->phi0;
|
|
leg_state->phi0_dot = get_dphi0(phi1,phi4,phi1_dot,phi4_dot);
|
|
}
|
|
|
|
void Leg_Input_update(Leg_Input_t* leg_Input, float F,float Tp)
|
|
{
|
|
leg_Input->F = F;
|
|
leg_Input->Tp = Tp;
|
|
}
|
|
|
|
void Leg_calc_output(Leg_State_t* leg_state,Leg_Input_t* leg_input,Leg_Output_t* leg_output)
|
|
{
|
|
// float T12[2] = {0,0};
|
|
// VMC_jacobian(leg_state->L0,leg_state->phi0,leg_state->phi1,
|
|
// leg_state->phi2,leg_state->phi3,leg_state->phi4,
|
|
// leg_input->F,leg_input->Tp,T12);
|
|
// leg_output->T1 = T12[0];
|
|
// leg_output->T2 = T12[1];
|
|
}
|
|
|
|
|
|
void calculate_leg_pos(Leg_State_t* leg_state)
|
|
{
|
|
//以机体中点建立坐标系
|
|
float xb = -l[4]/2+l[0]* arm_cos_f32(leg_state->phi1);
|
|
float yb = l[0]* arm_sin_f32(leg_state->phi1);
|
|
|
|
float xd = l[4]/2+l[3]* arm_cos_f32(leg_state->phi4);
|
|
float yd = l[3]* arm_sin_f32(leg_state->phi4);
|
|
|
|
float A0 = 2*l[1]*(xd-xb);
|
|
float B0 = 2*l[1]*(yd-yb);
|
|
|
|
float lbd2 = (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb);
|
|
float C0 = l[1]*l[1]+lbd2-l[2]*l[2];
|
|
|
|
float tanPhi = 0;
|
|
|
|
arm_sqrt_f32((A0*A0 + B0*B0 - C0*C0),&tanPhi);
|
|
//tanPhi = (B0+tanPhi)/(A0+C0);
|
|
|
|
float phi2 = 2* atan2f(B0+tanPhi,A0+C0);
|
|
float xc = xb + l[1] * arm_cos_f32(phi2);
|
|
float yc = yb + l[1] * arm_sin_f32(phi2);
|
|
|
|
float phi3 = acosf(xc-xd/l[2]);
|
|
float L0 = 0;
|
|
arm_sqrt_f32((xc*xc + yc*yc),&L0);
|
|
float phi0 = atan2f(yc,xc);
|
|
|
|
//赋值
|
|
leg_state->L0 = L0;
|
|
leg_state->phi0 = phi0;
|
|
leg_state->phi2 = phi2;
|
|
|
|
leg_state->phi3 = phi3;
|
|
}
|
|
|
|
void VMC_getT(LegInstance* legInstance)
|
|
{
|
|
arm_matrix_instance_f32 T_matrix;
|
|
arm_matrix_instance_f32 F_Tp_matrix;
|
|
float T_array[4] = {0};
|
|
float F_Tp_array[2] = {0};
|
|
|
|
F_Tp_array[0] = legInstance->legInput.F;
|
|
F_Tp_array[1] = legInstance->legInput.Tp;
|
|
|
|
float sinPhi23 = arm_sin_f32(legInstance->legState.phi2 - legInstance->legState.phi3);
|
|
float sinPhi12 = arm_sin_f32(legInstance->legState.phi1 - legInstance->legState.phi2);
|
|
float sinPhi34 = arm_sin_f32(legInstance->legState.phi3 - legInstance->legState.phi4);
|
|
|
|
float sinPhi03 = arm_sin_f32(legInstance->legState.phi0 - legInstance->legState.phi3);
|
|
float sinPhi02 = arm_sin_f32(legInstance->legState.phi0 - legInstance->legState.phi2);
|
|
float cosPhi03 = arm_cos_f32(legInstance->legState.phi0 - legInstance->legState.phi3);
|
|
float cosPhi02 = arm_cos_f32(legInstance->legState.phi0 - legInstance->legState.phi2);
|
|
|
|
T_array[0] = -(l[0]* sinPhi03*sinPhi12)/sinPhi23;
|
|
T_array[1] = -(l[0]* cosPhi03*sinPhi12)/(legInstance->legState.L0 * sinPhi23);
|
|
|
|
T_array[2] = -(l[3]* sinPhi02*sinPhi34)/sinPhi23;
|
|
T_array[3] = -(l[3]* cosPhi02*sinPhi34)/(legInstance->legState.L0 * sinPhi23);
|
|
|
|
arm_mat_init_f32(&T_matrix,2,2,T_array);
|
|
arm_mat_init_f32(&F_Tp_matrix,2,1,F_Tp_array);
|
|
|
|
arm_matrix_instance_f32 T12_matrix;
|
|
float T12[2] = {0};
|
|
arm_mat_init_f32(&T12_matrix,2,1,T12);
|
|
|
|
arm_mat_mult_f32(&T_matrix,&F_Tp_matrix,&T12_matrix);
|
|
legInstance->legOutput.T1 = T12_matrix.pData[0];
|
|
legInstance->legOutput.T2 = T12_matrix.pData[1];
|
|
}
|
|
|
|
|
|
////此段代码为matlab生成
|
|
//static void calculate_leg_pos(const float phi1, const float phi4, float leg_pos[4])
|
|
//{
|
|
// float A0;
|
|
// float B0;
|
|
// float C0;
|
|
// float L0;
|
|
// float phi2;
|
|
// float scale;
|
|
// float xc;
|
|
// float xd;
|
|
// float yb_tmp;
|
|
// /* 以机体中点建立坐标系 */
|
|
// L0 = 0.15F * cosf(phi1);
|
|
// yb_tmp = 0.15F * sinf(phi1);
|
|
// xd = 0.15F * cosf(phi4) + 0.075F;
|
|
// A0 = 0.54F * (xd - (L0 - 0.075F));
|
|
// C0 = 0.15F * sinf(phi4) - yb_tmp;
|
|
// B0 = 0.54F * C0;
|
|
// scale = (0.15F * cosf(phi4) + 0.075F) - (L0 - 0.075F);
|
|
// C0 = ((scale * scale + C0 * C0) + 0.0729000047F) - 0.0729000047F;
|
|
// phi2 = 2.0F * atanf((B0 + sqrtf((A0 * A0 + B0 * B0) - C0 * C0)) / (A0 + C0));
|
|
// xc = L0 + 0.27F * cosf(phi2);
|
|
// C0 = yb_tmp + 0.27F * sinf(phi2);
|
|
// scale = 1.29246971E-26F;
|
|
// A0 = fabsf(xc);
|
|
// if (A0 > 1.29246971E-26F) {
|
|
// L0 = 1.0F;
|
|
// scale = A0;
|
|
// } else {
|
|
// B0 = A0 / 1.29246971E-26F;
|
|
// L0 = B0 * B0;
|
|
// }
|
|
// A0 = fabsf(C0);
|
|
// if (A0 > scale) {
|
|
// B0 = scale / A0;
|
|
// L0 = L0 * B0 * B0 + 1.0F;
|
|
// scale = A0;
|
|
// } else {
|
|
// B0 = A0 / scale;
|
|
// L0 += B0 * B0;
|
|
// }
|
|
// L0 = scale * sqrtf(L0);
|
|
// leg_pos[0] = L0;
|
|
// leg_pos[1] = atanf(C0 / xc);
|
|
// leg_pos[2] = phi2;
|
|
// leg_pos[3] = acosf(xc - xd / 0.27F);
|
|
//}
|
|
//
|
|
void VMC_jacobian(float L0, float phi0, float phi1, float phi2, float phi3,
|
|
float phi4, float F, float Tp, float T12[2])
|
|
{
|
|
float T_tmp;
|
|
float b_T_tmp;
|
|
float c_T_tmp;
|
|
float d_T_tmp;
|
|
float e_T_tmp;
|
|
float f_T_tmp;
|
|
T_tmp = phi0 - phi3;
|
|
b_T_tmp = arm_sin_f32(phi1 - phi2);
|
|
c_T_tmp = arm_sin_f32(phi2 - phi3);
|
|
d_T_tmp = phi0 - phi2;
|
|
e_T_tmp = arm_sin_f32(phi3 - phi4);
|
|
f_T_tmp = L0 * c_T_tmp;
|
|
T12[0] = -(0.15F * arm_sin_f32(T_tmp) * b_T_tmp) / c_T_tmp * F +
|
|
-(0.15F * arm_cos_f32(T_tmp) * b_T_tmp) / f_T_tmp * Tp;
|
|
T12[1] = -(0.15F * arm_sin_f32(d_T_tmp) * e_T_tmp) / c_T_tmp * F +
|
|
-(0.15F * arm_cos_f32(d_T_tmp) * e_T_tmp) / f_T_tmp * Tp;
|
|
}
|
|
|
|
|
|
|