#ifndef DOG_SOLVE_H
#define DOG_SOLVE_H


#define M_PI 3.1415926
#define leg_length1  0.050//单位为m
#define leg_length2  0.340
#define leg_length3  0.400

//extern float kp;
//extern float kd;

struct dog_pd{
	float kp;
	float kd;
};
extern struct dog_pd dog_pd[12];//每条腿的pd参数

struct dog_motor{
	double sol_gamma;
	double sol_alfa;
	double sol_beta;
	double  a1, l4; // 计算的角度和长度(中间变量)
};
extern struct dog_motor leg_angle[4];

struct dog_foot{
	double x;
	double y;
	double z;
};
extern struct dog_foot foot_position[4];

struct dog_motor_ouput{
	double sol_gamma;
	double sol_alfa;
	double sol_beta;
};
extern struct dog_motor_ouput leg_angle_ouput[4];


void dog_solve();//运动学逆解
// void dog_solve_output();//把逆解计算出来的结果处理后输出给电机



#endif