【六足机器人】03步态算法

温馨提示：此部分内容需要较强的数学能力，包括但不限于矩阵运算、坐标变换、数学几何。 

一、数学知识

1.1 正逆运动学（几何法）

逆运动学解算函数
// 逆运动学-->计算出三个角度
void inverse_caculate(double x, double y, double z)
{
    double L1 = 0.054; // 单位m
    double L2 = 0.061;
    double L3 = 0.155;
    double R = sqrt(x * x + y * y);
    double aerfaR = atan2(-z, R - L1); // 使用atan2以获得正确的象限
    double Lr = sqrt(z * z + (R - L1) * (R - L1));
    double aerfa1 = acos((L2 * L2 + Lr * Lr - L3 * L3) / (2 * Lr * L2));
    theta1_new = atan2(y, x); // atan2自动处理y=0的情况
    theta2_new = aerfa1 - aerfaR;
    double aerfa2 = acos((Lr * Lr + L3 * L3 - L2 * L2) / (2 * Lr * L3));
    theta3_new = -(aerfa1 + aerfa2);
}
正运动学解算函数 
// 正运动学-->计算出坐标(以COXA为原点)
void forward_kinematics(double theta1, double theta2, double theta3)
{
    double L1 = 0.054; // 单位m
    double L2 = 0.061;
    double L3 = 0.155;
    double Lr = L2 * L2 + L3 * L3 - 2 * L2 * L3 * cos(PI - theta3);
    double aerfa1 = acos((Lr * Lr + L2 * L2 - L3 * L3) / (2 * Lr * L2));
    double aerfaR = aerfa1 - theta2;
    my_z = -Lr * cos(aerfaR);
    double R = L1 + Lr * sin(aerfaR);
    my_x = R * cos(theta1);
    my_y = R * sin(theta1);
}
封装后的代码
// 正运动学解算(输入关节角度计算足端坐标)
void Forward_kinematics(double theta1, double theta2, double theta3, uint8_t leg)
{	
	Myaxis_Init(&Pi3_axis[leg]);
	Pi3_axis[leg].x = cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));
	Pi3_axis[leg].y = sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));
	Pi3_axis[leg].x = L3 * sin(theta2 + theta3) + L2 * sin(theta2);
}
// 逆运动学解算(根据足端坐标计算出三个角度rad)
void Inverse_Kinematics(double x, double y, double z, uint8_t leg)
{
	Hexapod_thetas_Init(&Hexapod_leg[leg]);
	double R = sqrt(x * x + y * y);
	double aerfaR = atan2(-z, R - L1); // 使用atan2以获得正确的象限
	double Lr = sqrt(z * z + (R - L1) * (R - L1));
	double aerfa1 = acos((L2 * L2 + Lr * Lr - L3 * L3) / (2 * Lr * L2));
	Hexapod_leg[leg].Theta[0] = atan2(y, x); // atan2自动处理y=0的情况
	Hexapod_leg[leg].Theta[1] = aerfa1 - aerfaR;
	double aerfa2 = acos((Lr * Lr + L3 * L3 - L2 * L2) / (2 * Lr * L3));
	Hexapod_leg[leg].Theta[2] = -(aerfa1 + aerfa2);
}
 

1.2 DH参数

1.2.1 旋转矩阵变换

1.2.2 坐标变换

1.2.3 DH参数（标准版/改进版）

 

1.2.4 MATLAB仿真代码
D-H参数
                            单位:mm
    关节转角    关节距离    连杆长度        转角
    Theta(n)     d(n)       a(n-1)     Alpha(n-1)    
    theta1        0           0             0
    theta2        0           54           pi/2
    theta3        0           61            0
      0           0           155           0

// 正运动解算
x = cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));
y = sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));
z = L3 * sin(theta2 + theta3) + L2 * sin(theta2);

// 逆运动解算
L1 = 0.054; %单位m
L2 = 0.061;
L3 = 0.155;
R = sqrt(x * x + y * y);
aerfaR = atan2(-z, R - L1); %使用atan2以获得正确的象限
Lr = sqrt(z * z + (R - L1) * (R - L1));
aerfa1 = acos((L2 * L2 + Lr * Lr - L3 * L3) / (2 * Lr * L2));
theta1_new = atan2(y, x); %atan2自动处理y=0的情况
theta2_new = aerfa1 - aerfaR;
aerfa2 = acos((Lr * Lr + L3 * L3 - L2 * L2) / (2 * Lr * L3));
theta3_new = -(aerfa1 + aerfa2);

二、步态算法

2.1 总线舵机通信协议

servo.c

/**************************************************************************************************************/
/******************************************** 算法控制 2024.9.10 *********************************************/
/********************************************   六足机器人：GL   *********************************************/
/*************************************************************************************************************/

#include "servo.h"
#include "usart.h"
#include <stdio.h>
#include "stdarg.h"
#include "ControlServo.h"
#include "arm_math.h"

uint16_t batteryVolt; // 电压

/****************************************************************************************************************
	舵机控制板实现六足机器人	舵机控制板实现六足机器人	舵机控制板实现六足机器人	舵机控制板实现六足机器人
****************************************************************************************************************/

// 功能：控制单个舵机转动
void moveServo(uint8_t servoID, uint16_t Position, uint16_t Time)
{
	HexapodTxBuf[0] = HexapodTxBuf[1] = FRAME_HEADER; // 填充帧头
	HexapodTxBuf[2] = 8;							  // 数据长度=要控制舵机数*3+5，此处=1*3+5
	HexapodTxBuf[3] = CMD_SERVO_MOVE;				  // 填充舵机移动指令
	HexapodTxBuf[4] = 1;							  // 要控制的舵机个数
	HexapodTxBuf[5] = GET_LOW_BYTE(Time);			  // 取得时间的低八位1
	HexapodTxBuf[6] = GET_HIGH_BYTE(Time);			  // 取得时间的高八位
	HexapodTxBuf[7] = servoID;						  // 舵机ID
	HexapodTxBuf[8] = GET_LOW_BYTE(Position);		  // 取得目标位置的低八位
	HexapodTxBuf[9] = GET_HIGH_BYTE(Position);		  // 取得目标位置的高八位

	// HAL_UART_Transmit_DMA(&huart1, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送给串口打印
	// printf("\r\n发送给舵机的指令:");
	// printf("\r\n");
	HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送给控制板
}

