RC_WheelLeg/01_doc/control/movement_notes.md at 0b91dfffe67d2aff362cf38038bc9efa1b25cca6

Robocon/RC_WheelLeg

Fork 0

Files

T

zeitvex 0b91dfffe6 [release] 发布8DOF串联足中期检查版本

2026-07-21 12:18:00 +08:00

52 KiB

Raw Blame History

#include "main.h" #include "Init.h"

PID_motor m3508set[8] = {0}; PID_motor m2006set[8] = {0}; u8 Bound_Flag_Point[2]={1,1}; /**

@name motor_pid_set
@brief 电机pid初始化
@param DJ：内环pid：1000、6、0 外环0.1、0、0.006
@param mode：1使用外环
@param deadband：10 */ void motor_pid_set(void) { static u8 i[2]; static float PID[6] = {1000.0f, 6.0f, 0.0f, 0.1f, 0.0f, 0.006f}; static float max_out[2] = {16000.0f, 400.0f}; static float max_idout[2] = {2000.0f, 100.0f};

for(i[0] = 0; i[0] < 4; i[0]++) { PID_motoinit(&Motor_PID[i[0]], 1, PID, max_out, max_idout, 0);//3508_1 PID_motoinit(&Motor_PID[i[0] + 4], 1, PID, max_out, max_idout, 0); //3508_2 PID_motoinit(&Motor_PID[i[0] + 8], 1, PID, max_out, max_idout, 0); //2006_1 PID_motoinit(&Motor_PID[i[0] + 12], 1, PID, max_out, max_idout, 0); //2006_2 }

for(i[1] = 0; i[1] < 4; i[1]++) { m3508set[i[1]].setpos = 0; m3508set[i[1]].setspeed = 0; m3508set[i[1] + 4].setpos = 0; m3508set[i[1] + 4].setspeed = 0;
```
  m2006set[i[1]].setpos = 0;
  m2006set[i[1]].setspeed = 0;
  m2006set[i[1] + 4].setpos = 0;
  m2006set[i[1] + 4].setspeed = 0;
```
} }

/**

@name MOTOR_PID_CHANGE
@brief
@param Mid为0为正常 Mid为1柔性 Mid 为2 速度环加P
@param
@retval */ void MOTOR_PID_CHANGE(u8 Mid) { static float OUT_KP[2]= {0.1,0.03}; static float NUM_max[2]= {16000,8000}; static float IN_KP[2]= {1000,1400}; if(Mid==0) { Motor_PID[0].out_Kp=OUT_KP[0]; Motor_PID[1].out_Kp=OUT_KP[0]; Motor_PID[2].out_Kp=OUT_KP[0]; Motor_PID[3].out_Kp=OUT_KP[0]; Motor_PID[4].out_Kp=OUT_KP[0]; Motor_PID[5].out_Kp=OUT_KP[0]; Motor_PID[6].out_Kp=OUT_KP[0]; Motor_PID[7].out_Kp=OUT_KP[0]; Motor_PID[0].in_Kp =IN_KP[0]; Motor_PID[1].in_Kp =IN_KP[0]; Motor_PID[2].in_Kp =IN_KP[0]; Motor_PID[3].in_Kp =IN_KP[0]; Motor_PID[4].in_Kp =IN_KP[0]; Motor_PID[5].in_Kp =IN_KP[0]; Motor_PID[6].in_Kp =IN_KP[0]; Motor_PID[7].in_Kp =IN_KP[0]; Motor_PID[0].in_max_out=NUM_max[0]; Motor_PID[1].in_max_out=NUM_max[0]; Motor_PID[2].in_max_out=NUM_max[0]; Motor_PID[3].in_max_out=NUM_max[0]; Motor_PID[4].in_max_out=NUM_max[0]; Motor_PID[5].in_max_out=NUM_max[0]; Motor_PID[6].in_max_out=NUM_max[0]; Motor_PID[7].in_max_out=NUM_max[0]; } else if(Mid==1) { Motor_PID[0].out_Kp=OUT_KP[1]; Motor_PID[1].out_Kp=OUT_KP[1]; Motor_PID[2].out_Kp=OUT_KP[1]; Motor_PID[3].out_Kp=OUT_KP[1]; Motor_PID[4].out_Kp=OUT_KP[1]; Motor_PID[5].out_Kp=OUT_KP[1]; Motor_PID[6].out_Kp=OUT_KP[1]; Motor_PID[7].out_Kp=OUT_KP[1]; Motor_PID[0].in_Kp =IN_KP[0]; Motor_PID[1].in_Kp =IN_KP[0]; Motor_PID[2].in_Kp =IN_KP[0]; Motor_PID[3].in_Kp =IN_KP[0]; Motor_PID[4].in_Kp =IN_KP[0]; Motor_PID[5].in_Kp =IN_KP[0]; Motor_PID[6].in_Kp =IN_KP[0]; Motor_PID[7].in_Kp =IN_KP[0]; Motor_PID[0].in_max_out=NUM_max[1]; Motor_PID[1].in_max_out=NUM_max[1]; Motor_PID[2].in_max_out=NUM_max[1]; Motor_PID[3].in_max_out=NUM_max[1]; Motor_PID[4].in_max_out=NUM_max[1]; Motor_PID[5].in_max_out=NUM_max[1]; Motor_PID[6].in_max_out=NUM_max[1]; Motor_PID[7].in_max_out=NUM_max[1]; } else if(Mid==2) { Motor_PID[0].out_Kp=OUT_KP[0]; Motor_PID[1].out_Kp=OUT_KP[0]; Motor_PID[2].out_Kp=OUT_KP[0]; Motor_PID[3].out_Kp=OUT_KP[0]; Motor_PID[4].out_Kp=OUT_KP[0]; Motor_PID[5].out_Kp=OUT_KP[0]; Motor_PID[6].out_Kp=OUT_KP[0]; Motor_PID[7].out_Kp=OUT_KP[0]; Motor_PID[0].in_Kp =IN_KP[1]; Motor_PID[1].in_Kp =IN_KP[1]; Motor_PID[2].in_Kp =IN_KP[1]; Motor_PID[3].in_Kp =IN_KP[1]; Motor_PID[4].in_Kp =IN_KP[1]; Motor_PID[5].in_Kp =IN_KP[1]; Motor_PID[6].in_Kp =IN_KP[1]; Motor_PID[7].in_Kp =IN_KP[1]; Motor_PID[0].in_max_out=NUM_max[0]; Motor_PID[1].in_max_out=NUM_max[0]; Motor_PID[2].in_max_out=NUM_max[0]; Motor_PID[3].in_max_out=NUM_max[0]; Motor_PID[4].in_max_out=NUM_max[0]; Motor_PID[5].in_max_out=NUM_max[0]; Motor_PID[6].in_max_out=NUM_max[0]; Motor_PID[7].in_max_out=NUM_max[0]; }

} /**

@name Stop_flat_ground(float Height_body)
@brief stop_flag_speed=0初始速度较小，
@param stop_flag_speed=1速度较大电流大
@param

@retval control.s[3]==2 为平地步态下的位置 control.s[3]==3为上斜坡下的位置 control.s[3]==1为下斜坡的位置 / void Stop_flat_ground(float Height_body) { // static float step=170; // static float Slope_Err[2]= {60,60}; //100 240 // static float Slope_step[2]= {170,170}; //100,170 Speed=0.00f; /对刚体坐标E点坐标初始化/ if(control.s[3]==2&&(control.s[5]==2||control.s[5]==3||control.s[5]==1)&&control.s[4]==2)//平地归零 { MOTOR_PID_CHANGE(0);

  Xstart[0]=0;
  Xstart[1]=0;
  Xstart[2]=0;
  Xstart[3]=0;
  Xend[0]=Xstart[0];
  Xend[1]=Xstart[1];
  Xend[2]=Xstart[2];
  Xend[3]=Xstart[3];

  Hight[0]=0;
  Hight[3]=Hight[2]=Hight[1]=Hight[0];

  Zstart[0]=-Height_body;  //can2 id 1,2  腿1
  Zstart[1]=-Height_body;  //can2 id 3,4  腿2
  Zstart[2]=-Height_body+20;  //can1 id 1,2  腿3
  Zstart[3]=-Height_body+20;  //can1 id 3,4  腿4
  M3508_stop_postion(10,0.5,TS);

} else if(control.s[3]==3)//上坡落点 { MOTOR_PID_CHANGE(0); Xstart[0]=0; Xstart[1]=0; Xstart[2]=0; Xstart[3]=0;

  Xend[0]=Xstart[0];
  Xend[1]=Xstart[1];
  Xend[2]=Xstart[2];
  Xend[3]=Xstart[3];

  Hight[0]=0;
  Hight[3]=Hight[2]=Hight[1]=Hight[0];

  Zstart[0]=-Height_body+50;  //can2 id 1,2  腿1
  Zstart[1]=-Height_body+50;  //can2 id 3,4  腿2
  Zstart[2]=-Height_body;  //can1 id 1,2  腿3
  Zstart[3]=-Height_body;  //can1 id 3,4  腿4

  M3508_stop_postion(15,0.5,TS);

} else if(control.s[3]==1)//下坡落点 { MOTOR_PID_CHANGE(0); Xstart[0]=0; Xstart[1]=0; Xstart[2]=0; Xstart[3]=0;

  Xend[0]=Xstart[0];
  Xend[1]=Xstart[1];
  Xend[2]=Xstart[2];
  Xend[3]=Xstart[3];

  Hight[0]=0;
  Hight[3]=Hight[2]=Hight[1]=Hight[0];

  Zstart[0]=-Height_body;  //can2 id 1,2  腿1
  Zstart[1]=-Height_body;  //can2 id 3,4  腿2
  Zstart[2]=-Height_body+50;  //can1 id 1,2  腿3
  Zstart[3]=-Height_body+50;  //can1 id 3,4  腿4

  M3508_stop_postion(15,0.5,TS);

} // else if(control.s[3]==2&&control.s[5]==3)//跷跷板出发位置 // { // MOTOR_PID_CHANGE(0);

// Xstart[0]=-(Slope_step[0]+Slope_Err[0])/2; // Xstart[1]=(Slope_step[0]-Slope_Err[0])/2; // Xstart[2]=(Slope_step[1]-Slope_Err[1])/2; // Xstart[3]=-(Slope_step[1]+Slope_Err[1])/2;

// Xend[0]=Xstart[0] ; //+step为前进腿1 // Xend[1]=Xstart[1] ; //腿2 // Xend[2]=Xstart[2] ; //腿3 // Xend[3]=Xstart[3] ; //腿4 // Hight[0]=0; // Hight[3]=Hight[2]=Hight[1]=Hight[0];

// Zstart[0]=-Height_body+15; //can2 id 1,2 腿1 // Zstart[1]=-Height_body+15; //can2 id 3,4 腿2 // Zstart[2]=-Height_body+20; //can1 id 1,2 腿3 // Zstart[3]=-Height_body+20; //can1 id 3,4 腿4 //
// M3508_stop_postion(20,0.5,TS); // } }

void M3508_stop_postion(float setspeed,float arr,float Ts) { static float M3508_Angle[16]= {0} ; static float M2006_Angle[16]= {0} ;

/******************生成E,点坐标********************************/
Leg_cycloid(0,&link[0],arr,Ts,Hight[0],Zstart[0],Xstart[0],Xend[0]);
Leg_cycloid(1,&link[1],arr,Ts,Hight[1],Zstart[1],Xstart[1],Xend[1]);

Leg_cycloid(2,&link[2],arr,Ts,Hight[2],Zstart[2],Xstart[2],Xend[2]);
Leg_cycloid(3,&link[3],arr,Ts,Hight[3],Zstart[3],Xstart[3],Xend[3]);


Inverse_leg(&leg[0],&link[0]);
Inverse_leg(&leg[1],&link[1]);	 //E3,E4的运动学逆解
Inverse_leg(&leg[2],&link[2]);
Inverse_leg(&leg[3],&link[3]);	 //E3,E4的运动学逆解

M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio;	 //后 can2 ID1,2 腿1
M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;

M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio;	   //后 can2 ID3,4 腿3
M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio;   //前 can1 ID1,2 腿2
M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio;	   //前 can1 ID3,4 腿4
M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

m2006set[0].setspeed=setspeed;
m2006set[1].setspeed=setspeed;
m2006set[2].setspeed=setspeed;
m2006set[3].setspeed=setspeed;

m3508set[0].setspeed=setspeed;
m3508set[1].setspeed=setspeed;
m3508set[2].setspeed=setspeed;
m3508set[3].setspeed=setspeed;

m2006set[0].setpos = M2006_Angle[0];
m2006set[1].setpos = M2006_Angle[1];
m2006set[2].setpos = M2006_Angle[2];
m2006set[3].setpos = M2006_Angle[3];

m3508set[0].setpos = M3508_Angle[0];
m3508set[1].setpos = M3508_Angle[1];
m3508set[2].setpos = M3508_Angle[2];
m3508set[3].setpos = M3508_Angle[3];

} /**

@name trot_flat_ground
@brief 对角步态
@param speed 迈步速度
@param T[0]=T[3]=0 T[2]=T[1]=0.5 顺序位左后右前->左前有后
@retval control.s[3]==2 为平地步态下的位置 control.s[3]==3为上斜坡下的位置 control.s[3]==1为下斜坡的位置 */

