网站怎样秒收录,苏州网站建设外包,首页网站备案号添加,那个视频网站可以做桌面背景温馨提示#xff1a;此部分内容需要较强的数学能力#xff0c;包括但不限于矩阵运算、坐标变换、数学几何。 一、数学知识
1.1 正逆运动学#xff08;几何法#xff09;
逆运动学解算函数
// 逆运动学--计算出三个角度
void inverse_caculate(double x, double y, … 温馨提示此部分内容需要较强的数学能力包括但不限于矩阵运算、坐标变换、数学几何。 一、数学知识
1.1 正逆运动学几何法
逆运动学解算函数
// 逆运动学--计算出三个角度
void inverse_caculate(double x, double y, double z)
{double L1 0.054; // 单位mdouble 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自动处理y0的情况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; // 单位mdouble 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自动处理y0的情况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 0theta2 0 54 pi/2theta3 0 61 00 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自动处理y0的情况
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.huint16_t batteryVolt; // 电压/****************************************************************************************************************舵机控制板实现六足机器人 舵机控制板实现六足机器人 舵机控制板实现六足机器人 舵机控制板实现六足机器人
****************************************************************************************************************/// 功能控制单个舵机转动
void moveServo(uint8_t servoID, uint16_t Position, uint16_t Time)
{HexapodTxBuf[0] HexapodTxBuf[1] FRAME_HEADER; // 填充帧头HexapodTxBuf[2] 8; // 数据长度要控制舵机数*35此处1*35HexapodTxBuf[3] CMD_SERVO_MOVE; // 填充舵机移动指令HexapodTxBuf[4] 1; // 要控制的舵机个数HexapodTxBuf[5] GET_LOW_BYTE(Time); // 取得时间的低八位1HexapodTxBuf[6] GET_HIGH_BYTE(Time); // 取得时间的高八位HexapodTxBuf[7] servoID; // 舵机IDHexapodTxBuf[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 5HexapodTxBuf[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); // 可参数中取得舵机IDHexapodTxBuf[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舵机IDPT舵机位置
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 5HexapodTxBuf[3] CMD_SERVO_MOVE; // 舵机移动指令HexapodTxBuf[4] Num; // 要控制舵机数HexapodTxBuf[5] GET_LOW_BYTE(Time); // 取得时间的低八位HexapodTxBuf[6] GET_HIGH_BYTE(Time); // 取得时间的高八位HexapodTxBuf[index] ID1; // 填充舵机IDHexapodTxBuf[index] GET_LOW_BYTE(PT1); // 填充目标位置低八位HexapodTxBuf[index] GET_HIGH_BYTE(PT1); // 填充目标位置高八位HexapodTxBuf[index] ID2; // 填充舵机IDHexapodTxBuf[index] GET_LOW_BYTE(PT2); // 填充目标位置低八位HexapodTxBuf[index] GET_HIGH_BYTE(PT2); // 填充目标位置高八位HexapodTxBuf[index] ID3; // 填充舵机IDHexapodTxBuf[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; // 数据长度数据帧除帧头部分数据字节数此命令固定为5HexapodTxBuf[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; // 数据长度数据帧除帧头部分数据字节数此命令固定为2HexapodTxBuf[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; // 数据长度数据帧除帧头部分数据字节数此命令固定为5HexapodTxBuf[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; // 数据长度数据帧除帧头部分数据字节数此命令固定为2HexapodTxBuf[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#endif2.2 控制机器人腿
ControlServo.c
#include ControlServo.h
#include math.h
#include string.h
#include usart.h
#include stdio.h
#include servo.h
#include arm_math.hMyaxis 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自动处理y0的情况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)temp0 —— 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; // 计算当前腿的舵机IDfor (int seg 0; seg 3; seg){int servo_ID base_index seg; // 根据关节位置计算舵机IDswitch (seg){case 0: // COXAServo_Set_Position(servo_ID, Hexapod_leg.Theta[0], Time);break;case 1: // FEMURServo_Set_Position(servo_ID, Hexapod_leg.Theta[1], Time);break;case 2: // TIBIAServo_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); // 控制一条腿运动#endif2.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, // MR56 * PI / 180, // FR(56 68) * PI / 180, // FLPI, // 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: // COXAServo_Set_Position(index, Hexapod_Init_Position.a, 200);break;case 1: // FEMURServo_Set_Position(index, Hexapod_Init_Position.b, 200);break;case 2: // TIBIAServo_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_LegForward_kinematics(0, -(PI / 4), 2 * PI / 3, 1); // 1号——B_LegForward_kinematics(0, -(PI / 4), 2 * PI / 3, 2); // 2号——C_LegForward_kinematics(0, -(PI / 4), 2 * PI / 3, 3); // 3号——D_LegForward_kinematics(0, -(PI / 4), 2 * PI / 3, 4); // 4号——E_LegForward_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_LegInverse_Kinematics(Pi3_axis[1].x, Pi3_axis[1].y, Pi3_axis[1].z, 1); // 1号——B_LegInverse_Kinematics(Pi3_axis[2].x, Pi3_axis[2].y, Pi3_axis[2].z, 2); // 2号——C_LegInverse_Kinematics(Pi3_axis[3].x, Pi3_axis[3].y, Pi3_axis[3].z, 3); // 3号——D_LegInverse_Kinematics(Pi3_axis[4].x, Pi3_axis[4].y, Pi3_axis[4].z, 4); // 4号——E_LegInverse_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_LegHexapod_center_Position03_Init(0.07600, 0, 0, 1); // 1号——B_LegHexapod_center_Position03_Init(0.07600, -0.08485, -56 * PI / 180, 2); // 2号——C_LegHexapod_center_Position03_Init(-0.07600, 0.08485, (56 68) * PI / 180, 3); // 3号——D_LegHexapod_center_Position03_Init(-0.07600, 0, PI, 4); // 4号——E_LegHexapod_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_LegInverse_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_LegInverse_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_LegInverse_Kinematics(Move_Position03.x SL_half_edge * 0.5, Move_Position03.y SL_half_edge, Move_Position03.z, 2); // 2号——C_LegInverse_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_LegInverse_Kinematics(Move_Position03.x - SL_half_edge * 0.5, Move_Position03.y - SL_half_edge, Move_Position03.z, 2); // 2号——C_LegInverse_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_LegInverse_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_LegInverse_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_LegInverse_Kinematics(Move_Position03.x SL_half_edge * 0.5, Move_Position03.y SL_half_edge, Move_Position03.z, 3); // 3号——D_LegInverse_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_LegInverse_Kinematics(Move_Position03.x - SL_half_edge * 0.5, Move_Position03.y - SL_half_edge, Move_Position03.z, 3); // 3号——D_LegInverse_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_LegInverse_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_LegInverse_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_LegInverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 4); // 4号——E_LegInverse_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_LegInverse_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_LegInverse_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_LegInverse_Kinematics(Move_Position03.x SL_half_edge, Move_Position03.y SL_half_edge * 0.5, Move_Position03.z, 3); // 3号——D_LegInverse_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_LegInverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 1); // 4号——E_LegInverse_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 文章转载自： http://www.morning.fynkt.cn.gov.cn.fynkt.cn http://www.morning.kmrgl.cn.gov.cn.kmrgl.cn http://www.morning.knmby.cn.gov.cn.knmby.cn http://www.morning.kqqk.cn.gov.cn.kqqk.cn http://www.morning.rczrq.cn.gov.cn.rczrq.cn http://www.morning.tbksk.cn.gov.cn.tbksk.cn http://www.morning.kmcby.cn.gov.cn.kmcby.cn http://www.morning.qztdz.cn.gov.cn.qztdz.cn http://www.morning.xbyyd.cn.gov.cn.xbyyd.cn http://www.morning.lsyk.cn.gov.cn.lsyk.cn http://www.morning.mtrz.cn.gov.cn.mtrz.cn http://www.morning.stbfy.cn.gov.cn.stbfy.cn http://www.morning.glkhx.cn.gov.cn.glkhx.cn http://www.morning.hnhkz.cn.gov.cn.hnhkz.cn http://www.morning.frmmp.cn.gov.cn.frmmp.cn http://www.morning.rmxk.cn.gov.cn.rmxk.cn http://www.morning.jsmyw.cn.gov.cn.jsmyw.cn http://www.morning.knjj.cn.gov.cn.knjj.cn http://www.morning.hjwkq.cn.gov.cn.hjwkq.cn http://www.morning.rrwgh.cn.gov.cn.rrwgh.cn http://www.morning.ljjmr.cn.gov.cn.ljjmr.cn http://www.morning.tqjwx.cn.gov.cn.tqjwx.cn http://www.morning.rpzth.cn.gov.cn.rpzth.cn http://www.morning.pcngq.cn.gov.cn.pcngq.cn http://www.morning.tymnr.cn.gov.cn.tymnr.cn http://www.morning.hxgly.cn.gov.cn.hxgly.cn http://www.morning.rwtlj.cn.gov.cn.rwtlj.cn http://www.morning.ynjhk.cn.gov.cn.ynjhk.cn http://www.morning.qqxmj.cn.gov.cn.qqxmj.cn http://www.morning.dnqpq.cn.gov.cn.dnqpq.cn http://www.morning.coffeedelsol.com.gov.cn.coffeedelsol.com http://www.morning.znqztgc.cn.gov.cn.znqztgc.cn http://www.morning.feites.com.gov.cn.feites.com http://www.morning.lhrcr.cn.gov.cn.lhrcr.cn http://www.morning.jntdf.cn.gov.cn.jntdf.cn http://www.morning.flxqm.cn.gov.cn.flxqm.cn http://www.morning.npfrj.cn.gov.cn.npfrj.cn http://www.morning.jfnlj.cn.gov.cn.jfnlj.cn http://www.morning.wjplr.cn.gov.cn.wjplr.cn http://www.morning.xxwl1.com.gov.cn.xxwl1.com http://www.morning.hjjhjhj.com.gov.cn.hjjhjhj.com http://www.morning.xtyyg.cn.gov.cn.xtyyg.cn http://www.morning.a3e2r.com.gov.cn.a3e2r.com http://www.morning.xtyyg.cn.gov.cn.xtyyg.cn http://www.morning.qtfss.cn.gov.cn.qtfss.cn http://www.morning.yxgqr.cn.gov.cn.yxgqr.cn http://www.morning.pqnpd.cn.gov.cn.pqnpd.cn http://www.morning.cwrnr.cn.gov.cn.cwrnr.cn http://www.morning.rgpbk.cn.gov.cn.rgpbk.cn http://www.morning.qrsrs.cn.gov.cn.qrsrs.cn http://www.morning.kzxlc.cn.gov.cn.kzxlc.cn http://www.morning.wctqc.cn.gov.cn.wctqc.cn http://www.morning.cwjsz.cn.gov.cn.cwjsz.cn http://www.morning.kstlm.cn.gov.cn.kstlm.cn http://www.morning.fbzdn.cn.gov.cn.fbzdn.cn http://www.morning.xpmhs.cn.gov.cn.xpmhs.cn http://www.morning.kxrld.cn.gov.cn.kxrld.cn http://www.morning.rrwft.cn.gov.cn.rrwft.cn http://www.morning.bpmmq.cn.gov.cn.bpmmq.cn http://www.morning.kzcfr.cn.gov.cn.kzcfr.cn http://www.morning.qxgmp.cn.gov.cn.qxgmp.cn http://www.morning.kndst.cn.gov.cn.kndst.cn http://www.morning.wbqk.cn.gov.cn.wbqk.cn http://www.morning.srkqs.cn.gov.cn.srkqs.cn http://www.morning.dgsx.cn.gov.cn.dgsx.cn http://www.morning.ktnmg.cn.gov.cn.ktnmg.cn http://www.morning.fkcjs.cn.gov.cn.fkcjs.cn http://www.morning.hlshn.cn.gov.cn.hlshn.cn http://www.morning.ppbrq.cn.gov.cn.ppbrq.cn http://www.morning.xrwsg.cn.gov.cn.xrwsg.cn http://www.morning.hmlpn.cn.gov.cn.hmlpn.cn http://www.morning.xqjrg.cn.gov.cn.xqjrg.cn http://www.morning.knryp.cn.gov.cn.knryp.cn http://www.morning.qgfkn.cn.gov.cn.qgfkn.cn http://www.morning.zzgkk.cn.gov.cn.zzgkk.cn http://www.morning.bhxzx.cn.gov.cn.bhxzx.cn http://www.morning.tpxgm.cn.gov.cn.tpxgm.cn http://www.morning.ymjgx.cn.gov.cn.ymjgx.cn http://www.morning.xnfg.cn.gov.cn.xnfg.cn http://www.morning.wynqg.cn.gov.cn.wynqg.cn