// 控制多个舵机转动
// Num:舵机个数,Time:转动时间,...:舵机ID,转动角，舵机ID,转动角度 如此类推
void moveServos(uint8_t Num, uint16_t Time, ...)
{
	uint8_t index = 7;
	uint8_t i = 0;
	uint16_t temp;
	va_list arg_ptr; //

	va_start(arg_ptr, Time); // 取得可变参数首地址
	if (Num < 1 || Num > 32)
	{
		return; // 舵机数不能为零和大与32，时间不能小于0
	}
	HexapodTxBuf[0] = HexapodTxBuf[1] = FRAME_HEADER; // 填充帧头
	HexapodTxBuf[2] = Num * 3 + 5;					  // 数据长度 = 要控制舵机数 * 3 + 5
	HexapodTxBuf[3] = CMD_SERVO_MOVE;				  // 舵机移动指令
	HexapodTxBuf[4] = Num;							  // 要控制舵机数
	HexapodTxBuf[5] = GET_LOW_BYTE(Time);			  // 取得时间的低八位
	HexapodTxBuf[6] = GET_HIGH_BYTE(Time);			  // 取得时间的高八位

	for (i = 0; i < Num; i++)
	{								 // 从可变参数中取得并循环填充舵机ID和对应目标位置
		temp = va_arg(arg_ptr, int); // 可参数中取得舵机ID
		HexapodTxBuf[index++] = GET_LOW_BYTE(((uint16_t)temp));
		temp = va_arg(arg_ptr, int);							// 可变参数中取得对应目标位置
		HexapodTxBuf[index++] = GET_LOW_BYTE(((uint16_t)temp)); // 填充目标位置低八位
		HexapodTxBuf[index++] = GET_HIGH_BYTE(temp);			// 填充目标位置高八位
	}
	va_end(arg_ptr); // 置空arg_ptr
	// printf("动作组指令：");
	HAL_UART_Transmit_DMA(&huart1, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送给串口打印
	// printf("%s", HexapodTxBuf);
	HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送给控制板
}

// 控制一条腿 --> 三个舵机
// ID：舵机ID，PT：舵机位置
void Move_Hexapod_Leg(uint8_t Num, uint16_t Time, uint16_t ID1, uint16_t PT1, uint16_t ID2, uint16_t PT2, uint16_t ID3, uint16_t PT3)
{
	uint8_t index = 7;

	HexapodTxBuf[0] = HexapodTxBuf[1] = FRAME_HEADER; // 填充帧头
	HexapodTxBuf[2] = Num * 3 + 5;					  // 数据长度 = 要控制舵机数 * 3 + 5
	HexapodTxBuf[3] = CMD_SERVO_MOVE;				  // 舵机移动指令
	HexapodTxBuf[4] = Num;							  // 要控制舵机数
	HexapodTxBuf[5] = GET_LOW_BYTE(Time);			  // 取得时间的低八位
	HexapodTxBuf[6] = GET_HIGH_BYTE(Time);			  // 取得时间的高八位

	HexapodTxBuf[index++] = ID1;				// 填充舵机ID
	HexapodTxBuf[index++] = GET_LOW_BYTE(PT1);	// 填充目标位置低八位
	HexapodTxBuf[index++] = GET_HIGH_BYTE(PT1); // 填充目标位置高八位
	HexapodTxBuf[index++] = ID2;				// 填充舵机ID
	HexapodTxBuf[index++] = GET_LOW_BYTE(PT2);	// 填充目标位置低八位
	HexapodTxBuf[index++] = GET_HIGH_BYTE(PT2); // 填充目标位置高八位
	HexapodTxBuf[index++] = ID3;				// 填充舵机ID
	HexapodTxBuf[index++] = GET_LOW_BYTE(PT3);	// 填充目标位置低八位
	HexapodTxBuf[index++] = GET_HIGH_BYTE(PT3); // 填充目标位置高八位

	HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送
}

// 运行指定动作组
// Times:执行次数
void runActionGroup(uint8_t numOfAction, uint16_t Times)
{
	HexapodTxBuf[0] = HexapodTxBuf[1] = FRAME_HEADER; // 填充帧头
	HexapodTxBuf[2] = 5;							  // 数据长度，数据帧除帧头部分数据字节数，此命令固定为5
	HexapodTxBuf[3] = CMD_ACTION_GROUP_RUN;			  // 填充运行动作组命令
	HexapodTxBuf[4] = numOfAction;					  // 填充要运行的动作组号
	HexapodTxBuf[5] = GET_LOW_BYTE(Times);			  // 取得要运行次数的低八位
	HexapodTxBuf[6] = GET_HIGH_BYTE(Times);			  // 取得要运行次数的高八位

	HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, 7); // 发送
																// HAL_UART_Transmit_DMA(&huart1, (uint8_t *)HexapodTxBuf, 7);   //发送
}

// 停止动作组运行
void stopActionGroup(void)
{
	HexapodTxBuf[0] = FRAME_HEADER; // 填充帧头
	HexapodTxBuf[1] = FRAME_HEADER;
	HexapodTxBuf[2] = 2;					 // 数据长度，数据帧除帧头部分数据字节数，此命令固定为2
	HexapodTxBuf[3] = CMD_ACTION_GROUP_STOP; // 填充停止运行动作组命令

	HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送
}

// 设定指定动作组的运行速度
void setActionGroupSpeed(uint8_t numOfAction, uint16_t Speed)
{
	HexapodTxBuf[0] = HexapodTxBuf[1] = FRAME_HEADER; // 填充帧头
	HexapodTxBuf[2] = 5;							  // 数据长度，数据帧除帧头部分数据字节数，此命令固定为5
	HexapodTxBuf[3] = CMD_ACTION_GROUP_SPEED;		  // 填充设置动作组速度命令
	HexapodTxBuf[4] = numOfAction;					  // 填充要设置的动作组号
	HexapodTxBuf[5] = GET_LOW_BYTE(Speed);			  // 获得目标速度的低八位
	HexapodTxBuf[6] = GET_HIGH_BYTE(Speed);			  // 获得目标熟读的高八位

	HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送
}

// 设置所有动作组的运行速度
void setAllActionGroupSpeed(uint16_t Speed)
{
	setActionGroupSpeed(0xFF, Speed); // 调用动作组速度设定，组号为0xFF时设置所有组的速度
}

// 发送获取电池电压命令
void getBatteryVoltage(void)
{
	//	uint16_t Voltage = 0;
	HexapodTxBuf[0] = FRAME_HEADER; // 填充帧头
	HexapodTxBuf[1] = FRAME_HEADER;
	HexapodTxBuf[2] = 2;					   // 数据长度，数据帧除帧头部分数据字节数，此命令固定为2
	HexapodTxBuf[3] = CMD_GET_BATTERY_VOLTAGE; // 填充获取电池电压命令

	HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送
}
servo.h
#ifndef __SERVO_H_
#define __SERVO_H_

#include "main.h"
#include "usart.h"

#define FRAME_HEADER 0x55             //帧头
#define CMD_SERVO_MOVE 0x03           //舵机移动指令
#define CMD_ACTION_GROUP_RUN 0x06     //运行动作组指令
#define CMD_ACTION_GROUP_STOP 0x07    //停止动作组指令
#define CMD_ACTION_GROUP_SPEED 0x0B   //设置动作组运行速度
#define CMD_GET_BATTERY_VOLTAGE 0x0F  //获取电池电压指令

#define GET_LOW_BYTE(A) ((uint8_t)(A))		   // 获得A的低八位
#define GET_HIGH_BYTE(A) ((uint8_t)((A) >> 8)) // 获得A的高八位

extern uint16_t batteryVolt;

void moveServo(uint8_t servoID, uint16_t Position, uint16_t Time);
void moveServos(uint8_t Num, uint16_t Time, ...);
void runActionGroup(uint8_t numOfAction, uint16_t Times);
void stopActionGroup(void);
void setActionGroupSpeed(uint8_t numOfAction, uint16_t Speed);
void setAllActionGroupSpeed(uint16_t Speed);
void getBatteryVoltage(void);