void trot_flat_ground(float step,float length_hight,float Height_body,float speed) { MOTOR_PID_CHANGE(0); static float Deviation_Err=5;//8 平地误差 static float Deviation_Err_s5=0;//8 翘翘板误差 static float Slope_Err[2]= {60,60}; //60 60 跷跷板滞后位置 static float Slope_step[2]= {140,140}; //170,170 跷跷板步长 /对刚体坐标E点坐标初始化*/

if(control.s[5]==3)//跷跷板前进轨迹
{
    Speed=0.0035;
		if(control.s[1]==4)
		{
			Xstart[0]=-(Slope_Err[0]+Slope_step[0]+38)/2.0f;
			Xstart[1]=-(Slope_Err[0]+Slope_step[0]-30)/2.0f;
    Xstart[2]=-(Slope_Err[1]+Slope_step[1]+38)/2.0f;
    Xstart[3]=-(Slope_Err[1]+Slope_step[1]-30)/2.0f;

    Xend[0]=Xstart[0] + Slope_step[0] +38 ; //+step为前进   腿1
    Xend[1]=Xstart[1] + Slope_step[0] -30  ;                     //腿2
    Xend[2]=Xstart[2] + Slope_step[1] +38 ;       //腿3
    Xend[3]=Xstart[3] + Slope_step[1] -30  ;                     //腿4

		}
		else if(control.s[1]==3)
		{
			Xstart[0]=-(Slope_Err[0]+Slope_step[0]-38)/2.0f;
			Xstart[1]=-(Slope_Err[0]+Slope_step[0]+48)/2.0f;
    Xstart[2]=-(Slope_Err[1]+Slope_step[1]-38)/2.0f;
    Xstart[3]=-(Slope_Err[1]+Slope_step[1]+48)/2.0f;

    Xend[0]=Xstart[0] + Slope_step[0] -38 ; //+step为前进   腿1
    Xend[1]=Xstart[1] + Slope_step[0] +48  ;                     //腿2
    Xend[2]=Xstart[2] + Slope_step[1] -38 ;       //腿3
    Xend[3]=Xstart[3] + Slope_step[1] +48  ;                     //腿4

		}
		else
		{
			Xstart[0]=-(Slope_Err[0]+Slope_step[0]-Deviation_Err_s5)/2.0f;
			Xstart[1]=-(Slope_Err[0]+Slope_step[0])/2.0f;
    Xstart[2]=-(Slope_Err[1]+Slope_step[1]-Deviation_Err_s5)/2.0f;
    Xstart[3]=-(Slope_Err[1]+Slope_step[1])/2.0f;

    Xend[0]=Xstart[0] + Slope_step[0] -Deviation_Err_s5 ; //+step为前进   腿1
    Xend[1]=Xstart[1] + Slope_step[0]-0  ;                     //腿2
    Xend[2]=Xstart[2] + Slope_step[1] -Deviation_Err_s5 ;       //腿3
    Xend[3]=Xstart[3] + Slope_step[1]-0  ;                     //腿4
		}
    Hight[0]=length_hight-20;
    Hight[1]=length_hight-20;
    Hight[2]=length_hight-20;
    Hight[3]=length_hight-20;

    Zstart[0]=-Height_body+15;     //can2 id 1,2 腿1
    Zstart[1]=-Height_body+15;     //can2 id 3,4 腿2
    Zstart[2]=-Height_body+20;  //can1 id 1,2 腿3
    Zstart[3]=-Height_body+20;  //can1 id 3,4 腿4
}

 if(control.s[3]==2&&(control.s[5]==2||control.s[5]==1)&&control.s[4]==2)//正常出发轨迹
{
    Speed=speed;
		if(control.s[1]==4)
		{
    Xstart[2]=Xstart[0]=-(130-Deviation_Err)/2.0f;
    Xstart[3]=Xstart[1]=-100/2.0f;
    Xend[0]=Xstart[0] + 130 -Deviation_Err ; //+step为前进   腿1
    Xend[1]=Xstart[1] + 100  ;                     //腿2
    Xend[2]=Xstart[2] + 130 -Deviation_Err ;       //腿3
    Xend[3]=Xstart[3] + 100  ; 
			}
		else if(control.s[1]==3)
		{
    Xstart[2]=Xstart[0]=-(100-Deviation_Err)/2.0f;
    Xstart[3]=Xstart[1]=-130/2.0f;
    Xend[0]=Xstart[0] + 100 -Deviation_Err ; //+step为前进   腿1
    Xend[1]=Xstart[1] + 130  ;                     //腿2
    Xend[2]=Xstart[2] + 100 -Deviation_Err ;       //腿3
    Xend[3]=Xstart[3] + 130  ; 
		}
		else 
		{
    Xstart[2]=Xstart[0]=-(100-Deviation_Err)/2.0f;
    Xstart[3]=Xstart[1]=-100/2.0f;
    Xend[0]=Xstart[0] + 100 -Deviation_Err ; //+step为前进   腿1
    Xend[1]=Xstart[1] + 100  ;                     //腿2
    Xend[2]=Xstart[2] + 100 -Deviation_Err ;       //腿3
    Xend[3]=Xstart[3] + 100  ;                     //腿4
		}
    Hight[0]=length_hight+0;
    Hight[1]=length_hight+0;
    Hight[2]=length_hight+0;
    Hight[3]=length_hight+0;

    Zstart[0]=-Height_body;     //can2 id 1,2 腿1
    Zstart[1]=-Height_body;     //can2 id 3,4 腿2
    Zstart[2]=-Height_body+20;  //can1 id 1,2 腿3
    Zstart[3]=-Height_body+20;  //can1 id 3,4 腿4
}
else if(control.s[3]==3&&control.s[5]==2)//上坡调整轨迹 步长为50
{
    Speed=speed;

// if(control.s[1]==4) // { // // Xstart[0]=-(85)/2.0f; // Xstart[1]=-50/2.0f; // Xstart[2]=-(85+50)/2.0f; // Xstart[3]=-(50+50)/2.0f;

// Xend[0]=Xstart[0] + 85 ; //+step为前进腿1 // Xend[1]=Xstart[1] + 50 ; //腿2 // Xend[2]=Xstart[2] + 85 ; //腿3 // Xend[3]=Xstart[3] + 50 ; //腿4

// } // else if(control.s[1]==3) // { // // Xstart[0]=-(55)/2.0f; // Xstart[1]=-80/2.0f; // Xstart[2]=-(55+50)/2.0f; // Xstart[3]=-(80+50)/2.0f;

// Xend[0]=Xstart[0] + 55 ; //+step为前进腿1 // Xend[1]=Xstart[1] + 80 ; //腿2 // Xend[2]=Xstart[2] + 55 ; //腿3 // Xend[3]=Xstart[3] + 80 ; //腿4

// }

    Xstart[0]=-(30)/2.0f;
    Xstart[1]=-30/2.0f;
    Xstart[2]=-(0+30)/2.0f;
    Xstart[3]=-(0+30)/2.0f;

    Xend[0]=Xstart[0] + 30  ; //+step为前进   腿1
    Xend[1]=Xstart[1] + 30  ;                     //腿2
    Xend[2]=Xstart[2] + 30  ;       //腿3
    Xend[3]=Xstart[3] + 30  ;                     //腿4
		
    Hight[0]=length_hight-30;
    Hight[1]=length_hight-30;
    Hight[2]=length_hight-10;
    Hight[3]=length_hight-10;

    Zstart[0]=-Height_body+40;     //can2 id 1,2 腿1
    Zstart[1]=-Height_body+40;     //can2 id 3,4 腿2
    Zstart[2]=-Height_body-10;  //can1 id 1,2 腿3
    Zstart[3]=-Height_body-10;  //can1 id 3,4 腿4
}
else if(control.s[3]==1&&control.s[5]==2)
{
    Speed=0.0032;

    Xstart[0]=-(75-Deviation_Err)/2.0f;
    Xstart[1]=-75/2.0f;
    Xstart[2]=-(75-Deviation_Err)/2.0f;
    Xstart[3]=-(75)/2.0f;

    Xend[0]=Xstart[0] + 75 -Deviation_Err ; //+step为前进   腿1
    Xend[1]=Xstart[1] + 75  ;                     //腿2
    Xend[2]=Xstart[2] + 75 -Deviation_Err ;       //腿3
    Xend[3]=Xstart[3] + 75  ;                     //腿4
            //腿4

    Hight[0]=length_hight-0;
    Hight[1]=length_hight-0;
    Hight[2]=length_hight-30;
    Hight[3]=length_hight-30;

    Zstart[0]=-Height_body-10;     //can2 id 1,2 腿1
    Zstart[1]=-Height_body-10;     //can2 id 3,4 腿2
    Zstart[2]=-Height_body+40;  //can1 id 1,2 腿3
    Zstart[3]=-Height_body+40;  //can1 id 3,4 腿4
}

M3508_trot_postion(0.5,TS);

}

