讓XR806控制四足機器人，單腿運動、步態(tài)規(guī)劃都能行

%擺線方程（matlab）sigma=2*pi*t/(Ts);xep=(xf-xs)*((sigma-sin(sigma))/(2*pi))+xs;zep=h*(1-cos(sigma))/2+zs;x=[x,xep];z=[z,zep];

int Balance(float Angle,float Gyro,int Middle,float Balance_Kp,float Balance_Kd){ ? ? float Angle_bias,Gyro_bias; ?int balance; ?Angle_bias=Middle-Angle; ? ? ? ? ? ? ? ? ? ? ?  ?Gyro_bias=0-Gyro; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?balance=Balance_Kp*Angle_bias+Gyro_bias*Balance_Kd;  ?return balance;}

 if(HAL_UART_Init(UARTID, ¶m) != HAL_OK) ?return -1; /*使能DMA*/ if (HAL_UART_EnableTxDMA(UARTID) != HAL_OK) ?return -2; if (HAL_UART_EnableRxDMA(UARTID) != HAL_OK) ?return -3;

HAL_UART_Transmit_DMA(UARTID, (uint8_t *)buffer,sizeof(buffer));

 switch(RxBuffer_control) { ?case 'W': Motor_Control( 1, ?1, ?1, ?1); break; ?case 'S': Motor_Control(-1, -1, -1, -1); ?break; ?case 'A': Motor_Control(-1, -1, ?1, ?1); ?break; ?case 'D': Motor_Control( 1, ?1, -1, -1); ?break; ?case 'P': Motor_Control( 0, ?0, ?0, ?0); ?break; }