// 发送指令名
#define SERVO_MOVE_TIME_WRITE 1
#define SERVO_MOVE_TIME_READ 2
#define SERVO_MOVE_TIME_WAIT_WRITE 7
#define SERVO_MOVE_TIME_WAIT_READ 8
#define SERVO_MOVE_START 11
#define SERVO_MOVE_STOP 12
#define SERVO_ID_WRITE 13
#define SERVO_ID_READ 14
#define SERVO_ANGLE_OFFSET_ADJUST 17
#define SERVO_ANGLE_OFFSET_WRITE 18
#define SERVO_ANGLE_OFFSET_READ 19
#define SERVO_ANGLE_LIMIT_WRITE 20
#define SERVO_ANGLE_LIMIT_READ 21
#define SERVO_VIN_LIMIT_WRITE 22
#define SERVO_VIN_LIMIT_READ 23
#define SERVO_TEMP_MAX_LIMIT_WRITE 24
#define SERVO_TEMP_MAX_LIMIT_READ 25
#define SERVO_TEMP_READ 26
#define SERVO_VIN_READ 27
#define SERVO_POS_READ 28
#define SERVO_OR_MOTOR_MODE_WRITE 29
#define SERVO_OR_MOTOR_MODE_READ 30
#define SERVO_LOAD_OR_UNLOAD_WRITE 31
#define SERVO_LOAD_OR_UNLOAD_READ 32
#define SERVO_LED_CTRL_WRITE 33
#define SERVO_LED_CTRL_READ 34
#define SERVO_LED_ERROR_WRITE 35
#define SERVO_LED_ERROR_READ 36

// 指令长度
#define SERVO_MOVE_TIME_WRITE_LEN 7
#define SERVO_MOVE_TIME_READ_LEN 3
#define SERVO_MOVE_TIME_WAIT_WRITE_LEN 7
#define SERVO_MOVE_TIME_WAIT_READ_LEN 3
#define SERVO_MOVE_START_LEN 3
#define SERVO_MOVE_STOP_LEN 3
#define SERVO_ID_WRITE_LEN 4
#define SERVO_ID_READ_LEN 3
#define SERVO_ANGLE_OFFSET_ADJUST_LEN 4
#define SERVO_ANGLE_OFFSET_WRITE_LEN 3
#define SERVO_ANGLE_OFFSET_READ_LEN 3
#define SERVO_ANGLE_LIMIT_WRITE_LEN 7
#define SERVO_ANGLE_LIMIT_READ_LEN 3
#define SERVO_VIN_LIMIT_WRITE_LEN 7
#define SERVO_VIN_LIMIT_READ_LEN 3
#define SERVO_TEMP_MAX_LIMIT_WRITE_LEN 4
#define SERVO_TEMP_MAX_LIMIT_READ_LEN 3
#define SERVO_TEMP_READ_LEN 3
#define SERVO_VIN_READ_LEN 3
#define SERVO_POS_READ_LEN 3
#define SERVO_OR_MOTOR_MODE_WRITE_LEN 7
#define SERVO_OR_MOTOR_MODE_READ_LEN 3
#define SERVO_LOAD_OR_UNLOAD_WRITE_LEN 4
#define SERVO_LOAD_OR_UNLOAD_READ_LEN 3
#define SERVO_LED_CTRL_WRITE_LEN 4
#define SERVO_LED_CTRL_READ_LEN 3
#define SERVO_LED_ERROR_WRITE_LEN 4
#define SERVO_LED_ERROR_READ_LEN 3


//接收指令长度
#define RECV_SERVO_MOVE_TIME_READ_LEN 7
#define RECV_SERVO_MOVE_TIME_WAIT_READ_LEN 7
#define RECV_SERVO_ID_READ_LEN 4
#define RECV_SERVO_ANGLE_OFFSET_READ_LEN 4
#define RECV_SERVO_ANGLE_LIMIT_READ_LEN 7
#define RECV_SERVO_VIN_LIMIT_READ_LEN 7
#define RECV_SERVO_TEMP_MAX_LIMIT_READ_LEN 4
#define RECV_SERVO_TEMP_READ_LEN 4
#define RECV_SERVO_VIN_READ_LEN 5
#define RECV_SERVO_POS_READ_LEN 5
#define RECV_SERVO_OR_MOTOR_MODE_READ_LEN 7
#define RECV_SERVO_LOAD_OR_UNLOAD_READ_LEN 4
#define RECV_SERVO_LED_CTRL_READ_LEN 4
#define RECV_SERVO_LED_ERROR_READ_LEN 4

#define SERVO_BROADCAST_ID 0xFE // 广播id

#endif

2.2 控制机器人腿

ControlServo.c
#include "ControlServo.h"
#include <math.h>
#include <string.h>
#include <usart.h>
#include <stdio.h>
#include <servo.h>
#include "arm_math.h"

Myaxis Pi3_axis[6];		   // 以髋关节为基准坐标系下的Pi3坐标,i为腿部编号(0-5:A-F)
HexapodLeg Hexapod_leg[6]; // 定义机器人的腿部结构体,i为腿部编号(0-5:A-F)
Myaxis adjhjk;

// 初始化坐标系
void Myaxis_Init(Myaxis *axis)
{
	axis->x = 0;
	axis->y = 0;
	axis->z = 0;
}

// 初始化腿部关节角度值
void Hexapod_thetas_Init(HexapodLeg *Hexapod_leg)
{
	Hexapod_leg->Theta[0] = 0;
	Hexapod_leg->Theta[1] = 0;
	Hexapod_leg->Theta[2] = 0;
}

// 正运动学解算(输入关节角度计算足端坐标)
void Forward_kinematics(double theta1, double theta2, double theta3, uint8_t leg)
{	
	Myaxis_Init(&Pi3_axis[leg]);
	Pi3_axis[leg].x = cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));
	Pi3_axis[leg].y = sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));
	Pi3_axis[leg].x = L3 * sin(theta2 + theta3) + L2 * sin(theta2);
}
// 逆运动学解算(根据足端坐标计算出三个角度rad)
void Inverse_Kinematics(double x, double y, double z, uint8_t leg)
{
	Hexapod_thetas_Init(&Hexapod_leg[leg]);
	double R = sqrt(x * x + y * y);
	double aerfaR = atan2(-z, R - L1); // 使用atan2以获得正确的象限
	double Lr = sqrt(z * z + (R - L1) * (R - L1));
	double aerfa1 = acos((L2 * L2 + Lr * Lr - L3 * L3) / (2 * Lr * L2));
	Hexapod_leg[leg].Theta[0] = atan2(y, x); // atan2自动处理y=0的情况
	Hexapod_leg[leg].Theta[1] = aerfa1 - aerfaR;
	double aerfa2 = acos((Lr * Lr + L3 * L3 - L2 * L2) / (2 * Lr * L3));
	Hexapod_leg[leg].Theta[2] = -(aerfa1 + aerfa2);
}

/********************************************************************************************************
	LX224总线舵机调节范围：
		0 —— 1000		500
		----------------10° --> 10 * (1000/240.0) = temp
						实际发送：500 ± (int)temp
		0 —— 240°
	弧度-->舵机角度：int degrees = (int)(angle * (180.0 / PI) * (1000 / 240.0));
	舵机角度-->弧度：temp = 400*(240/1000)(PI/180)
********************************************************************************************************/
// 角度转换 (弧度-->度数-->舵机角度)
int Angle_convert(double angle)
{
	double degrees = angle * (180.0 / PI);
	int servoAngle = (int)(degrees * (1000 / 240.0));
	return servoAngle;
}