/**

@name trot_flat_ground
@brief 对角步态
@param speed 迈步速度
@param T[0]=T[3]=0 T[2]=T[1]=0.5 顺序位左后右前->左前有后

@retval control.s[3]==2 为平地步态下的位置 control.s[3]==3为上斜坡下的位置 control.s[3]==1为下斜坡的位置 */ void trot_behind_flat_ground(float step,float length_hight,float Height_body,float speed) { MOTOR_PID_CHANGE(0);

Speed=speed; if(control.s[3]==2)//平地正常 { /对刚体坐标E点坐标初始化*/ Xstart[2]=Xstart[0]=step/2.0f; Xstart[3]=Xstart[1]=step/2.0f;

  Xend[0]=Xstart[0] - step ; //-step 为后退
  Xend[1]=Xstart[1] - step ;
  Xend[2]=Xstart[2] - step ;
  Xend[3]=Xstart[3] - step ;

  Hight[3]=length_hight;
  Hight[2]=length_hight;
  Hight[1]=length_hight;
  Hight[0]=length_hight;


  Zstart[0]=-Height_body+15;  //can2 id 1,2 腿1
  Zstart[1]=-Height_body+15;  //can2 id 3,4 腿2
  Zstart[2]=-Height_body;  //can1 id 1,2 腿3
  Zstart[3]=-Height_body;  //can1 id 3,4 腿4

} if(control.s[3]==3)//上坡 { Xstart[0]=(50)/2.0f; Xstart[1]=50/2.0f; Xstart[2]=(50)/2.0f; Xstart[3]=(50)/2.0f; /对刚体坐标E点坐标初始化*/ // Xstart[0]=step/2.0f; // Xstart[1]=step/2.0f; // Xstart[2]=(step)/2.0f; // Xstart[3]=(step)/2.0f;

  Xend[0]=Xstart[0] - 50 ; //-step 为后退
  Xend[1]=Xstart[1] - 50 ;
  Xend[2]=Xstart[2] - 50 ;
  Xend[3]=Xstart[3] - 50 ;

  Hight[0]=length_hight-40;
  Hight[1]=length_hight-40;
  Hight[2]=length_hight-40;
  Hight[3]=length_hight-40;

  Zstart[0]=-Height_body+40;  //can2 id 1,2 腿1
  Zstart[1]=-Height_body+40;  //can2 id 3,4 腿2
  Zstart[2]=-Height_body-10;  //can1 id 1,2 腿3
  Zstart[3]=-Height_body-10;  //can1 id 3,4 腿4

} M3508_trot_postion(0.5,TS); } /**

@name trot_flat_Right_ground
@brief 对角步态右转
@param speed 迈步速度
@param T[0]=T[3]=0 T[2]=T[1]=0.5 顺序位左后右前->左前有后
@retval / void trot_flat_Right_ground(float step,float length_hight,float Height_body,float speed) { // static float Deviation_Err=30;//8 MOTOR_PID_CHANGE(0); /对刚体坐标E点坐标初始化/ static float Deviation_Err_s5=50;//8 static float Slope_Err[2]= {60,60}; //100 240 static float Slope_step[2]= {150,150}; //100,170 /对刚体坐标E点坐标初始化*/ // if(control.s[5]==1) // { // Speed=0.0025;

