Servo_STS3032
The STS3032 control code of the desktop wheel-legged robot is added to the servo torque acquisition (the output voltage percentage (0~1000), the output value is positive and negative, reflecting the torque direction)
Existing features:
Simultaneous Write Multiple Servo Position Control (Selectable ACC, SPEED Settings)void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]);
Obtain servo torque (percentage of voltage output to motor (0~1000)) int ReadLoad(int ID);
Servo_STS3032.h
C
//飞特STS3032 servo 同步控制指令
//调用方式为sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
#ifndef SERVOSTS3032_H
#define SERVOSTS3032_H
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
//数据结构定义
typedef char s8;
typedef unsigned char u8;
typedef unsigned short u16;
typedef short s16;
typedef unsigned long u32;
typedef long s32;
//内存表定义
#define SMS_STS_ACC 41
// 飞特串行舵机协议指令定义
#define INST_READ 0x02
#define INST_SYNC_WRITE 0x83
//feetech serier bus servo通信层协议程序
class SCS{
public:
SCS();
SCS(u8 End);
SCS(u8 End, u8 Level);
void syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen);//同步写指令
int Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen);//读指令
int readWord(u8 ID, u8 MemAddr);//读2个字节
public:
u8 Level;//舵机返回等级
u8 End;//处理器大小端结构
u8 Error;//舵机状态
u8 syncReadRxPacketIndex;
u8 syncReadRxPacketLen;
u8 *syncReadRxPacket;
u8 *syncReadRxBuff;
u16 syncReadRxBuffLen;
u16 syncReadRxBuffMax;
u32 syncTimeOut;
protected:
virtual int writeSCS(unsigned char *nDat, int nLen) = 0;
virtual int readSCS(unsigned char *nDat, int nLen) = 0;
virtual int writeSCS(unsigned char bDat) = 0;
virtual void rFlushSCS() = 0;
virtual void wFlushSCS() = 0;
protected:
void writeBuf(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen, u8 Fun);
void Host2SCS(u8 DataL, u8 DataH, u16 Data);//1个16位数拆分为2个8位数
u16 SCS2Host(u8 DataL, u8 DataH);//2个8位数组合为1个16位数
int Ack(u8 ID);//返回应答
int checkHead();//帧头检测
};
//飞特串行舵机硬件接口层程序
class SCSerial : public SCS
{
public:
SCSerial();
SCSerial(u8 End);
SCSerial(u8 End, u8 Level);
protected:
int writeSCS(unsigned char *nDat, int nLen);//输出nLen字节
int readSCS(unsigned char *nDat, int nLen);//输入nLen字节
int writeSCS(unsigned char bDat);//输出1字节
void rFlushSCS();//
void wFlushSCS();//
public:
unsigned long IOTimeOut;//输入输出超时
HardwareSerial *pSerial;//串口指针
int Err;
public:
virtual int getErr(){ return Err; }
};
//-------SRAM(只读)--------
#define SMS_STS_PRESENT_POSITION_L 56
#define SMS_STS_PRESENT_POSITION_H 57
#define SMS_STS_PRESENT_SPEED_L 58
#define SMS_STS_PRESENT_SPEED_H 59
#define SMS_STS_PRESENT_LOAD_L 60
#define SMS_STS_PRESENT_LOAD_H 61
#define SMS_STS_PRESENT_VOLTAGE 62
#define SMS_STS_PRESENT_TEMPERATURE 63
#define SMS_STS_MOVING 66
#define SMS_STS_PRESENT_CURRENT_L 69
#define SMS_STS_PRESENT_CURRENT_H 70
//飞特SMS/STS系列串行舵机应用层程序
class SMS_STS : public SCSerial
{
public:
virtual void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]);//同步写多个舵机位置指令
virtual int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
private:
u8 Mem[SMS_STS_PRESENT_CURRENT_H-SMS_STS_PRESENT_POSITION_L+1];
};
#endif
Servo_STS3032.cpp
C
#include "Servo_STS3032.h"
#include <stddef.h>
//飞特串行舵机通信层协议程序
SCS::SCS()
{
Level = 1;//除广播指令所有指令返回应答
Error = 0;
}
SCS::SCS(u8 End)
{
Level = 1;
this->End = End;
Error = 0;
}
SCS::SCS(u8 End, u8 Level)
{
this->Level = Level;
this->End = End;
Error = 0;
}
//1个16位数拆分为2个8位数
//DataL为低位，DataH为高位
void SCS::Host2SCS(u8 DataL, u8 DataH, u16 Data)
{
if(End){
*DataL = (Data>>8);
*DataH = (Data&0xff);
}else{
*DataH = (Data>>8);
*DataL = (Data&0xff);
}
}
//2个8位数组合为1个16位数
//DataL为低位，DataH为高位
u16 SCS::SCS2Host(u8 DataL, u8 DataH)
{
u16 Data;
if(End){
Data = DataL;
Data<<=8;
Data |= DataH;
}else{
Data = DataH;
Data<<=8;
Data |= DataL;
}
return Data;
}
void SCS::writeBuf(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen, u8 Fun)
{
u8 msgLen = 2;
u8 bBuf[6];
u8 CheckSum = 0;
bBuf[0] = 0xff;
bBuf[1] = 0xff;
bBuf[2] = ID;
bBuf[4] = Fun;
if(nDat){
msgLen += nLen + 1;
bBuf[3] = msgLen;
bBuf[5] = MemAddr;
writeSCS(bBuf, 6);
}else{
bBuf[3] = msgLen;
writeSCS(bBuf, 5);
}
CheckSum = ID + msgLen + Fun + MemAddr;
u8 i = 0;
if(nDat){
for(i=0; i<nLen; i++){
CheckSum += nDat[i];
}
writeSCS(nDat, nLen);
}
writeSCS(~CheckSum);
}
//同步写指令
//舵机ID[]数组，IDN数组长度，MemAddr内存表地址，写入数据，写入长度
void SCS::syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen)
{
rFlushSCS();
u8 mesLen = ((nLen+1)*IDN+4);
u8 Sum = 0;
u8 bBuf[7];
bBuf[0] = 0xff;
bBuf[1] = 0xff;
bBuf[2] = 0xfe;
bBuf[3] = mesLen;
bBuf[4] = INST_SYNC_WRITE;
bBuf[5] = MemAddr;
bBuf[6] = nLen;
writeSCS(bBuf, 7);
Sum = 0xfe + mesLen + INST_SYNC_WRITE + MemAddr + nLen;
u8 i, j;
for(i=0; i<IDN; i++){
writeSCS(ID[i]);
writeSCS(nDat+i*nLen, nLen);
Sum += ID[i];
for(j=0; j<nLen; j++){
Sum += nDat[i*nLen+j];
}
}
writeSCS(~Sum);
wFlushSCS();
}
//读指令
//舵机ID，MemAddr内存表地址，返回数据nData，数据长度nLen
int SCS::Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen)
{
rFlushSCS();
writeBuf(ID, MemAddr, &nLen, 1, INST_READ);
wFlushSCS();
if(!checkHead()){
return 0;
}
u8 bBuf[4];
Error = 0;
if(readSCS(bBuf, 3)!=3){
return 0;
}
if(bBuf[0]!=ID && ID!=0xfe){
return 0;
}
if(bBuf[1]!=(nLen+2)){
return 0;
}
int Size = readSCS(nData, nLen);
if(Size!=nLen){
return 0;
}
if(readSCS(bBuf+3, 1)!=1){
return 0;
}
u8 calSum = bBuf[0]+bBuf[1]+bBuf[2];
u8 i;
for(i=0; i<Size; i++){
calSum += nData[i];
}
calSum = ~calSum;
if(calSum!=bBuf[3]){
return 0;
}
Error = bBuf[2];
return Size;
}
//读2字节，超时返回-1
int SCS::readWord(u8 ID, u8 MemAddr)
{
u8 nDat[2];
int Size;
u16 wDat;
Size = Read(ID, MemAddr, nDat, 2);
if(Size!=2)
return -1;
wDat = SCS2Host(nDat[0], nDat[1]);
return wDat;
}
int SCS::checkHead()
{
u8 bDat;
u8 bBuf[] = {0, 0};
u8 Cnt = 0;
while(1){
if(!readSCS(&bDat, 1)){
return 0;
}
bBuf[1] = bBuf[0];
bBuf[0] = bDat;
if(bBuf[0]==0xff && bBuf[1]==0xff){
break;
}
Cnt++;
if(Cnt>10){
return 0;
}
}
return 1;
}
//飞特串行舵机硬件接口层程序
SCSerial::SCSerial()
{
IOTimeOut = 10;
pSerial = NULL;
}
SCSerial::SCSerial(u8 End):SCS(End)
{
IOTimeOut = 10;
pSerial = NULL;
}
SCSerial::SCSerial(u8 End, u8 Level):SCS(End, Level)
{
IOTimeOut = 10;
pSerial = NULL;
}
int SCSerial::readSCS(unsigned char *nDat, int nLen)
{
int Size = 0;
int ComData;
unsigned long t_begin = millis();
unsigned long t_user;
while(1){
ComData = pSerial->read();
if(ComData!=-1){
if(nDat){
nDat[Size] = ComData;
}
Size++;
t_begin = millis();
}
if(Size>=nLen){
break;
}
t_user = millis() - t_begin;
if(t_user>IOTimeOut){
break;
}
}
return Size;
}
int SCSerial::writeSCS(unsigned char *nDat, int nLen)
{
if(nDat==NULL){
return 0;
}
return pSerial->write(nDat, nLen);
}
int SCSerial::writeSCS(unsigned char bDat)
{
return pSerial->write(&bDat, 1);
}
void SCSerial::rFlushSCS()
{
while(pSerial->read()!=-1);
}
void SCSerial::wFlushSCS()
{
}
//飞特SMS_STS系列串行舵机应用层程序
void SMS_STS::SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[])
{
u8 offbuf[7*IDN];
for(u8 i = 0; i<IDN; i++){
if(Position[i]<0){
Position[i] = -Position[i];
Position[i] |= (1<<15);
}
u16 V;
if(Speed){
V = Speed[i];
}else{
V = 0;
}
if(ACC){
offbuf[i*7] = ACC[i];
}else{
offbuf[i*7] = 0;
}
Host2SCS(offbuf+i*7+1, offbuf+i*7+2, Position[i]);
Host2SCS(offbuf+i*7+3, offbuf+i*7+4, 0);
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, V);
}
syncWrite(ID, IDN, SMS_STS_ACC, offbuf, 7);
}
// 根据舵机ID获取舵机输出至电机的电压百分比(0~1000)
int SMS_STS::ReadLoad(int ID)
{
int Load = -1;
if(ID==-1){
Load = Mem[SMS_STS_PRESENT_LOAD_H-SMS_STS_PRESENT_POSITION_L];
Load <<= 8;
Load |= Mem[SMS_STS_PRESENT_LOAD_L-SMS_STS_PRESENT_POSITION_L];
}else{
Err = 0;
Load = readWord(ID, SMS_STS_PRESENT_LOAD_L);
if(Load==-1){
Err = 1;
}
}
if(!Err && (Load&(1<<10))){
Load = -(Load&~(1<<10));
}
return Load;
}
Servo-purchasing links are as follows:
Aliexpress Platform: https://vi.aliexpress.com/item/1005006135452739.html?gatewayAdapt=glo2vnm