/*************************************************************************************/
/*************************************************************************************
	左边：
		coxa  关节舵机	角度增大：向前 角度减小：向后
		fumer 关节舵机	角度增大：向上 角度减小：向下
		tibia 关节舵机	角度增大：向下 角度减小：向上
	左边：
		coxa  关节舵机	角度增大：向后 角度减小：向前
		fumer 关节舵机	角度增大：向下 角度减小：向上
		tibia 关节舵机	角度增大：向上 角度减小：向下
	// 10	200-800
	// 11	100-900
	// 12	0-1000
	// 13	250-750
	// 14	100-900

	// 2	100-900
*************************************************************************************/
/*************************************************************************************/
// 设置舵机角度(弧度)
void Servo_Set_Position(u8 Servo_ID, double angle, uint16_t Time)
{
	int Servo_angle;
	Servo_angle = Angle_convert(angle);

	// 左边三条腿
	if (Servo_ID > 9)
	{
		if (Servo_ID == 10 || Servo_ID == 13 || Servo_ID == 16) // 如果是coxa关节
		{
			Servo_angle = 500 - Servo_angle;
			if (100 <= Servo_angle && Servo_angle <= 900)
			{
				printf("舵机调试信息coxa : Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
				moveServo(Servo_ID, Servo_angle, Time);
			}
			else
			{
				printf("left_coxa_angle_error!\r\n"); // 打印错误信息
			}
		}

		if (Servo_ID == 11 || Servo_ID == 14 || Servo_ID == 17) // 如果是femur关节
		{
			Servo_angle = 500 - Servo_angle;
			if (50 <= Servo_angle && Servo_angle <= 950)
			{
				printf("舵机调试信息femur: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
				moveServo(Servo_ID, Servo_angle, Time);
			}
			else
			{
				printf("left_femur_angle_error!\r\n"); // 打印错误信息
			}
		}
		if (Servo_ID == 12 || Servo_ID == 15 || Servo_ID == 18) // 如果是tibia关节
		{
			Servo_angle = 500 + Servo_angle;
			if (0 <= Servo_angle && Servo_angle <= 1000)
			{
				printf("舵机调试信息tibia: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
				moveServo(Servo_ID, Servo_angle, Time);
			}
			else
			{
				printf("left_tibia_angle_error!	"); // 打印错误信息
				printf("错误信息: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
			}
		}
	}
	// 右边三条腿
	else
	{
		if (Servo_ID == 1 || Servo_ID == 4 || Servo_ID == 7) // 如果是coxa关节
		{
			Servo_angle = 500 + Servo_angle;
			if (100 <= Servo_angle && Servo_angle <= 900)
			{
				printf("舵机调试信息coxa : Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
				moveServo(Servo_ID, Servo_angle, Time);
			}
			else
			{
				printf("right_coxa_angle_error!\r\n"); // 打印错误信息
			}
		}

		if (Servo_ID == 2 || Servo_ID == 5 || Servo_ID == 8) // 如果是femur关节
		{
			Servo_angle = 500 + Servo_angle;
			if (50 <= Servo_angle && Servo_angle <= 950)
			{
				printf("舵机调试信息femur: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
				moveServo(Servo_ID, Servo_angle, Time);
			}
			else
			{
				printf("right_femur_angle_error!\r\n"); // 打印错误信息
			}
		}

		if (Servo_ID == 3 || Servo_ID == 6 || Servo_ID == 9) // 如果是tibia关节
		{
			Servo_angle = 500 - Servo_angle;
			if (0 <= Servo_angle && Servo_angle <= 1000)
			{
				printf("舵机调试信息tibia: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
				moveServo(Servo_ID, Servo_angle, Time);
			}
			else
			{
				printf("right_tibia_angle_error!	"); // 打印错误信息
				printf("错误信息: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
			}
		}
	}
}

/**
 * @brief 控制一条腿运动
 * @param leg: leg为腿部编号(0-5:A-F)
 * @param HexapodLeg: 腿部结构体,用于存储关节的角度
 * @param Time: 运动时间
 * @retval 无
 */
void Servo_Control_Leg(uint8_t leg, HexapodLeg Hexapod_leg, uint16_t Time)
{
	int base_index = 3 * leg + 1; // 计算当前腿的舵机ID
	for (int seg = 0; seg < 3; seg++)
	{
		int servo_ID = base_index + seg; // 根据关节位置计算舵机ID
		switch (seg)
		{
		case 0: // COXA
			Servo_Set_Position(servo_ID, Hexapod_leg.Theta[0], Time);
			break;
		case 1: // FEMUR
			Servo_Set_Position(servo_ID, Hexapod_leg.Theta[1], Time);
			break;
		case 2: // TIBIA
			Servo_Set_Position(servo_ID, Hexapod_leg.Theta[2], Time);
			break;
		default:
			break;
		}
	}
}
ControlServo.h
/****************************/
//舵机控制板实现六足机器人
/****************************/
#ifndef __CONTROLSERVO_H_
#define __CONTROLSERVO_H_

#include "main.h"

#define PI 3.14159265358979323846

/***********************************************************************
    *   leg4   FL   12  11  10   ** **   1   2   3   FR     leg1
    *   leg5   ML   15  14  13   ** **   4   5   6   MR-----leg2--------逆时针 ↑↑
    *   leg6   HL   18  17  16   ** **   7   8   9   HR     leg3
***********************************************************************/
#define MR 2    //leg2
#define FR 1    //leg1
#define FL 4    //leg4
#define ML 5    //leg5
#define HL 6    //leg6
#define HR 3    //leg3


/**********************************************************************/
/*************************Hexapod3.0基本信息***************************/
/**********************************************************************/
#define L1  0.054  // coxa  关节长度 单位m
#define L2  0.061  // fumer 关节长度 单位m
#define L3  0.155  // tibia 关节长度 单位m

//坐标轴
typedef struct {	
	double x;
	double y;
	double z;		
}Myaxis;

//机器人腿部关节成员
typedef struct{
	//double Theta_Write[3];//用于输出舵机控制信号(反向运动学)
	//double Theta_Read[18];//用于读取舵机角度(正向运动学)
	
	double Theta[3]; //每条腿对应的3个关节角度

    //uint8_t servo_low[18],servo_high[18];//高低位数据接收(测试用)
	
}HexapodLeg;


/**********************************************************************/
/**********************************************************************/


//自己写的函数
void Forward_kinematics(double theta1, double theta2, double theta3, uint8_t leg);   // 正运动学解算(输入关节角度计算足端坐标)
void Inverse_Kinematics(double x,double y,double z, uint8_t leg);                    // 逆运动学解算(根据足端坐标计算出三个角度rad)

void Myaxis_Init(Myaxis *axis);	//初始化坐标系
void Hexapod_init(void);
int Angle_convert(double angle);                                        // 角度转换 (弧度-->度数-->舵机角度)
void Servo_Control_Leg(uint8_t leg, HexapodLeg Hexapod_leg, uint16_t Time);   // 设置舵机角度(弧度)
void Servo_Set_Position(u8 Servo_ID, double angle, uint16_t Time);      // 控制一条腿运动


#endif

2.3 三角步态

Gait.c

/**************************************************************************************************************/
/******************************************** 步态规划 2024.9.10 *********************************************/
/********************************************   六足机器人：GL   *********************************************/
/*************************************************************************************************************/

#include "Gait.h"
#include "servo.h"
#include "ControlServo.h"

// 定义六个coxa关节相对于中央点坐标系下的位置
double body_contact_points[6][4] = {
	{0.07600, 0, 0, 1},			// MR 中右
	{0.07600, 0.08485, 0, 1},	// FR 前右
	{-0.07600, 0.08485, 0, 1},	// FL 前左
	{-0.07600, 0, 0, 1},		// ML 中左
	{-0.07600, -0.08485, 0, 1}, // HL 后左
	{0.07600, -0.08485, 0, 1}	// HR 后右
};

// 定义六个coxa对应中央坐标系的朝向转角(rad)
double body_contact_points_rotation[] = {
	0,					   // MR
	56 * PI / 180,		   // FR
	(56 + 68) * PI / 180,  // FL
	PI,					   // ML
	-(56 + 68) * PI / 180, // HL
	-56 * PI / 180,		   // HR
};

/**********************************************************************/
/*************************Hexapod3.0基本信息***************************/
/**********************************************************************
 *   leg4   FL   12  11  10   ** **   1   2   3   FR     leg1
 *   leg5   ML   15  14  13   ** **   4   5   6   MR-----leg2--------逆时针 ↑↑
 *   leg6   HL   18  17  16   ** **   7   8   9   HR     leg3
 ***********************************************************************/
/**********************************************************************/
// a:coxa	b:fumer	c:tibia
// 初始姿态的三个关节角度设置
typedef struct
{
	double a;
	double b;
	double c;
} hexapod_Position;

hexapod_Position Hexapod_Init_Position;
// Hexapod初始化位置: 在这里修改机器人的初始位置
void init_hexapod_position(hexapod_Position *pos)
{
	pos->a = 0.0;
	// pos->b = 0.4182;
	// pos->c = 4 * 0.4182;
	pos->b = 100.0 * (240.0 / 1000.0) * (PI / 180.0);
	pos->c = -400.0 * (240.0 / 1000.0) * (PI / 180.0);
}

void Hexapod_init(void)
{
	int index = 1;
	int leg = 0;
	int seg = 0;
	init_hexapod_position(&Hexapod_Init_Position);
	for (leg = 0; leg < 6; leg++)
	{
		for (seg = 0; seg < 3; seg++)
		{
			switch (seg)
			{
			case 0: // COXA
				Servo_Set_Position(index, Hexapod_Init_Position.a, 200);
				break;
			case 1: // FEMUR
				Servo_Set_Position(index, Hexapod_Init_Position.b, 200);
				break;
			case 2: // TIBIA
				Servo_Set_Position(index, Hexapod_Init_Position.c, 200);
				break;
			}
			index++;
		}
		// delay_ms(1000);
	}
}

/*****************************************************************************************************************************/
/*步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划*/
/*****************************************************************************************************************************/

//**********************(步态规划)************************//
/*
 *   leg4   FL   12  11  10   D** **A   1   2   3   FR     leg1
 *   leg5   ML   15  14  13   E** **B   4   5   6   MR-----leg2--------逆时针 ↑↑
 *   leg6   HL   18  17  16   F** **C   7   8   9   HR     leg3
 */
//**********************(步态规划)************************//

Myaxis CPi3_axis[6];	// 定义以机身几何中心为绝对坐标系参考下的Pi3坐标,i为腿部编号(0-5:A-F)
Myaxis Pi0_Pi3_axis[6]; // 定义以机身几何中心为绝对坐标系参考下的Pi0Pi3向量
extern Myaxis Pi3_axis[6];
// HexapodLeg Hexapod_Moveleg[6]; // 用于存储移动时的腿部信息
extern HexapodLeg Hexapod_leg[6]; // 定义机器人的腿部结构体,i为腿部编号(0-5:A-F)
// double P[3] = {0.1577, 0, -0.1227}; // 每个coxa坐标下，足末端的位置

/**
 * @brief 初始化机器人中心坐标系下向量Pi0_Pi3的坐标向量坐标
 * @param Y_Tran: 机器人中心坐标系下coxa的纵坐标
 * @param R_Angle: 机器人中心坐标系下coxa的偏转角度
 * @param leg: 腿部编号(0-5:A-F)
 * @retval 无
 */
void Hexapod_center_Position03_Init(double X, double Y, double R_Angle, uint8_t leg)
{
	double theta1 = Hexapod_Init_Position.a;
	double theta2 = Hexapod_Init_Position.b;
	double theta3 = Hexapod_Init_Position.c;

	Myaxis_Init(&CPi3_axis[leg]);	 // 初始化坐标系
	Myaxis_Init(&Pi0_Pi3_axis[leg]); // 初始化坐标系

	CPi3_axis[leg].x = X + cos(R_Angle) * cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2)) - sin(R_Angle) * (sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2)));
	CPi3_axis[leg].y = Y + sin(R_Angle) * (cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2))) + cos(R_Angle) * (sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2)));
	CPi3_axis[leg].z = L3 * sin(theta2 + theta3) + L2 * sin(theta2);

	// CPi3_axis[leg].x = X + 132.9736 * sin(R_Angle);
	// CPi3_axis[leg].y = Y + 132.9736 * cos(R_Angle);	//132.9736为初始站立姿态关节P0到关节P3的直线距离
	// CPi3_axis[leg].z = - 77.3670;							//77.3670为初始站立姿态高度

	/*机身几何中心坐标系中Pi0Pi3向量*/
	Pi0_Pi3_axis[leg].x = CPi3_axis[leg].x - X;
	Pi0_Pi3_axis[leg].y = CPi3_axis[leg].y - Y;
	Pi0_Pi3_axis[leg].z = CPi3_axis[leg].z - 0;
}