// Xstart[0]=-(Slope_Err[0]+Slope_step[0])/2.0f; // Xstart[1]=-(Slope_Err[0]+Slope_step[0]-Deviation_Err_s5)/2.0f; // Xstart[2]=-(Slope_Err[1]+Slope_step[1])/2.0f; // Xstart[3]=-(Slope_Err[1]+Slope_step[1]-Deviation_Err_s5)/2.0f;

// Xend[0]=Xstart[0] + Slope_step[0] ; //+step为前进腿1 // Xend[1]=Xstart[1] + Slope_step[0] -Deviation_Err_s5 ; //腿2 // Xend[2]=Xstart[2] + Slope_step[1] ; //腿3 // Xend[3]=Xstart[3] + Slope_step[1] -Deviation_Err_s5 ; //腿4

// Hight[0]=length_hight-0; // Hight[1]=length_hight-0; // Hight[2]=length_hight-20; // Hight[3]=length_hight-20;

// Zstart[0]=-Height_body+25; //can2 id 1,2 腿1 // Zstart[1]=-Height_body+25; //can2 id 3,4 腿2 // Zstart[2]=-Height_body+20; //can1 id 1,2 腿3 // Zstart[3]=-Height_body+20; //can1 id 3,4 腿4 // } if(control.s[3]==2&&control.s[5]==2) { Speed=speed; Xstart[0]=-step/2.0f; Xstart[1]=step/2.0f; Xstart[2]=-step/2.0f; Xstart[3]=step/2.0f;

    Xend[0]=Xstart[0] + step ;
    Xend[1]=Xstart[1] - step ;
    Xend[2]=Xstart[2] + step ;
    Xend[3]=Xstart[3] - step ;


    Hight[3]=length_hight;
    Hight[2]=length_hight;
    Hight[1]=length_hight;
    Hight[0]=length_hight;

    Zstart[0]=-Height_body;  //can2 id 1,2
    Zstart[1]=-Height_body;  //can2 id 3,4
    Zstart[2]=-Height_body;  //can1 id 1,2
    Zstart[3]=-Height_body;  //can1 id 3,4
}
if(control.s[3]==3&&control.s[5]==2)
{
    Speed=speed;
			Xstart[0]=(30)/2.0f;
    Xstart[1]=30/2.0f;
    Xstart[2]=-(30)/2.0f;
    Xstart[3]=-(30)/2.0f;

    Xend[0]=Xstart[0] + 30 ;
    Xend[1]=Xstart[1] - 30 ;
    Xend[2]=Xstart[2] + 30 ;
    Xend[3]=Xstart[3] - 30 ;

    Hight[3]=length_hight-40;
    Hight[2]=length_hight-40;
    Hight[1]=length_hight-40;
    Hight[0]=length_hight-40;

    Zstart[0]=-Height_body+50;  //can2 id 1,2
    Zstart[1]=-Height_body+50;  //can2 id 3,4
    Zstart[2]=-Height_body;  //can1 id 1,2
    Zstart[3]=-Height_body;  //can1 id 3,4
}
if(control.s[3]==1&&control.s[5]==2)
{
    Speed=speed;
			Xstart[0]=(20)/2.0f;
    Xstart[1]=20/2.0f;
    Xstart[2]=-(20)/2.0f;
    Xstart[3]=-(20)/2.0f;

    Xend[0]=Xstart[0] + step ;
    Xend[1]=Xstart[1] - step ;
    Xend[2]=Xstart[2] + step ;
    Xend[3]=Xstart[3] - step ;

    Hight[3]=length_hight-40;
    Hight[2]=length_hight-40;
    Hight[1]=length_hight-40;
    Hight[0]=length_hight-40;

    Zstart[0]=-Height_body;  //can2 id 1,2
    Zstart[1]=-Height_body;  //can2 id 3,4
    Zstart[2]=-Height_body+50;  //can1 id 1,2
    Zstart[3]=-Height_body+50;  //can1 id 3,4
}
M3508_trot_postion(0.5,TS);

} /**

@name trot_flat_Left_ground
@brief 对角步态左转
@param speed 迈步速度
@param T[0]=T[3]=0 T[2]=T[1]=0.5 顺序位左后右前->左前有后
@retval / void trot_flat_Left_ground(float step,float length_hight,float Height_body,float speed) { // static float Deviation_Err=-30;//8 // static float Slope_Err[2]= {100,240}; //100 240 // static float Slope_step[2]= {170,120}; //100,170 MOTOR_PID_CHANGE(0); /对刚体坐标E点坐标初始化/ static float Deviation_Err_s5=50;//8 static float Slope_Err[2]= {60,60}; //100 240 static float Slope_step[2]= {150,150}; //100,170 /对刚体坐标E点坐标初始化*/ // if(control.s[5]==1) // { // Speed=0.0025;

// Xstart[0]=-(Slope_Err[0]+Slope_step[0]-Deviation_Err_s5)/2.0f; // Xstart[1]=-(Slope_Err[0]+Slope_step[0])/2.0f; // Xstart[2]=-(Slope_Err[1]+Slope_step[1]-Deviation_Err_s5)/2.0f; // Xstart[3]=-(Slope_Err[1]+Slope_step[1])/2.0f;

