飞特总线舵机STS3215位置模式控制代码

钻石独角兽

已于 2024-10-17 19:57:56 修改

阅读量845

点赞数 27

文章标签： stm32 c语言单片机物联网

于 2024-07-12 21:40:48 首次发布

本文链接：https://blog.csdn.net/2301_79800911/article/details/140389075

版权

总线舵机STS3215的几个控制函数：

增加了一个开关函数，因为后面两个函数无法让正在转的舵机停下来

/*
**********************************************************************
*函数名称：void outputKey(uint8_t id, uint8_t KeyFlag))
*函数功能：飞特STS3215舵机扭矩输出开关
*使用说明：1为开，0为关
*入口参数：id:舵机ID, KeyFlag:扭矩开关
*返回值：无
**********************************************************************

void output0(uint8_t id, int KeyFlag)
{
   uint8_t parameters[3];
   parameters[0] = 0x28;
   parameters[1] = (uint8_t)KeyFlag;
   parameters[2] = (uint8_t)0x00 ;

   uint8_t length = 0x05;       // 指令包长度
uint8_t instruction = 0x03;       // 指令类型:写入字符
   uint8_t checksum = ~(id + length + instruction + parameters[0] + parameters[1] + parameters[2]) & 0xFF;

   uint8_t packet[9];
   int idx = 0;
packet[idx++] = 0xFF; // 头部第一个字节
packet[idx++] = 0xFF; // 头部第二个字节
packet[idx++] = id; // ID
packet[idx++] = length; // 长度
packet[idx++] = instruction; // 指令
   for (int i = 0; i < 3; i++) {
packet[idx++] = parameters[i];
}
   packet[idx++] = checksum;
// HAL_UART_Transmit(&huart6, packet, sizeof(packet), 100);
}

/*
**********************************************************************
*函数名称：void output(uint8_t id, int input)
*函数功能：控制STS3215舵机趋向转动
*
*使用说明：无
*入口参数：id:舵机ID，input: 实际位置与目标位置差值
*返回值：无
**********************************************************************
*/
void output(uint8_t id, int input)       //input为实际位置与目标位置差值
{
const int16_t MAX_SPEED = 3400; // 最大速度常量
const int16_t MIN_SPEED = 1; // 最小速度常量

outputKey(1, 1);               //打开扭矩输出

if (input == 0) return; //差值为0时返回

uint16_t position = (input > 0) ? 4087 : 1;       // 根据输入的正负确定方向

int16_t speed = (int16_t)(input > 0 ? input : -input);       // 将输入的绝对值赋值给speed
if (speed > MAX_SPEED) {
speed = MAX_SPEED;
} else if (speed < MIN_SPEED) {
speed = MIN_SPEED;
}

uint8_t header[] = {0xFF, 0xFF};       // 定义指令包的头部

uint8_t length = 0x09;       // 指令包长度

uint8_t instruction = 0x03;       // 指令类型

uint8_t address = 0x2A;       // 地址

uint16_t time = 0x0000;       // 时间数据固定为0，不考虑时间控制

// 初步组合指令包
uint8_t parameters[7];
parameters[0] = address; // 地址参数
parameters[1] = (uint8_t)(position & 0xFF); // 位置低8位
parameters[2] = (uint8_t)((position >> 8) & 0xFF); // 位置高8位
parameters[3] = (uint8_t)(time & 0xFF); // 时间低8位
parameters[4] = (uint8_t)((time >> 8) & 0xFF); // 时间高8位
parameters[5] = (uint8_t)(speed & 0xFF); // 速度低8位
parameters[6] = (uint8_t)((speed >> 8) & 0xFF); // 速度高8位

// 计算校验和，使用所有数据的和取反后取低8位
uint8_t checksum = ~(id + length + instruction +
parameters[0] + parameters[1] + parameters[2] +
parameters[3] + parameters[4] + parameters[5] +
parameters[6]) & 0xFF;

// 组合完整指令包
uint8_t packet[13];
int idx = 0;
packet[idx++] = header[0]; // 帧头第一个字节
packet[idx++] = header[1]; // 帧头第二个字节
packet[idx++] = id; // ID
packet[idx++] = length; // 长度
packet[idx++] = instruction; // 指令

// 将参数添加到指令包中
for (int i = 0; i < sizeof(parameters); i++) {
packet[idx++] = parameters[i];
}

packet[idx++] = checksum; // 添加校验和到指令包

// 发送指令包的代码
// for (int i = 0; i < sizeof(packet); i++) printf("%d", packet[i]);
// HAL_UART_Transmit(&huart3, packet, sizeof(packet), 100);
// HAL_UART_Transmit(&huart3, tail, 4, 100);
}

/*
**********************************************************************
*函数名称：void outputPosition(uint8_t id, int input)
*函数功能：控制STS3215舵机按照指定角度转动
*
*使用说明：注意舵机的0°位置
*入口参数：id:舵机ID，input: 目标角度
*返回值：无
**********************************************************************
*/

void outputPosition(uint8_t id, int input) //input为目标角度
{
const int16_t MAX_SPEED = 3400;
const int16_t MIN_SPEED = 1;
const int16_t MAX_position = 4087;
const int16_t MIN_position = 1;

outputKey(1, 1); //打开扭矩输出

   input = 4087 * input / 360;            //转化成位置 1-4087
   uint16_t position = 1;
   position = (input > MAX_position ? MAX_position : input);
   position = (input < MIN_position ? MIN_position : input);


int16_t speed = (int16_t) (input > 0 ? input : -input);
if (speed > MAX_SPEED) {
speed = MAX_SPEED;
} else if (speed < MIN_SPEED) {
speed = MIN_SPEED;
}


   // 指令包要素
uint8_t header[] = {0xFF, 0xFF};
uint8_t length = 0x09;
uint8_t instruction = 0x03;
uint8_t address = 0x2A;
uint16_t time = 0x0000; //时间数据固定为0，不考虑时间控制

   //初步组合指令包
uint8_t parameters[7];
parameters[0] = address;
parameters[1] = (uint8_t)(position & 0xFF);
parameters[2] = (uint8_t)((position >> 8) & 0xFF);
parameters[3] = (uint8_t)(time & 0xFF);
parameters[4] = (uint8_t)((time >> 8) & 0xFF);
parameters[5] = (uint8_t)(speed & 0xFF);
parameters[6] = (uint8_t)((speed >> 8) & 0xFF);

// 计算校验和
uint8_t checksum = ~(id + length + instruction +
parameters[0] + parameters[1] + parameters[2] +
parameters[3] + parameters[4] + parameters[5] +
parameters[6]) & 0xFF;

   //组合完整指令包
uint8_t packet[13];
int idx = 0;
packet[idx++] = header[0];
packet[idx++] = header[1];
packet[idx++] = id;
packet[idx++] = length;
packet[idx++] = instruction;

for (int i = 0; i < sizeof(parameters); i++) packet[idx++] = parameters[i];

packet[idx++] = checksum;

// 发送指令包
//   for(int i = 0; i < sizeof(packet); i++) printf("%d",packet[i]);
//   HAL_UART_Transmit(&huart3, packet, sizeof(packet), 100);
//   HAL_UART_Transmit(&huart3, tail, 4,100);
}