/**
 * @brief 初始站立状态的位姿和Pi3坐标初始化
 * @retval 无
 */
void Hexapod_Gait_Init(void)
{
	// 计算初始站立姿态的Pi3相对于腿部基准坐标的坐标位置
	Forward_kinematics(0, -(PI / 4), 2 * PI / 3, 0); // 0号——A_Leg
	Forward_kinematics(0, -(PI / 4), 2 * PI / 3, 1); // 1号——B_Leg
	Forward_kinematics(0, -(PI / 4), 2 * PI / 3, 2); // 2号——C_Leg
	Forward_kinematics(0, -(PI / 4), 2 * PI / 3, 3); // 3号——D_Leg
	Forward_kinematics(0, -(PI / 4), 2 * PI / 3, 4); // 4号——E_Leg
	Forward_kinematics(0, -(PI / 4), 2 * PI / 3, 5); // 5号——F_Leg

	// 输出初始站立姿态的角度且固定腿部基准坐标系
	Inverse_Kinematics(Pi3_axis[0].x, Pi3_axis[0].y, Pi3_axis[0].z, 0); // 0号——A_Leg
	Inverse_Kinematics(Pi3_axis[1].x, Pi3_axis[1].y, Pi3_axis[1].z, 1); // 1号——B_Leg
	Inverse_Kinematics(Pi3_axis[2].x, Pi3_axis[2].y, Pi3_axis[2].z, 2); // 2号——C_Leg
	Inverse_Kinematics(Pi3_axis[3].x, Pi3_axis[3].y, Pi3_axis[3].z, 3); // 3号——D_Leg
	Inverse_Kinematics(Pi3_axis[4].x, Pi3_axis[4].y, Pi3_axis[4].z, 4); // 4号——E_Leg
	Inverse_Kinematics(Pi3_axis[5].x, Pi3_axis[5].y, Pi3_axis[5].z, 5); // 5号——F_Leg

	// 得到不同状态下的Pi0Pi3向量相对于几何中心的位置描述
	Hexapod_center_Position03_Init(0.07600, 0.08485, 56 * PI / 180, 0);			  // 0号——A_Leg
	Hexapod_center_Position03_Init(0.07600, 0, 0, 1);							  // 1号——B_Leg
	Hexapod_center_Position03_Init(0.07600, -0.08485, -56 * PI / 180, 2);		  // 2号——C_Leg
	Hexapod_center_Position03_Init(-0.07600, 0.08485, (56 + 68) * PI / 180, 3);	  // 3号——D_Leg
	Hexapod_center_Position03_Init(-0.07600, 0, PI, 4);							  // 4号——E_Leg
	Hexapod_center_Position03_Init(-0.07600, -0.08485, -(56 + 68) * PI / 180, 5); // 5号——F_Leg
}