// Xend[0]=Xstart[0] + Slope_step[0] -Deviation_Err_s5 ; //+step为前进腿1 // Xend[1]=Xstart[1] + Slope_step[0] ; //腿2 // Xend[2]=Xstart[2] + Slope_step[1] -Deviation_Err_s5 ; //腿3 // Xend[3]=Xstart[3] + Slope_step[1] ; //腿4

// Hight[0]=length_hight-0; // Hight[1]=length_hight-0; // Hight[2]=length_hight-20; // Hight[3]=length_hight-20;

if(control.s[3]==2&&control.s[5]==2)
{
		Speed =speed;
    /*************对刚体坐标E点坐标初始化**************/
    Xstart[0]=step/2.0f;
    Xstart[1]=-step/2.0f;
    Xstart[2]=step/2.0f;
    Xstart[3]=-step/2.0f;

    Xend[0]=Xstart[0] - step ;
    Xend[1]=Xstart[1] + step ;
    Xend[2]=Xstart[2] - step ;
    Xend[3]=Xstart[3] + step ;
    Zstart[0]=-Height_body;  //can2 id 1,2
    Zstart[1]=-Height_body;  //can2 id 3,4
    Zstart[2]=-Height_body;  //can1 id 1,2
    Zstart[3]=-Height_body;  //can1 id 3,4
    Hight[3]=Hight[2]=Hight[1]=Hight[0]=length_hight;

}
if(control.s[3]==3&&control.s[5]==2)
{
		Speed =speed;
    /*************对刚体坐标E点坐标初始化**************/
			Xstart[0]=(30)/2.0f;
    Xstart[1]=-30/2.0f;
    Xstart[2]=(50)/2.0f;
    Xstart[3]=-(50)/2.0f;

    Xend[0]=Xstart[0] - 30 ;
    Xend[1]=Xstart[1] + 30 ;
    Xend[2]=Xstart[2] - 50 ;
    Xend[3]=Xstart[3] + 50 ;
    Zstart[0]=-Height_body+50;  //can2 id 1,2
    Zstart[1]=-Height_body+50;  //can2 id 3,4
    Zstart[2]=-Height_body;  //can1 id 1,2
    Zstart[3]=-Height_body;  //can1 id 3,4

    Hight[0]=length_hight-40;
    Hight[1]=length_hight-40;
    Hight[2]=length_hight-40;
    Hight[3]=length_hight-40;
}
if(control.s[3]==1&&control.s[5]==2)
{
		Speed =speed;
    /*************对刚体坐标E点坐标初始化**************/
			Xstart[0]=(30)/2.0f;
    Xstart[1]=30/2.0f;
    Xstart[2]=-(30)/2.0f;
    Xstart[3]=-(30)/2.0f;

    Xend[0]=Xstart[0] - step ;
    Xend[1]=Xstart[1] + step ;
    Xend[2]=Xstart[2] - step ;
    Xend[3]=Xstart[3] + step ;
    Zstart[0]=-Height_body;  //can2 id 1,2
    Zstart[1]=-Height_body;  //can2 id 3,4
    Zstart[2]=-Height_body+50;  //can1 id 1,2
    Zstart[3]=-Height_body+50;  //can1 id 3,4

    Hight[0]=length_hight-40;
    Hight[1]=length_hight-40;
    Hight[2]=length_hight-40;
    Hight[3]=length_hight-40;
}
M3508_trot_postion(0.5,TS);

}

@name M3508_postion
@brief 对角步态电机位置
@param trot :arr占空比一般取0.5
@param Walk :arr占空比一般取0.25
@param Bound:arr占空比一般取0.1
@retval / void M3508_trot_postion(float arr,float Ts) { static float M3508_Angle[16]= {0} ; static float M2006_Angle[16]= {0} ; /生成E,点坐标*************/ Leg_cycloid(0,&link[0],arr,Ts,Hight[0],Zstart[0],Xstart[0],Xend[0]); Leg_cycloid(1,&link[1],arr,Ts,Hight[1],Zstart[1],Xstart[1],Xend[1]); Leg_cycloid(2,&link[2],arr,Ts,Hight[2],Zstart[2],Xstart[2],Xend[2]); Leg_cycloid(3,&link[3],arr,Ts,Hight[3],Zstart[3],Xstart[3],Xend[3]);

Inverse_leg(&leg[0],&link[0]); Inverse_leg(&leg[1],&link[1]); //E3,E4的运动学逆解 Inverse_leg(&leg[2],&link[2]); Inverse_leg(&leg[3],&link[3]); //E3,E4的运动学逆解

M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio; //后 can2 ID1,2 腿1 M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;

M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio; //后 can2 ID3,4 腿3 M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio; //前 can1 ID1,2 腿2 M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio; //前 can1 ID3,4 腿4 M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

if(link[0].point_E.x==Xend[0]||link[0].point_E.x==Xstart[0]) { m2006set[0].setspeed = 0; m2006set[1].setspeed = 0; // LED0=0; HAL_GPIO_WritePin(GPIOE,GPIO_PIN_4,GPIO_PIN_RESET); } else { m2006set[0].setspeed = 150; m2006set[1].setspeed = 150; // LED0=1; HAL_GPIO_WritePin(GPIOE,GPIO_PIN_4,GPIO_PIN_SET); } if(link[1].point_E.x==Xend[1]||link[1].point_E.x==Xstart[1]) { m2006set[2].setspeed = 0; m2006set[3].setspeed = 0; // LED0=0;

HAL_GPIO_WritePin(GPIOE,GPIO_PIN_4,GPIO_PIN_RESET); } else { m2006set[2].setspeed = 150; m2006set[3].setspeed = 150; // LED0=1; HAL_GPIO_WritePin(GPIOE,GPIO_PIN_4,GPIO_PIN_SET); } if(link[2].point_E.x==Xend[2]||link[2].point_E.x==Xstart[2]) { m3508set[0].setspeed = 0; m3508set[1].setspeed = 0; // LED0=0; HAL_GPIO_WritePin(GPIOE,GPIO_PIN_4,GPIO_PIN_RESET); } else { m3508set[0].setspeed = 150; m3508set[1].setspeed = 150; // LED0=1;// HAL_GPIO_WritePin(GPIOE,GPIO_PIN_4,GPIO_PIN_SET); } if(link[3].point_E.x==Xend[3]||link[3].point_E.x==Xstart[3]) { m3508set[2].setspeed = 0; m3508set[3].setspeed = 0; // LED0=0; HAL_GPIO_WritePin(GPIOE,GPIO_PIN_4,GPIO_PIN_RESET); } else { m3508set[2].setspeed = 150; m3508set[3].setspeed = 150; // LED0=1; HAL_GPIO_WritePin(GPIOE,GPIO_PIN_4,GPIO_PIN_SET); }

m2006set[0].setpos = M2006_Angle[0];
m2006set[1].setpos = M2006_Angle[1];
m2006set[2].setpos = M2006_Angle[2];
m2006set[3].setpos = M2006_Angle[3];

m3508set[0].setpos = M3508_Angle[0];
m3508set[1].setpos = M3508_Angle[1];
m3508set[2].setpos = M3508_Angle[2];
m3508set[3].setpos = M3508_Angle[3];

}

/**

@name BOUND_Inclined_Plane_flat_ground
@brief 斜向上跳跃
@param speed 迈步速度
@param T[0]=T[3]=0 T[2]=T[1]=0.5 顺序位左后右前->左前有后

@retval / void BOUND_Inclined_Plane_flat_ground(float Height_body_start,float Height_body_end) { static float M2006_Angle[4]= {0}; static float M3508_Angle[4]= {0}; static float point_Speed[6]= {0,15,150,150,150,20}; static u8 bound_flag_point2=1; LCD_Num(50, 240,bound_flag_point2,1, 16); /第一个点***/ if(Bound_Flag_Point[0]==1) { MOTOR_PID_CHANGE(1); Leg_Point(0,&link[0],-Height_body_start,-50); Leg_Point(1,&link[1],-Height_body_start,-50); Leg_Point(2,&link[2],-Height_body_start,-85); Leg_Point(3,&link[3],-Height_body_start,-85);//85

  Inverse_leg(&leg[0],&link[0]);
  Inverse_leg(&leg[1],&link[1]);
  Inverse_leg(&leg[2],&link[2]);
  Inverse_leg(&leg[3],&link[3]);

  M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio;	 //后 can2 ID1,2 腿1
  M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;

  M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio;	   //后 can2 ID3,4 腿3
  M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio;   //前 can1 ID1,2 腿2
  M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio;	   //前 can1 ID3,4 腿4
  M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

  m2006set[0].setspeed  = point_Speed[1];
  m2006set[1].setspeed  = point_Speed[1];
  m2006set[2].setspeed  = point_Speed[1];
  m2006set[3].setspeed  = point_Speed[1];
  m3508set[0].setspeed  = point_Speed[1];
  m3508set[1].setspeed  = point_Speed[1];
  m3508set[2].setspeed  = point_Speed[1];
  m3508set[3].setspeed  = point_Speed[1];

  m2006set[0].setpos = M2006_Angle[0];
  m2006set[1].setpos = M2006_Angle[1];
  m2006set[2].setpos = M2006_Angle[2];
  m2006set[3].setpos = M2006_Angle[3];

  m3508set[0].setpos = M3508_Angle[0];
  m3508set[1].setpos = M3508_Angle[1];
  m3508set[2].setpos = M3508_Angle[2];
  m3508set[3].setpos = M3508_Angle[3];
  if(control.s[0]==1)Bound_Flag_Point[0]=2;

} else if(control.s[2]==3) {bound_flag_point2=1;} /第二个点***********/ else if(Bound_Flag_Point[0]==2&&(control.s[2]==2||control.s[2]==1)) { MOTOR_PID_CHANGE(0);//速度P加大 if(control.s[2]==3)bound_flag_point2=1;

  Leg_Point(0,&link[0],-Height_body_end,-140);
  Leg_Point(1,&link[1],-Height_body_end,-140);//双目桥
  Leg_Point(2,&link[2],-Height_body_end,-140);
  Leg_Point(3,&link[3],-Height_body_end,-140);

  Inverse_leg(&leg[0],&link[0]);
  Inverse_leg(&leg[1],&link[1]);
  Inverse_leg(&leg[2],&link[2]);
  Inverse_leg(&leg[3],&link[3]);

  M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio;	 //后 can2 ID1,2 腿1
  M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;

  M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio;	   //后 can2 ID3,4 腿3
  M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio;   //前 can1 ID1,2 腿2
  M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio;	   //前 can1 ID3,4 腿4
  M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

  m2006set[0].setspeed  = point_Speed[2];
  m2006set[1].setspeed  = point_Speed[2];
  m2006set[2].setspeed  = point_Speed[2];
  m2006set[3].setspeed  = point_Speed[2];
  m3508set[0].setspeed  = point_Speed[2];
  m3508set[1].setspeed  = point_Speed[2];
  m3508set[2].setspeed  = point_Speed[2];
  m3508set[3].setspeed  = point_Speed[2];

  m2006set[0].setpos = M2006_Angle[0];
  m2006set[1].setpos = M2006_Angle[1];
  m2006set[2].setpos = M2006_Angle[2];
  m2006set[3].setpos = M2006_Angle[3];
  m3508set[0].setpos = M3508_Angle[0];
  m3508set[1].setpos = M3508_Angle[1];
  m3508set[2].setpos = M3508_Angle[2];
  m3508set[3].setpos = M3508_Angle[3];
  if((fabs(m2006set[0].setpos-m2006[0].real_angle)<15)&&(fabs(m2006set[1].setpos-m2006[1].real_angle)<15)&&(control.s[2]==2||control.s[2]==1))Bound_Flag_Point[0]=3;

} else if(Bound_Flag_Point[0]==3&&(control.s[2]==2||control.s[2]==1)) { /3点*******/ MOTOR_PID_CHANGE(0); if(control.s[2]==3)bound_flag_point2=1;

  Leg_Point(0,&link[0],-Height_body_start,0);
  Leg_Point(1,&link[1],-Height_body_start,0);
  Leg_Point(2,&link[2],-Height_body_start,0);
  Leg_Point(3,&link[3],-Height_body_start,0);
  Inverse_leg(&leg[0],&link[0]);
  Inverse_leg(&leg[1],&link[1]);
  Inverse_leg(&leg[2],&link[2]);
  Inverse_leg(&leg[3],&link[3]);

  M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio;	 //后 can2 ID1,2 腿1
  M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;

  M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio;	   //后 can2 ID3,4 腿3
  M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio;   //前 can1 ID1,2 腿2
  M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio;	   //前 can1 ID3,4 腿4
  M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

  m2006set[0].setspeed  = point_Speed[3];
  m2006set[1].setspeed  = point_Speed[3];
  m2006set[2].setspeed  = point_Speed[3];
  m2006set[3].setspeed  = point_Speed[3];
  m3508set[0].setspeed  = point_Speed[3];
  m3508set[1].setspeed  = point_Speed[3];
  m3508set[2].setspeed  = point_Speed[3];
  m3508set[3].setspeed  = point_Speed[3];

  m2006set[0].setpos = M2006_Angle[0];
  m2006set[1].setpos = M2006_Angle[1];
  m2006set[2].setpos = M2006_Angle[2];
  m2006set[3].setpos = M2006_Angle[3];
  m3508set[0].setpos = M3508_Angle[0];
  m3508set[1].setpos = M3508_Angle[1];
  m3508set[2].setpos = M3508_Angle[2];
  m3508set[3].setpos = M3508_Angle[3];
  if((fabs(m3508set[1].setpos-m3508[1].real_angle)<20)&&(fabs(m3508set[0].setpos-m3508[0].real_angle)<20)&&(control.s[2]==2||control.s[2]==1))Bound_Flag_Point[0]=4;

} /第4个点***********/ else if(Bound_Flag_Point[0]==4&&(control.s[2]==2||control.s[2]==1)) { MOTOR_PID_CHANGE(1); if(control.s[2]==3)bound_flag_point2=1;

  Leg_Point(0,&link[0],-Height_body_start-50,120);//-60,120
  Leg_Point(1,&link[1],-Height_body_start-50,120);
  Leg_Point(2,&link[2],-Height_body_start-50,120);
  Leg_Point(3,&link[3],-Height_body_start-50,120);
  Inverse_leg(&leg[0],&link[0]);
  Inverse_leg(&leg[1],&link[1]);
  Inverse_leg(&leg[2],&link[2]);
  Inverse_leg(&leg[3],&link[3]);

  M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio;	 //后 can2 ID1,2 腿1
  M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;

  M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio;	   //后 can2 ID3,4 腿3
  M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio;   //前 can1 ID1,2 腿2
  M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio;	   //前 can1 ID3,4 腿4
  M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

  m2006set[0].setspeed  = point_Speed[4];
  m2006set[1].setspeed  = point_Speed[4];
  m2006set[2].setspeed  = point_Speed[4];
  m2006set[3].setspeed  = point_Speed[4];
  m3508set[0].setspeed  = point_Speed[4];
  m3508set[1].setspeed  = point_Speed[4];
  m3508set[2].setspeed  = point_Speed[4];
  m3508set[3].setspeed  = point_Speed[4];

  m2006set[0].setpos = M2006_Angle[0];
  m2006set[1].setpos = M2006_Angle[1];
  m2006set[2].setpos = M2006_Angle[2];
  m2006set[3].setpos = M2006_Angle[3];
  m3508set[0].setpos = M3508_Angle[0];
  m3508set[1].setpos = M3508_Angle[1];
  m3508set[2].setpos = M3508_Angle[2];
  m3508set[3].setpos = M3508_Angle[3];
  if((fabs(m2006set[1].setpos-m2006[1].real_angle)<100)&&(fabs(m2006set[0].setpos-m2006[0].real_angle)<100)&&(control.s[2]==2||control.s[2]==1))
  {
      Bound_Flag_Point[0]=5;
  }

} else if(Bound_Flag_Point[0]==5&&(control.s[2]==2||control.s[2]==1)) { MOTOR_PID_CHANGE(1); if(control.s[2]==3) bound_flag_point2=1;

  Leg_Point(0,&link[0],-Height_body_start-50,-20);
  Leg_Point(1,&link[1],-Height_body_start-50,-20);
  Leg_Point(2,&link[2],-Height_body_start-50,-20);
  Leg_Point(3,&link[3],-Height_body_start-50,-20);
  Inverse_leg(&leg[0],&link[0]);
  Inverse_leg(&leg[1],&link[1]);
  Inverse_leg(&leg[2],&link[2]);
  Inverse_leg(&leg[3],&link[3]);

  M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio;	 //后 can2 ID1,2 腿1
  M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;
  M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio;	   //后 can2 ID3,4 腿2
  M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio;   //前 can1 ID1,2 腿3
  M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio;	   //前 can1 ID3,4 腿4
  M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

  m2006set[0].setspeed  = point_Speed[5];
  m2006set[1].setspeed  = point_Speed[5];
  m2006set[2].setspeed  = point_Speed[5];
  m2006set[3].setspeed  = point_Speed[5];
  m3508set[0].setspeed  = point_Speed[5];
  m3508set[1].setspeed  = point_Speed[5];
  m3508set[2].setspeed  = point_Speed[5];
  m3508set[3].setspeed  = point_Speed[5];

  m2006set[0].setpos = M2006_Angle[0];
  m2006set[1].setpos = M2006_Angle[1];
  m2006set[2].setpos = M2006_Angle[2];
  m2006set[3].setpos = M2006_Angle[3];
  m3508set[0].setpos = M3508_Angle[0];
  m3508set[1].setpos = M3508_Angle[1];
  m3508set[2].setpos = M3508_Angle[2];
  m3508set[3].setpos = M3508_Angle[3];
  if((fabs(m2006set[1].setpos-m2006[1].real_angle)<50)&&(fabs(m2006set[0].setpos-m2006[0].real_angle)<50)&&(control.s[2]==2||control.s[2]==1)) {
      Bound_Flag_Point[0]=6;
  }

} else if(Bound_Flag_Point[0]==6) {if(control.s[2]==3)Bound_Flag_Point[0]=1;}

}