//**********************(纵向三角步态规划)************************//
//*************************ACE——BDF交替**************************//

static Myaxis Move_Position03;		   // 重新缓存Pi3坐标
/*ACE转角-前进*/
void ACE_Advance_Gait_LegUp(void)
{
	Move_Position03 = Pi3_axis[0]; // 依照每条腿的机械结构和关节转动特点设立的坐标系,所以腿部基准坐标系都一样，这里随便取一个Pi3坐标
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	Inverse_Kinematics(Move_Position03.x - SL_half_edge * 0.25, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 0); // 0号——A_Leg
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.25, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 2); // 2号——C_Leg
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 4);						  // 4号——E_Leg
	// 发送控制指令
	Servo_Control_Leg(A, Hexapod_leg[A], pace_time);
	Servo_Control_Leg(C, Hexapod_leg[C], pace_time);
	Servo_Control_Leg(E, Hexapod_leg[E], pace_time);

	delay_ms(pace_time);
}

/*ACE转角-落脚*/
void ACE_Advance_Gait_LegMove_1(void)
{
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	Inverse_Kinematics(Move_Position03.x - SL_half_edge * 0.5, Move_Position03.y + SL_half_edge, Move_Position03.z, 0); // 0号——A_Leg
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y + SL_half_edge, Move_Position03.z, 2); // 2号——C_Leg
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y + SL_half_edge, Move_Position03.z, 4);						// 4号——E_Leg

	// 发送控制指令
	Servo_Control_Leg(A, Hexapod_leg[A], pace_time);
	Servo_Control_Leg(C, Hexapod_leg[C], pace_time);
	Servo_Control_Leg(E, Hexapod_leg[E], pace_time);

	delay_ms(pace_time);
}

/*ACE移动*/
void ACE_Advance_Gait_LegMove_2(void)
{
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y - SL_half_edge, Move_Position03.z, 0); // 0号——A_Leg
	Inverse_Kinematics(Move_Position03.x - SL_half_edge * 0.5, Move_Position03.y - SL_half_edge, Move_Position03.z, 2); // 2号——C_Leg
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y - SL_half_edge, Move_Position03.z, 4);						// 4号——E_Leg

	// 发送控制指令
	Servo_Control_Leg(A, Hexapod_leg[A], pace_time);
	Servo_Control_Leg(C, Hexapod_leg[C], pace_time);
	Servo_Control_Leg(E, Hexapod_leg[E], pace_time);

	delay_ms(pace_time);
}

/*BDF抬脚-转角*/
void BDF_Advance_Gait_LegUp(void)
{
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 1);						// 1号——B_Leg
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.25, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 3); // 3号——D_Leg
	Inverse_Kinematics(Move_Position03.x - SL_half_edge * 0.25, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 5); // 5号——F_Leg

	// 发送控制指令
	Servo_Control_Leg(B, Hexapod_leg[B], pace_time);
	Servo_Control_Leg(D, Hexapod_leg[D], pace_time);
	Servo_Control_Leg(F, Hexapod_leg[F], pace_time);

	delay_ms(pace_time);
}

/*BDF转角-落脚*/
void BDF_Advance_Gait_LegMove_1(void)
{
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y + SL_half_edge, Move_Position03.z, 1);					  // 1号——B_Leg
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y + SL_half_edge, Move_Position03.z, 3); // 3号——D_Leg
	Inverse_Kinematics(Move_Position03.x - SL_half_edge * 0.5, Move_Position03.y + SL_half_edge, Move_Position03.z, 5); // 5号——F_Leg

	// 发送控制指令
	Servo_Control_Leg(B, Hexapod_leg[B], pace_time);
	Servo_Control_Leg(D, Hexapod_leg[D], pace_time);
	Servo_Control_Leg(F, Hexapod_leg[F], pace_time);

	delay_ms(pace_time);
}

/*BDF前进*/
void BDF_Advance_Gait_LegMove_2(void)
{
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y - SL_half_edge, Move_Position03.z, 1);					  // 1号——B_Leg
	Inverse_Kinematics(Move_Position03.x - SL_half_edge * 0.5, Move_Position03.y - SL_half_edge, Move_Position03.z, 3); // 3号——D_Leg
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y - SL_half_edge, Move_Position03.z, 5); // 5号——F_Leg

	// 发送控制指令
	Servo_Control_Leg(B, Hexapod_leg[B], pace_time);
	Servo_Control_Leg(D, Hexapod_leg[D], pace_time);
	Servo_Control_Leg(F, Hexapod_leg[F], pace_time);

	delay_ms(pace_time);
}

/*Hexapod前进步态*/
void Hexapod_Advance_Gait(void)
{
	BDF_Advance_Gait_LegUp();
	ACE_Advance_Gait_LegMove_2();
	BDF_Advance_Gait_LegMove_1();

	ACE_Advance_Gait_LegUp();
	BDF_Advance_Gait_LegMove_2();
	ACE_Advance_Gait_LegMove_1();
}

/*
*********************(横向三角步态规划)************************
*************************ACE——BDF交替*************************
*/

void ACE_RightTrans_Gait_LegUp(void)
{
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y - SL_half_edge * 0.25, Move_Position03.z + Hip_edge, 0);	 // 0号——A_Leg
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y + SL_half_edge * 0.25, Move_Position03.z + Hip_edge, 2);  // 2号——C_Leg	
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y, Move_Position03.z + Hip_Redge, 4);						 // 4号——E_Leg

	// 发送控制指令
	Servo_Control_Leg(A, Hexapod_leg[A], pace_time);
	Servo_Control_Leg(C, Hexapod_leg[C], pace_time);
	Servo_Control_Leg(E, Hexapod_leg[E], pace_time);

	delay_ms(pace_time);
}

void ACE_RightTrans_Gait_LegMove_1(void)
{
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	Inverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y - SL_half_edge * 0.5, Move_Position03.z, 0);	 // 0号——A_Leg
	Inverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z, 2);  // 2号——C_Leg	
	Inverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y, Move_Position03.z, 4);						 // 4号——E_Leg

	// 发送控制指令
	Servo_Control_Leg(A, Hexapod_leg[A], pace_time);
	Servo_Control_Leg(C, Hexapod_leg[C], pace_time);
	Servo_Control_Leg(E, Hexapod_leg[E], pace_time);

	delay_ms(pace_time);
}

void ACE_RightTrans_Gait_LegMove_2(void)
{
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 2); // 2号——C_Leg
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 4); // 4号——E_Leg
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 0); // 0号——A_Leg
}

void BDF_RightTrans_Gait_LegUp(void)
{
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度	
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y, Move_Position03.z + Hip_Redge, 1); // 1号——B_Leg
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y + SL_half_edge * 0.25, Move_Position03.z + Hip_edge, 3);  // 3号——D_Leg
	Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y - SL_half_edge * 0.25, Move_Position03.z + Hip_edge, 5);  // 5号——F_Leg

	// 发送控制指令
	Servo_Control_Leg(B, Hexapod_leg[B], pace_time);
	Servo_Control_Leg(D, Hexapod_leg[D], pace_time);
	Servo_Control_Leg(F, Hexapod_leg[F], pace_time);

	delay_ms(pace_time);
}