/**

@name BOUND_Inclined_Plane_flat_ground
@brief 平跳
@param point_Speed 每个点期望速度
@param

@retval */ void BOUND_Plane_flat_ground(float Height_body_start,float Height_body_end) { static float M2006_Angle[4]= {0}; static float M3508_Angle[4]= {0}; static float point_Speed[6]= {0,15,150,150,150,20}; static u8 bound_flag_point2=1; LCD_Num(50, 220,bound_flag_point2,1, 16);

/第一个点****/ if(Bound_Flag_Point[1]==1) { MOTOR_PID_CHANGE(0); Leg_Point(0,&link[0],-Height_body_start,-50); Leg_Point(1,&link[1],-Height_body_start,-50); Leg_Point(2,&link[2],-Height_body_start,-50); Leg_Point(3,&link[3],-Height_body_start,-50);

  Inverse_leg(&leg[0],&link[0]);
  Inverse_leg(&leg[1],&link[1]);
  Inverse_leg(&leg[2],&link[2]);
  Inverse_leg(&leg[3],&link[3]);

  M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio;	 //后 can2 ID1,2 腿1
  M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;

  M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio;	   //后 can2 ID3,4 腿3
  M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio;   //前 can1 ID1,2 腿2
  M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio;	   //前 can1 ID3,4 腿4
  M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

  m2006set[0].setspeed  = point_Speed[1];
  m2006set[1].setspeed  = point_Speed[1];
  m2006set[2].setspeed  = point_Speed[1];
  m2006set[3].setspeed  = point_Speed[1];
  m3508set[0].setspeed  = point_Speed[1];
  m3508set[1].setspeed  = point_Speed[1];
  m3508set[2].setspeed  = point_Speed[1];
  m3508set[3].setspeed  = point_Speed[1];

  m2006set[0].setpos = M2006_Angle[0];
  m2006set[1].setpos = M2006_Angle[1];
  m2006set[2].setpos = M2006_Angle[2];
  m2006set[3].setpos = M2006_Angle[3];

  m3508set[0].setpos = M3508_Angle[0];
  m3508set[1].setpos = M3508_Angle[1];
  m3508set[2].setpos = M3508_Angle[2];
  m3508set[3].setpos = M3508_Angle[3];
  if(control.s[0]==1)Bound_Flag_Point[1]=2;

} else if(control.s[2]==3) {bound_flag_point2=1;} /第二个点***********/ else if(Bound_Flag_Point[1]==2&&(control.s[2]==2||control.s[2]==1)) { MOTOR_PID_CHANGE(0);//速度P加大 if(control.s[2]==3)bound_flag_point2=1;

  Leg_Point(0,&link[0],-Height_body_end,-140);
  Leg_Point(1,&link[1],-Height_body_end,-140);
  Leg_Point(2,&link[2],-Height_body_end,-140);
  Leg_Point(3,&link[3],-Height_body_end,-140);

  Inverse_leg(&leg[0],&link[0]);
  Inverse_leg(&leg[1],&link[1]);
  Inverse_leg(&leg[2],&link[2]);
  Inverse_leg(&leg[3],&link[3]);

  M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio;	 //后 can2 ID1,2 腿1
  M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;

  M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio;	   //后 can2 ID3,4 腿3
  M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio;   //前 can1 ID1,2 腿2
  M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio;	   //前 can1 ID3,4 腿4
  M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

  m2006set[0].setspeed  = point_Speed[2];
  m2006set[1].setspeed  = point_Speed[2];
  m2006set[2].setspeed  = point_Speed[2];
  m2006set[3].setspeed  = point_Speed[2];
  m3508set[0].setspeed  = point_Speed[2];
  m3508set[1].setspeed  = point_Speed[2];
  m3508set[2].setspeed  = point_Speed[2];
  m3508set[3].setspeed  = point_Speed[2];

  m2006set[0].setpos = M2006_Angle[0];
  m2006set[1].setpos = M2006_Angle[1];
  m2006set[2].setpos = M2006_Angle[2];
  m2006set[3].setpos = M2006_Angle[3];
  m3508set[0].setpos = M3508_Angle[0];
  m3508set[1].setpos = M3508_Angle[1];
  m3508set[2].setpos = M3508_Angle[2];
  m3508set[3].setpos = M3508_Angle[3];
  if((fabs(m2006set[0].setpos-m2006[0].real_angle)<10)&&(fabs(m2006set[1].setpos-m2006[1].real_angle)<10)&&(control.s[2]==2||control.s[2]==1))Bound_Flag_Point[1]=3;

} else if(Bound_Flag_Point[1]==3&&(control.s[2]==2||control.s[2]==1)) { /3点*******/ MOTOR_PID_CHANGE(0); if(control.s[2]==3)bound_flag_point2=1;

  Leg_Point(0,&link[0],-Height_body_start,0);
  Leg_Point(1,&link[1],-Height_body_start,0);
  Leg_Point(2,&link[2],-Height_body_start,0);
  Leg_Point(3,&link[3],-Height_body_start,0);
  Inverse_leg(&leg[0],&link[0]);
  Inverse_leg(&leg[1],&link[1]);
  Inverse_leg(&leg[2],&link[2]);
  Inverse_leg(&leg[3],&link[3]);

  M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio;	 //后 can2 ID1,2 腿1
  M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;

  M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio;	   //后 can2 ID3,4 腿3
  M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio;   //前 can1 ID1,2 腿2
  M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio;	   //前 can1 ID3,4 腿4
  M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

  m2006set[0].setspeed  = point_Speed[3];
  m2006set[1].setspeed  = point_Speed[3];
  m2006set[2].setspeed  = point_Speed[3];
  m2006set[3].setspeed  = point_Speed[3];
  m3508set[0].setspeed  = point_Speed[3];
  m3508set[1].setspeed  = point_Speed[3];
  m3508set[2].setspeed  = point_Speed[3];
  m3508set[3].setspeed  = point_Speed[3];

  m2006set[0].setpos = M2006_Angle[0];
  m2006set[1].setpos = M2006_Angle[1];
  m2006set[2].setpos = M2006_Angle[2];
  m2006set[3].setpos = M2006_Angle[3];
  m3508set[0].setpos = M3508_Angle[0];
  m3508set[1].setpos = M3508_Angle[1];
  m3508set[2].setpos = M3508_Angle[2];
  m3508set[3].setpos = M3508_Angle[3];
  if((fabs(m3508set[1].setpos-m3508[1].real_angle)<20)&&(fabs(m3508set[0].setpos-m3508[0].real_angle)<20)&&(control.s[2]==2||control.s[2]==1))Bound_Flag_Point[1]=4;

} /第4个点***********/ else if(Bound_Flag_Point[1]==4&&(control.s[2]==2||control.s[2]==1)) { MOTOR_PID_CHANGE(1); if(control.s[2]==3)bound_flag_point2=1;

  Leg_Point(0,&link[0],-Height_body_start-60,120);
  Leg_Point(1,&link[1],-Height_body_start-60,120);
  Leg_Point(2,&link[2],-Height_body_start-60,120);
  Leg_Point(3,&link[3],-Height_body_start-60,120);
  Inverse_leg(&leg[0],&link[0]);
  Inverse_leg(&leg[1],&link[1]);
  Inverse_leg(&leg[2],&link[2]);
  Inverse_leg(&leg[3],&link[3]);

  M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio;	 //后 can2 ID1,2 腿1
  M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;

  M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio;	   //后 can2 ID3,4 腿3
  M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio;   //前 can1 ID1,2 腿2
  M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio;	   //前 can1 ID3,4 腿4
  M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

  m2006set[0].setspeed  = point_Speed[4];
  m2006set[1].setspeed  = point_Speed[4];
  m2006set[2].setspeed  = point_Speed[4];
  m2006set[3].setspeed  = point_Speed[4];
  m3508set[0].setspeed  = point_Speed[4];
  m3508set[1].setspeed  = point_Speed[4];
  m3508set[2].setspeed  = point_Speed[4];
  m3508set[3].setspeed  = point_Speed[4];

  m2006set[0].setpos = M2006_Angle[0];
  m2006set[1].setpos = M2006_Angle[1];
  m2006set[2].setpos = M2006_Angle[2];
  m2006set[3].setpos = M2006_Angle[3];
  m3508set[0].setpos = M3508_Angle[0];
  m3508set[1].setpos = M3508_Angle[1];
  m3508set[2].setpos = M3508_Angle[2];
  m3508set[3].setpos = M3508_Angle[3];
  if((fabs(m2006set[1].setpos-m2006[1].real_angle)<20)&&(fabs(m2006set[0].setpos-m2006[0].real_angle)<20)&&(control.s[2]==2||control.s[2]==1)) {
      Bound_Flag_Point[1]=5;
  }

} else if(Bound_Flag_Point[1]==5&&(control.s[2]==2||control.s[2]==1)) { MOTOR_PID_CHANGE(1); if(bound_flag_point2==5&&control.s[2]==3)bound_flag_point2=1;

  Leg_Point(0,&link[0],-Height_body_start-60,-30);
  Leg_Point(1,&link[1],-Height_body_start-60,-30);
  Leg_Point(2,&link[2],-Height_body_start-60,-30);
  Leg_Point(3,&link[3],-Height_body_start-60,-30);
  Inverse_leg(&leg[0],&link[0]);
  Inverse_leg(&leg[1],&link[1]);
  Inverse_leg(&leg[2],&link[2]);
  Inverse_leg(&leg[3],&link[3]);

  M2006_Angle[0]=(RESET_ANGLE+leg[0].real_angle[0])*M3508Reduction_Ratio;	 //后 can2 ID1,2 腿1
  M2006_Angle[1]=(-RESET_ANGLE2-leg[0].real_angle[1])*M3508Reduction_Ratio;
  M2006_Angle[2]=(-RESET_ANGLE-leg[1].real_angle[0])*M3508Reduction_Ratio;	   //后 can2 ID3,4 腿2
  M2006_Angle[3]=(RESET_ANGLE2+leg[1].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[0]=(RESET_ANGLE+leg[2].real_angle[0])*M3508Reduction_Ratio;   //前 can1 ID1,2 腿3
  M3508_Angle[1]=(-RESET_ANGLE2-leg[2].real_angle[1])*M3508Reduction_Ratio;

  M3508_Angle[2]=(-RESET_ANGLE-leg[3].real_angle[0])*M3508Reduction_Ratio;	   //前 can1 ID3,4 腿4
  M3508_Angle[3]=(RESET_ANGLE2+leg[3].real_angle[1])*M3508Reduction_Ratio;

  m2006set[0].setspeed  = point_Speed[5];
  m2006set[1].setspeed  = point_Speed[5];
  m2006set[2].setspeed  = point_Speed[5];
  m2006set[3].setspeed  = point_Speed[5];
  m3508set[0].setspeed  = point_Speed[5];
  m3508set[1].setspeed  = point_Speed[5];
  m3508set[2].setspeed  = point_Speed[5];
  m3508set[3].setspeed  = point_Speed[5];

  m2006set[0].setpos = M2006_Angle[0];
  m2006set[1].setpos = M2006_Angle[1];
  m2006set[2].setpos = M2006_Angle[2];
  m2006set[3].setpos = M2006_Angle[3];
  m3508set[0].setpos = M3508_Angle[0];
  m3508set[1].setpos = M3508_Angle[1];
  m3508set[2].setpos = M3508_Angle[2];
  m3508set[3].setpos = M3508_Angle[3];
  if((fabs(m2006set[1].setpos-m2006[1].real_angle)<50)&&(fabs(m2006set[0].setpos-m2006[0].real_angle)<50)&&(control.s[2]==2||control.s[2]==1)) {
      Bound_Flag_Point[1]=6;
  }

} else if(Bound_Flag_Point[1]==6&&(control.s[2]==2||control.s[2]==1))

{if(control.s[2]==3)Bound_Flag_Point[1]=1;}

}

52 KiB Raw Blame History Unescape Escape

52 KiB

Raw Blame History