void BDF_RightTrans_Gait_LegMove_1(void)
{
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度	
	Inverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y, Move_Position03.z, 1); // 1号——B_Leg
	Inverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z, 3);  // 3号——D_Leg
	Inverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y - SL_half_edge * 0.5, Move_Position03.z, 5);  // 5号——F_Leg

	// 发送控制指令
	Servo_Control_Leg(B, Hexapod_leg[B], pace_time);
	Servo_Control_Leg(D, Hexapod_leg[D], pace_time);
	Servo_Control_Leg(F, Hexapod_leg[F], pace_time);

	delay_ms(pace_time);
}

void BDF_RightTrans_Gait_LegMove_2(void)
{
	// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 5);						  // 2号——C_Leg
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 1); // 4号——E_Leg
	Inverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 3);						  // 0号——A_Leg
}

/*Hexapod右转*/
void Hexapod_RightTrans_Gait(void)
{
	ACE_RightTrans_Gait_LegUp();
	BDF_RightTrans_Gait_LegMove_2();
	ACE_RightTrans_Gait_LegMove_1();

	BDF_RightTrans_Gait_LegUp();
	ACE_RightTrans_Gait_LegMove_2();
	BDF_RightTrans_Gait_LegMove_1();
}

/*
*********************(右转三角步态规划)************************
*************************ACE——BDF交替*************************
*/

void ACE_TurnRight_Gait_LegUp(void)
{	
	// //前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	// Inverse_Kinematics(Move_Position3_X + SL_half * 1/2,Move_Position3_Y ,Move_Position3_Z + Hip_Redge,2);//2号——C_Leg
	// Inverse_Kinematics(Move_Position3_X - SL_half * 1/2,Move_Position3_Y,Move_Position3_Z + Hip_Redge,4);//4号——E_Leg	
	// Inverse_Kinematics(Move_Position3_X + SL_half * 1/2,Move_Position3_Y ,Move_Position3_Z + Hip_Redge,0);//0号——A_Leg					
}

void ACE_TurnRight_Gait_LegMove_1(void)
{
	
}

void ACE_TurnRight_Gait_LegMove_2(void)
{

}

void BDF_TurnRight_Gait_LegUp(void)
{	
	// //前、中、后腿的姿态规划不一样,分开用逆运动学取角度
	// Inverse_Kinematics(Move_Position3_X - SL_half * 1/2,Move_Position3_Y ,Move_Position3_Z + Hip_Redge,2);//2号——C_Leg
	// Inverse_Kinematics(Move_Position3_X + SL_half * 1/2,Move_Position3_Y,Move_Position3_Z + Hip_Redge,4);//4号——E_Leg	
	// Inverse_Kinematics(Move_Position3_X - SL_half * 1/2,Move_Position3_Y ,Move_Position3_Z + Hip_Redge,0);//0号——A_Leg				
}

void BDF_TurnRight_Gait_LegMove_1(void)
{

}

void BDF_TurnRight_Gait_LegMove_2(void)
{

}

void Hexapod_Turnight_Gait(void)
{
	ACE_TurnRight_Gait_LegUp();
	BDF_TurnRight_Gait_LegMove_2();
	ACE_TurnRight_Gait_LegMove_1();
		
	BDF_TurnRight_Gait_LegUp();
	ACE_TurnRight_Gait_LegMove_2();
	BDF_TurnRight_Gait_LegMove_1();
}



/************************************************************************************************************************************************/
/************************************************************* 算法控制 2024.7.30 **************************************************************/
/***********************************************************************************************************************************************/
Gait,h
#ifndef __GAIT_H_
#define __GAIT_H_
#include "main.h"

#define Hip_edge 0.01  // 新足端执行器抬升高度
#define Hip_Redge 0.01 // 足端执行器抬升高度
// #define SL_half_edge 0.10342409f/2 // 中心舵机1,7,10,16在x轴方向的步长(新步长)———髋关节转动15度确立的步长
#define SL_half_edge 0.0517204
// #define SL_half_edge 45.7388  // 中心舵机1,7,10,16在x轴方向的步长(新步长)———髋关节转动20度确立的步长
#define SL_half 66.4868 // 中心舵机1,7,10,16在x轴方向的步长(老步长)———髋关节转动30度确立的步长
#define pace_time 500   // 步伐时间(ms)

// 定义机器人的腿部序号
#define A 0
#define B 1
#define C 2
#define D 3
#define E 4
#define F 5

/*自己写的函数*/

void Hexapod_center_Position03_Init(double X_Tran, double Y_Tran, double R_Angle, uint8_t leg); // 初始化机器人中心坐标系下向量Pi0_Pi3的坐标向量坐标
void Hexapod_Gait_Init(void);                                                                   // 初始站立状态的位姿和坐标初始化
void Hexapod_Advance_Gait(void);                                                                // Hexapod前进
void Hexapod_Back_Gait(void);                                                                   // Hexapod后退
void Hexapod_LeftTrans_Gait(void);                                                              // Hexapod左平移
void Hexapod_RightTrans_Gait(void);                                                             // Hexapod右平移
void Hexapod_TurnLeft_Gait(void);                                                               // Hexapod左转
void Hexapod_Turnight_Gait(void);                                                               // Hexapod右转

#endif

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/929488.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

【WRF后处理】WRF时区（UTC）需转化为北京时间（CST）！！！

【WRF后处理】WRF时区（UTC）需转化为北京时间（CST）！！！

目录 WRF运行时间标准注意事项-本地时区问题 输入数据&#xff1a;ERA5时间标准ERA5数据和WRF模型需要转换为北京时间&#xff01;&#xff01;&#xff01;北京时间&#xff08;CST&#xff09;与协调世界时&#xff08;UTC&#xff09;的关系转换方法 参考 WRF运行时间标准 …
阅读更多...
如何撰写标准操作流程（SOP）：9个快速步骤

如何撰写标准操作流程（SOP）：9个快速步骤

要了解一个公司日常的实际运营情况&#xff0c;只需查看他们的标准操作流程&#xff08;SOP&#xff09;即可。 尽管SOP在任何成功组织中都扮演着至关重要的角色&#xff0c;但它们往往声名不佳。 人们通常认为&#xff0c;这些针对日常任务的详细指令只是为了限制员工的灵活性…
阅读更多...
InfluxDB 集成 Grafana

InfluxDB 集成 Grafana

将InfluxDB集成到Grafana进行详细配置通常包括以下几个步骤&#xff1a;安装与配置InfluxDB、安装与配置Grafana、在Grafana中添加InfluxDB数据源以及创建和配置仪表板。以下是一个详细的配置指南&#xff1a; 一、安装与配置InfluxDB 下载与安装&#xff1a; 从InfluxDB的官…
阅读更多...
SpringBoot项目启动报错-Slf4j日志相关类找不到

SpringBoot项目启动报错-Slf4j日志相关类找不到

天行健&#xff0c;君子以自强不息&#xff1b;地势坤&#xff0c;君子以厚德载物。 每个人都有惰性&#xff0c;但不断学习是好好生活的根本&#xff0c;共勉&#xff01; 文章均为学习整理笔记&#xff0c;分享记录为主&#xff0c;如有错误请指正&#xff0c;共同学习进步。…
阅读更多...
AI新动向：豆包文生图升级，文心一言领先市场

AI新动向：豆包文生图升级，文心一言领先市场

在今日的AI资讯中&#xff0c;我们关注到了几个重要的行业动态&#xff0c;其中包括字节跳动AI助手豆包的功能升级&#xff0c;以及百度文心一言在生成式AI市场的领先地位。 字节跳动旗下的智能AI助手豆包近期对其文生图能力进行了显著提升&#xff0c;用户现在可以通过一键操…
阅读更多...
企业网双核心交换机实现冗余和负载均衡（MSTP+VRRP）

企业网双核心交换机实现冗余和负载均衡（MSTP+VRRP）

MSTP&#xff08;多生成树协议&#xff09; 通过创建多个VLAN实例&#xff0c;将原有的STP、RSTP升级&#xff0c;避免单一VLAN阻塞后导致带宽的浪费&#xff0c;通过将VLAN数据与实例绑定&#xff0c;有效提升网络速率。 VRRP&#xff08;虚拟路由冗余协议&#xff09; 用…
阅读更多...
使用Edu教育邮箱免费使用JetBrains专业版

使用Edu教育邮箱免费使用JetBrains专业版

需要准备的原料&#xff1a; 1个Edu邮箱&#xff08;最好是公立大学&#xff09; / JetBrains账户 Edu邮箱是什么&#xff1f; Edu邮箱是由美国高校和教育机构发放的邮箱&#xff0c;通常以“edu”结尾。拥有这个邮箱&#xff0c;你不仅能享受校园内的各种福利&#xff0c;还能…
阅读更多...
️️耗时一周，肝了一个超丝滑的卡盒小程序

️️耗时一周，肝了一个超丝滑的卡盒小程序

前言 先看看成品效果&#xff1a; 在上个月&#xff0c;我出于提升自己的英语造句能力的目的&#xff0c;想要找一个阅读或者练习造句类的英语学习 APP&#xff0c;但是最终找了几个 APP 不是不太好用就是要付费。于是我转换思路&#xff0c;找到了一本书&#xff0c;叫《36…
阅读更多...
初学者微服务Nocos快速了解使用

初学者微服务Nocos快速了解使用

Windows安装部署nacos 1.Windows启动nacos服务 下载nacos安装包&#xff1a;下载地址&#xff08;需要梯子访问&#xff09;&#xff1a;https://github.com/alibaba/nacos/releases 2.解压安装包&#xff0c;不要将nacos放置在中文路径下 3.在bin目录下双击startup.cmd文件 4.…
阅读更多...
Qt Quick 开发基础 + 实战（持续更新中…）

Qt Quick 开发基础 + 实战（持续更新中…）

最近更新日期&#xff1a;2024/12/5 目录 一、Qt Quick简介 1.3 新建Qt Quick Application工程 1.3.1 导入Qt资源文件 1.3.2 设置应用图标&#xff08;Windows系统&#xff09; 二、QML 2.2 import 2.2.1 import模块 2.2.2 import代码文件 2.3 属性&#xff1a;proper…
阅读更多...
jmeter基础07_组件的层级

jmeter基础07_组件的层级

课程大纲 1. 优先级/执行顺序&#xff08;一般情况&#xff09; 同级组件&#xff1a;按组件先后顺序执行。如&#xff1a;同一层的线程组、同一层的http请求。 上下级组件&#xff1a;先执行外层&#xff08;上级&#xff09;&#xff0c;再执行内层&#xff08;下级&#xff…
阅读更多...
Spring Cloud gateway 路由规则

Spring Cloud gateway 路由规则

Spring Cloud gateway 路由规则 文章目录 Spring Cloud gateway 路由规则一、路由常用属性解析1.1 示例配置1.2 属性解析 二、问题分析&#xff0c;springCloud微服务中没有任何路由配置&#xff0c;网关为什么能根据请求转发到相应的业务服务的2.1 开启&#xff0c;用于启用通…
阅读更多...
一、理论基础-PSI

一、理论基础-PSI

之前参加了隐语第2期&#xff0c;对隐语SecretFlow框架有了大致的了解&#xff0c;这次参加隐语第4期&#xff0c;学习下PSI和PIR。 一、PSI定义 首先介绍PSI的定义&#xff0c;PSI&#xff08;隐私集合求交&#xff0c;Private Set Intersection即PSI)是安全多方计算&#x…
阅读更多...
ZLMediaKit+wvp (ffmpeg+obs)推拉流测试

ZLMediaKit+wvp (ffmpeg+obs)推拉流测试

这里使用了两种方式: ffmpeg命令和 OBS OBS推流在网上找了些基本没有说明白的, 在ZLMediaKit的issues中看到了一个好大哥的提问在此记录一下 使用OBS推流&#xff0c;rtmp&#xff0c;报鉴权失败 推流 1. ffmpeg命令推流 官方说明文档地址: 推流规则 rtsp://192.168.1.4:10554…
阅读更多...
K8S，StatefulSet

K8S，StatefulSet

有状态应用 Deployment实际上并不足以覆盖所有的应用编排问题&#xff1f; 分布式应用&#xff0c;它的多个实例之间&#xff0c;往往有依赖关系&#xff0c;比如&#xff1a;主从关系、主备关系。 还有就是数据存储类应用&#xff0c;它的多个实例&#xff0c;往往都会在本地…
阅读更多...
openEuler 知：安装系统

openEuler 知：安装系统

文章目录 前言图形化安装文本方式安装 前言 本文只介绍安装过程中需要特别注意的地方&#xff0c;常规的内容需要参考其它文档。 图形化安装 自定义分区&#xff1a; 说明&#xff1a;anaconda 默认分区&#xff0c;在 OSNAME.conf 中进行了配置&#xff0c;openEuler 默认根…
阅读更多...
打通Vue3+Flask（python3）+Mysql-实现简单数据交互

打通Vue3+Flask（python3）+Mysql-实现简单数据交互

一、需要准备的工具 下载python3&#xff0c;Vscode&#xff0c;pycharm&#xff08;这里用的社区版&#xff09;&#xff0c;phpstudy_pro&#xff0c;Node.js&#xff08;建议下载长期支持版本&#xff0c;版本不宜过低&#xff0c;比如18,20&#xff09;&#xff0c;Vue.js…
阅读更多...
SpringBoot的validation参数校验

SpringBoot的validation参数校验

文章目录 前言一、引入validation 依赖二、validation中的注解说明 &#xff08;1&#xff09;Validated&#xff08;2&#xff09;Valid&#xff08;3&#xff09;NotNull&#xff08;4&#xff09;NotBlank&#xff08;5&#xff09;NotEmpty&#xff08;6&#xff09;Patte…
阅读更多...
Webhook应用指南：借助mc工具实现智能自动化

Webhook应用指南：借助mc工具实现智能自动化

欢迎来到我的博客&#xff0c;代码的世界里&#xff0c;每一行都是一个故事 &#x1f38f;&#xff1a;你只管努力&#xff0c;剩下的交给时间 &#x1f3e0; &#xff1a;小破站 Webhook应用指南&#xff1a;借助mc工具实现智能自动化 前言Webhook 基础知识什么是 Webhook&…
阅读更多...
【全网最新】若依管理系统基于SpringBoot的前后端分离版本配置部署

【全网最新】若依管理系统基于SpringBoot的前后端分离版本配置部署

目录 提前准备&#xff1a; 下载源代码 设置依赖 设置后台连接信息 运行后台 运行前端 安装npm依赖 启动前端 登录网页客户端 提前准备&#xff1a; 1、安装mysql 5以上就可以。 2、安装redis. 3、安装npm npm下载地址&#xff1a;https://nodejs.org/dist/v22.12…
阅读更多...
最新文章

Copyright @ 2022~2023