Keuronde/Feetech_PlateformIO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ping et lecture des registres lorsqu'un servo avec l'ID 1 est trouvé

Browse Source
This commit is contained in:
Samuel 2025-08-10 16:32:27 +02:00
commit fb94f7981b
16 changed files with 1824 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored Normal file
View File

@ -0,0 +1,5 @@
.pio
.vscode/.browse.c_cpp.db*
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/ipch

0
Readme.md Normal file
View File

7
platformio.ini Normal file
View File

@ -0,0 +1,7 @@
[env]
platform = raspberrypi
framework = arduino
[env:pico]
board = pico

260
src/HLSCL.cpp Normal file
View File

@ -0,0 +1,260 @@
/*
* HLSCL.cpp
* 飞特HTS/HLS系列串行舵机应用层程序
* 日期: 2024.11.21
* 作者: txl
*/
#include "HLSCL.h"
HLSCL::HLSCL()
{
End = 0;
}
HLSCL::HLSCL(u8 End):SCSerial(End)
{
}
HLSCL::HLSCL(u8 End, u8 Level):SCSerial(End, Level)
{
}
int HLSCL::WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC, u16 Torque)
{
if(Position<0){
Position = -Position;
Position |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, Position);
Host2SCS(bBuf+3, bBuf+4, Torque);
Host2SCS(bBuf+5, bBuf+6, Speed);
return genWrite(ID, HLSCL_ACC, bBuf, 7);
}
int HLSCL::RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC, u16 Torque)
{
if(Position<0){
Position = -Position;
Position |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, Position);
Host2SCS(bBuf+3, bBuf+4, Torque);
Host2SCS(bBuf+5, bBuf+6, Speed);
return regWrite(ID, HLSCL_ACC, bBuf, 7);
}
void HLSCL::SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[], u16 Torque[])
{
u8 offbuf[7*IDN];
for(u8 i = 0; i<IDN; i++){
if(Position[i]<0){
Position[i] = -Position[i];
Position[i] |= (1<<15);
}
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, Torque[i]);
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, Speed[i]);
}
syncWrite(ID, IDN, HLSCL_ACC, offbuf, 7);
}
void HLSCL::SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[], u16 Torque[])
{
u8 offbuf[7*IDN];
for(u8 i = 0; i<IDN; i++){
if(Speed[i]<0){
Speed[i] = -Speed[i];
Speed[i] |= (1<<15);
}
if(ACC){
offbuf[i*7] = ACC[i];
}else{
offbuf[i*7] = 0;
}
Host2SCS(offbuf+i*7+1, offbuf+i*7+2, 0);
Host2SCS(offbuf+i*7+3, offbuf+i*7+4, Torque[i]);
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, Speed[i]);
}
syncWrite(ID, IDN, HLSCL_ACC, offbuf, 7);
}
int HLSCL::WheelMode(u8 ID)
{
return writeByte(ID, HLSCL_MODE, 1);
}
int HLSCL::EleMode(u8 ID)
{
return writeByte(ID, HLSCL_MODE, 2);
}
int HLSCL::WriteSpe(u8 ID, s16 Speed, u8 ACC, u16 Torque)
{
if(Speed<0){
Speed = -Speed;
Speed |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, 0);
Host2SCS(bBuf+3, bBuf+4, Torque);
Host2SCS(bBuf+5, bBuf+6, Speed);
return genWrite(ID, HLSCL_ACC, bBuf, 7);
}
int HLSCL::WriteEle(u8 ID, s16 Torque)
{
if(Torque<0){
Torque = -Torque;
Torque |= (1<<15);
}
return writeWord(ID, HLSCL_GOAL_TORQUE_L, Torque);
}
int HLSCL::EnableTorque(u8 ID, u8 Enable)
{
return writeByte(ID, HLSCL_TORQUE_ENABLE, Enable);
}
int HLSCL::unLockEprom(u8 ID)
{
EnableTorque(ID, 0);
return writeByte(ID, HLSCL_LOCK, 0);
}
int HLSCL::LockEprom(u8 ID)
{
return writeByte(ID, HLSCL_LOCK, 1);
}
int HLSCL::CalibrationOfs(u8 ID)
{
EnableTorque(ID, 0);
unLockEprom(ID);
return Recal(ID);
}
int HLSCL::FeedBack(int ID)
{
int nLen = Read(ID, HLSCL_PRESENT_POSITION_L, Mem, sizeof(Mem));
if(nLen!=sizeof(Mem)){
return -1;
}
return nLen;
}
int HLSCL::ReadPos(int ID)
{
int Pos = -1;
if(ID==-1){
Pos = Mem[HLSCL_PRESENT_POSITION_H-HLSCL_PRESENT_POSITION_L];
Pos <<= 8;
Pos |= Mem[HLSCL_PRESENT_POSITION_L-HLSCL_PRESENT_POSITION_L];
}else{
Pos = readWord(ID, HLSCL_PRESENT_POSITION_L);
}
if(Pos&(1<<15)){
Pos = -(Pos&~(1<<15));
}
return Pos;
}
int HLSCL::ReadSpeed(int ID)
{
int Speed = -1;
if(ID==-1){
Speed = Mem[HLSCL_PRESENT_SPEED_H-HLSCL_PRESENT_POSITION_L];
Speed <<= 8;
Speed |= Mem[HLSCL_PRESENT_SPEED_L-HLSCL_PRESENT_POSITION_L];
}else{
Speed = readWord(ID, HLSCL_PRESENT_SPEED_L);
}
if(Speed&(1<<15)){
Speed = -(Speed&~(1<<15));
}
return Speed;
}
int HLSCL::ReadLoad(int ID)
{
int Load = -1;
if(ID==-1){
Load = Mem[HLSCL_PRESENT_LOAD_H-HLSCL_PRESENT_POSITION_L];
Load <<= 8;
Load |= Mem[HLSCL_PRESENT_LOAD_L-HLSCL_PRESENT_POSITION_L];
}else{
Load = readWord(ID, HLSCL_PRESENT_LOAD_L);
}
if(Load&(1<<10)){
Load = -(Load&~(1<<10));
}
return Load;
}
int HLSCL::ReadVoltage(int ID)
{
int Voltage = -1;
if(ID==-1){
Voltage = Mem[HLSCL_PRESENT_VOLTAGE-HLSCL_PRESENT_POSITION_L];
}else{
Voltage = readByte(ID, HLSCL_PRESENT_VOLTAGE);
}
return Voltage;
}
int HLSCL::ReadTemper(int ID)
{
int Temper = -1;
if(ID==-1){
Temper = Mem[HLSCL_PRESENT_TEMPERATURE-HLSCL_PRESENT_POSITION_L];
}else{
Temper = readByte(ID, HLSCL_PRESENT_TEMPERATURE);
}
return Temper;
}
int HLSCL::ReadMove(int ID)
{
int Move = -1;
if(ID==-1){
Move = Mem[HLSCL_MOVING-HLSCL_PRESENT_POSITION_L];
}else{
Move = readByte(ID, HLSCL_MOVING);
}
return Move;
}
int HLSCL::ReadCurrent(int ID)
{
int Current = -1;
if(ID==-1){
Current = Mem[HLSCL_PRESENT_CURRENT_H-HLSCL_PRESENT_POSITION_L];
Current <<= 8;
Current |= Mem[HLSCL_PRESENT_CURRENT_L-HLSCL_PRESENT_POSITION_L];
}else{
Current = readWord(ID, HLSCL_PRESENT_CURRENT_L);
}
if(Current&(1<<15)){
Current = -(Current&~(1<<15));
}
return Current;
}
int HLSCL::ServoMode(u8 ID)
{
return writeByte(ID, HLSCL_MODE, 0);
}

89
src/HLSCL.h Normal file
View File

@ -0,0 +1,89 @@
/*
* HLSCL.h
* 飞特HLS系列串行舵机应用层程序
* 日期: 2024.11.21
* 作者: txl
*/
#ifndef _HLSCL_H
#define _HLSCL_H
//内存表定义
//-------EPROM(只读)--------
#define HLSCL_MODEL_L 3
#define HLSCL_MODEL_H 4
//-------EPROM(读写)--------
#define HLSCL_ID 5
#define HLSCL_BAUD_RATE 6
#define HLSCL_SECOND_ID 7
#define HLSCL_MIN_ANGLE_LIMIT_L 9
#define HLSCL_MIN_ANGLE_LIMIT_H 10
#define HLSCL_MAX_ANGLE_LIMIT_L 11
#define HLSCL_MAX_ANGLE_LIMIT_H 12
#define HLSCL_CW_DEAD 26
#define HLSCL_CCW_DEAD 27
#define HLSCL_OFS_L 31
#define HLSCL_OFS_H 32
#define HLSCL_MODE 33
//-------SRAM(读写)--------
#define HLSCL_TORQUE_ENABLE 40
#define HLSCL_ACC 41
#define HLSCL_GOAL_POSITION_L 42
#define HLSCL_GOAL_POSITION_H 43
#define HLSCL_GOAL_TORQUE_L 44
#define HLSCL_GOAL_TORQUE_H 45
#define HLSCL_GOAL_SPEED_L 46
#define HLSCL_GOAL_SPEED_H 47
#define HLSCL_TORQUE_LIMIT_L 48
#define HLSCL_TORQUE_LIMIT_H 49
#define HLSCL_LOCK 55
//-------SRAM(只读)--------
#define HLSCL_PRESENT_POSITION_L 56
#define HLSCL_PRESENT_POSITION_H 57
#define HLSCL_PRESENT_SPEED_L 58
#define HLSCL_PRESENT_SPEED_H 59
#define HLSCL_PRESENT_LOAD_L 60
#define HLSCL_PRESENT_LOAD_H 61
#define HLSCL_PRESENT_VOLTAGE 62
#define HLSCL_PRESENT_TEMPERATURE 63
#define HLSCL_MOVING 66
#define HLSCL_PRESENT_CURRENT_L 69
#define HLSCL_PRESENT_CURRENT_H 70
#include "SCSerial.h"
class HLSCL : public SCSerial
{
public:
HLSCL();
HLSCL(u8 End);
HLSCL(u8 End, u8 Level);
int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0, u16 Torque = 0);//普通写单个舵机位置指令
int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0, u16 Torque = 0);//异步写单个舵机位置指令(RegWriteAction生效)
void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[], u16 Torque[]);//同步写多个舵机位置指令
void SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[], u16 Torque[]);//同步写多个舵机速度指令
int ServoMode(u8 ID);//Servo模式
int WheelMode(u8 ID);//恒速模式
int EleMode(u8 ID);//恒力模式
int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0, u16 Torque = 0);//恒速模式控制指令
int WriteEle(u8 ID, s16 Torque);//恒力模式控制指令
int EnableTorque(u8 ID, u8 Enable);//扭力控制指令
int unLockEprom(u8 ID);//eprom解锁
int LockEprom(u8 ID);//eprom加锁
int CalibrationOfs(u8 ID);//中位校准
int FeedBack(int ID);//反馈舵机信息
int ReadPos(int ID);//读位置
int ReadSpeed(int ID);//读速度
int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
int ReadVoltage(int ID);//读电压
int ReadTemper(int ID);//读温度
int ReadMove(int ID);//读移动状态
int ReadCurrent(int ID);//读电流
private:
u8 Mem[HLSCL_PRESENT_CURRENT_H-HLSCL_PRESENT_POSITION_L+1];
};
#endif

51
src/INST.h Normal file
View File

@ -0,0 +1,51 @@
/*
* INST.h
* 飞特串行舵机协议指令定义
* 日期: 2024.11.24
* 作者: txl
*/
#ifndef _INST_H
#define _INST_H
typedef char s8;
typedef unsigned char u8;
typedef unsigned short u16;
typedef short s16;
typedef unsigned long u32;
typedef long s32;
enum SCS_ERR_LIST
{
ERR_NO_REPLY = 1,
ERR_CRC_CMP = 2,
ERR_SLAVE_ID = 3,
ERR_BUFF_LEN = 4,
};
#define INST_PING 0x01
#define INST_READ 0x02
#define INST_WRITE 0x03
#define INST_REG_WRITE 0x04
#define INST_REG_ACTION 0x05
#define INST_SYNC_READ 0x82
#define INST_SYNC_WRITE 0x83
#define INST_RECOVERY 0x06
#define INST_RESET 0x0A
#define INST_CAL 0x0B
//波特率定义
#define _1M 0
#define _0_5M 1
#define _250K 2
#define _128K 3
#define _115200 4
#define _76800 5
#define _57600 6
#define _38400 7
#define _19200 8
#define _14400 9
#define _9600 10
#define _4800 11
#endif

450
src/SCS.cpp Normal file
View File

@ -0,0 +1,450 @@
/*
* SCS.cpp
* 飞特串行舵机通信层协议程序
* 日期: 2024.12.21
* 作者: txl
*/
#include <stddef.h>
#include "SCS.h"
SCS::SCS()
{
Level = 1;//除广播指令所有指令返回应答
u8Status = 0;
}
SCS::SCS(u8 End)
{
Level = 1;
this->End = End;
u8Status = 0;
}
SCS::SCS(u8 End, u8 Level)
{
this->Level = Level;
this->End = End;
u8Status = 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，MemAddr内存表地址，写入数据，写入长度
int SCS::genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
{
rFlushSCS();
writeBuf(ID, MemAddr, nDat, nLen, INST_WRITE);
wFlushSCS();
return Ack(ID);
}
//异步写指令
//舵机ID，MemAddr内存表地址，写入数据，写入长度
int SCS::regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
{
rFlushSCS();
writeBuf(ID, MemAddr, nDat, nLen, INST_REG_WRITE);
wFlushSCS();
return Ack(ID);
}
//异步写执行指令
//舵机ID
int SCS::RegWriteAction(u8 ID)
{
rFlushSCS();
writeBuf(ID, 0, NULL, 0, INST_REG_ACTION);
wFlushSCS();
return Ack(ID);
}
//同步写指令
//舵机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();
}
int SCS::writeByte(u8 ID, u8 MemAddr, u8 bDat)
{
rFlushSCS();
writeBuf(ID, MemAddr, &bDat, 1, INST_WRITE);
wFlushSCS();
return Ack(ID);
}
int SCS::writeWord(u8 ID, u8 MemAddr, u16 wDat)
{
u8 bBuf[2];
Host2SCS(bBuf+0, bBuf+1, wDat);
rFlushSCS();
writeBuf(ID, MemAddr, bBuf, 2, INST_WRITE);
wFlushSCS();
return Ack(ID);
}
//读指令
//舵机ID，MemAddr内存表地址，返回数据nData，数据长度nLen
int SCS::Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen)
{
rFlushSCS();
writeBuf(ID, MemAddr, &nLen, 1, INST_READ);
wFlushSCS();
u8Error = 0;
if(!checkHead()){
u8Error = ERR_NO_REPLY;
return 0;
}
u8 bBuf[4];
u8Status = 0;
if(readSCS(bBuf, 3)!=3){
u8Error = ERR_NO_REPLY;
return 0;
}
if(bBuf[0]!=ID && ID!=0xfe){
u8Error = ERR_SLAVE_ID;
return 0;
}
if(bBuf[1]!=(nLen+2)){
u8Error = ERR_BUFF_LEN;
return 0;
}
int Size = readSCS(nData, nLen);
if(Size!=nLen){
u8Error = ERR_NO_REPLY;
return 0;
}
if(readSCS(bBuf+3, 1)!=1){
u8Error = ERR_NO_REPLY;
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]){
u8Error = ERR_CRC_CMP;
return 0;
}
u8Status = bBuf[2];
return Size;
}
//读1字节，超时返回-1
int SCS::readByte(u8 ID, u8 MemAddr)
{
u8 bDat;
int Size = Read(ID, MemAddr, &bDat, 1);
if(Size!=1){
return -1;
}else{
return bDat;
}
}
//读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;
}
//Ping指令，返回舵机ID，超时返回-1
int SCS::Ping(u8 ID)
{
rFlushSCS();
writeBuf(ID, 0, NULL, 0, INST_PING);
wFlushSCS();
u8Status = 0;
if(!checkHead()){
u8Error = ERR_NO_REPLY;
return -1;
}
u8 bBuf[4];
u8Error = 0;
if(readSCS(bBuf, 4)!=4){
u8Error = ERR_NO_REPLY;
return -1;
}
if(bBuf[0]!=ID && ID!=0xfe){
u8Error = ERR_SLAVE_ID;
return -1;
}
if(bBuf[1]!=2){
u8Error = ERR_BUFF_LEN;
return -1;
}
u8 calSum = ~(bBuf[0]+bBuf[1]+bBuf[2]);
if(calSum!=bBuf[3]){
u8Error = ERR_CRC_CMP;
return -1;
}
u8Status = bBuf[2];
return bBuf[0];
}
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;
}
int SCS::Ack(u8 ID)
{
u8Error = 0;
if(ID!=0xfe && Level){
if(!checkHead()){
u8Error = ERR_NO_REPLY;
return 0;
}
u8Status = 0;
u8 bBuf[4];
if(readSCS(bBuf, 4)!=4){
u8Error = ERR_NO_REPLY;
return 0;
}
if(bBuf[0]!=ID){
u8Error = ERR_SLAVE_ID;
return 0;
}
if(bBuf[1]!=2){
u8Error = ERR_BUFF_LEN;
return 0;
}
u8 calSum = ~(bBuf[0]+bBuf[1]+bBuf[2]);
if(calSum!=bBuf[3]){
u8Error = ERR_CRC_CMP;
return 0;
}
u8Status = bBuf[2];
}
return 1;
}
int SCS::syncReadPacketTx(u8 ID[], u8 IDN, u8 MemAddr, u8 nLen)
{
rFlushSCS();
syncReadRxPacketLen = nLen;
u8 checkSum = (4+0xfe) + IDN + MemAddr + nLen + INST_SYNC_READ;
u8 i;
writeSCS(0xff);
writeSCS(0xff);
writeSCS(0xfe);
writeSCS(IDN+4);
writeSCS(INST_SYNC_READ);
writeSCS(MemAddr);
writeSCS(nLen);
for(i=0; i<IDN; i++){
writeSCS(ID[i]);
checkSum += ID[i];
}
checkSum = ~checkSum;
writeSCS(checkSum);
wFlushSCS();
syncReadRxBuffLen = readSCS(syncReadRxBuff, syncReadRxBuffMax, syncTimeOut);
return syncReadRxBuffLen;
}
void SCS::syncReadBegin(u8 IDN, u8 rxLen, u32 TimeOut)
{
syncReadRxBuffMax = IDN*(rxLen+6);
syncReadRxBuff = new u8[syncReadRxBuffMax];
syncTimeOut = TimeOut;
}
void SCS::syncReadEnd()
{
if(syncReadRxBuff){
delete syncReadRxBuff;
syncReadRxBuff = NULL;
}
}
int SCS::syncReadPacketRx(u8 ID, u8 *nDat)
{
u16 syncReadRxBuffIndex = 0;
syncReadRxPacket = nDat;
syncReadRxPacketIndex = 0;
u8Error = 0;
while((syncReadRxBuffIndex+6+syncReadRxPacketLen)<=syncReadRxBuffLen){
u8 bBuf[] = {0, 0, 0};
u8 calSum = 0;
while(syncReadRxBuffIndex<syncReadRxBuffLen){
bBuf[0] = bBuf[1];
bBuf[1] = bBuf[2];
bBuf[2] = syncReadRxBuff[syncReadRxBuffIndex++];
if(bBuf[0]==0xff && bBuf[1]==0xff && bBuf[2]!=0xff){
break;
}
}
if(bBuf[2]!=ID){
continue;
}
if(syncReadRxBuff[syncReadRxBuffIndex++]!=(syncReadRxPacketLen+2)){
continue;
}
u8Status = syncReadRxBuff[syncReadRxBuffIndex++];
calSum = ID + (syncReadRxPacketLen+2) + u8Status;
for(u8 i=0; i<syncReadRxPacketLen; i++){
syncReadRxPacket[i] = syncReadRxBuff[syncReadRxBuffIndex++];
calSum += syncReadRxPacket[i];
}
calSum = ~calSum;
if(calSum!=syncReadRxBuff[syncReadRxBuffIndex++]){
u8Error = ERR_CRC_CMP;
return 0;
}
return syncReadRxPacketLen;
}
return 0;
}
int SCS::syncReadRxPacketToByte()
{
if(syncReadRxPacketIndex>=syncReadRxPacketLen){
u8Error = ERR_BUFF_LEN;
return -1;
}
return syncReadRxPacket[syncReadRxPacketIndex++];
}
int SCS::syncReadRxPacketToWrod(u8 negBit)
{
if((syncReadRxPacketIndex+1)>=syncReadRxPacketLen){
u8Error = ERR_BUFF_LEN;
return -1;
}
int Word = SCS2Host(syncReadRxPacket[syncReadRxPacketIndex], syncReadRxPacket[syncReadRxPacketIndex+1]);
syncReadRxPacketIndex += 2;
if(negBit){
if(Word&(1<<negBit)){
Word = -(Word & ~(1<<negBit));
}
}
return Word;
}
int SCS::Reset(u8 ID)
{
rFlushSCS();
writeBuf(ID, 0, NULL, 0, INST_RESET);
wFlushSCS();
return Ack(ID);
}
int SCS::Recal(u8 ID)
{
rFlushSCS();
writeBuf(ID, 0, NULL, 0, INST_CAL);
wFlushSCS();
return Ack(ID);
}

64
src/SCS.h Normal file
View File

@ -0,0 +1,64 @@
/*
* SCS.h
* 飞特串行舵机通信层协议程序
* 日期: 2024.11.21
* 作者: txl
*/
#ifndef _SCS_H
#define _SCS_H
#include "INST.h"
class SCS{
public:
SCS();
SCS(u8 End);
SCS(u8 End, u8 Level);
int genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen);//普通写指令
int regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen);//异步写指令
int RegWriteAction(u8 ID = 0xfe);//异步写执行指令
void syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen);//同步写指令
int writeByte(u8 ID, u8 MemAddr, u8 bDat);//写1个字节
int writeWord(u8 ID, u8 MemAddr, u16 wDat);//写2个字节
int Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen);//读指令
int readByte(u8 ID, u8 MemAddr);//读1个字节
int readWord(u8 ID, u8 MemAddr);//读2个字节
int Ping(u8 ID);//Ping指令
int syncReadPacketTx(u8 ID[], u8 IDN, u8 MemAddr, u8 nLen);//同步读指令包发送
int syncReadPacketRx(u8 ID, u8 *nDat);//同步读返回包解码，成功返回内存字节数，失败返回0
int syncReadRxPacketToByte();//解码一个字节
int syncReadRxPacketToWrod(u8 negBit=0);//解码两个字节，negBit为方向为，negBit=0表示无方向
void syncReadBegin(u8 IDN, u8 rxLen, u32 TimeOut);//同步读开始
void syncReadEnd();//同步读结束
int Reset(u8 ID);//重置舵机状态
int Recal(u8 ID);//重置舵机中位
u8 getState() { return u8Status; }
u8 getLastError() { return u8Error; }
public:
u8 Level;//舵机返回等级
u8 End;//处理器大小端结构
u8 u8Status;//舵机状态
u8 u8Error;//通信状态
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 readSCS(unsigned char *nDat, int nLen, unsigned long TimeOut) = 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();//帧头检测
};
#endif

203
src/SCSCL.cpp Normal file
View File

@ -0,0 +1,203 @@
/*
* SCSCL.cpp
* 飞特SCSCL系列串行舵机应用层程序
* 日期: 2024.4.2
* 作者:
*/
#include "SCSCL.h"
SCSCL::SCSCL()
{
End = 1;
}
SCSCL::SCSCL(u8 End):SCSerial(End)
{
}
SCSCL::SCSCL(u8 End, u8 Level):SCSerial(End, Level)
{
}
int SCSCL::WritePos(u8 ID, u16 Position, u16 Time, u16 Speed)
{
u8 bBuf[6];
Host2SCS(bBuf+0, bBuf+1, Position);
Host2SCS(bBuf+2, bBuf+3, Time);
Host2SCS(bBuf+4, bBuf+5, Speed);
return genWrite(ID, SCSCL_GOAL_POSITION_L, bBuf, 6);
}
int SCSCL::RegWritePos(u8 ID, u16 Position, u16 Time, u16 Speed)
{
u8 bBuf[6];
Host2SCS(bBuf+0, bBuf+1, Position);
Host2SCS(bBuf+2, bBuf+3, Time);
Host2SCS(bBuf+4, bBuf+5, Speed);
return regWrite(ID, SCSCL_GOAL_POSITION_L, bBuf, 6);
}
void SCSCL::SyncWritePos(u8 ID[], u8 IDN, u16 Position[], u16 Time[], u16 Speed[])
{
u8 offbuf[6*IDN];
for(u8 i = 0; i<IDN; i++){
u16 T, V;
if(Time){
T = Time[i];
}else{
T = 0;
}
if(Speed){
V = Speed[i];
}else{
V = 0;
}
Host2SCS(offbuf+i*6+0, offbuf+i*6+1, Position[i]);
Host2SCS(offbuf+i*6+2, offbuf+i*6+3, T);
Host2SCS(offbuf+i*6+4, offbuf+i*6+5, V);
}
syncWrite(ID, IDN, SCSCL_GOAL_POSITION_L, offbuf, 6);
}
int SCSCL::EnableTorque(u8 ID, u8 Enable)
{
return writeByte(ID, SCSCL_TORQUE_ENABLE, Enable);
}
int SCSCL::unLockEprom(u8 ID)
{
return writeByte(ID, SCSCL_LOCK, 0);
}
int SCSCL::LockEprom(u8 ID)
{
return writeByte(ID, SCSCL_LOCK, 1);
}
int SCSCL::FeedBack(int ID)
{
int nLen = Read(ID, SCSCL_PRESENT_POSITION_L, Mem, sizeof(Mem));
if(nLen!=sizeof(Mem)){
return -1;
}
return nLen;
}
int SCSCL::ReadPos(int ID)
{
int Pos = -1;
if(ID==-1){
Pos = Mem[SCSCL_PRESENT_POSITION_L-SCSCL_PRESENT_POSITION_L];
Pos <<= 8;
Pos |= Mem[SCSCL_PRESENT_POSITION_H-SCSCL_PRESENT_POSITION_L];
}else{
Pos = readWord(ID, SCSCL_PRESENT_POSITION_L);
}
return Pos;
}
int SCSCL::ReadSpeed(int ID)
{
int Speed = -1;
if(ID==-1){
Speed = Mem[SCSCL_PRESENT_SPEED_L-SCSCL_PRESENT_POSITION_L];
Speed <<= 8;
Speed |= Mem[SCSCL_PRESENT_SPEED_H-SCSCL_PRESENT_POSITION_L];
}else{
Speed = readWord(ID, SCSCL_PRESENT_SPEED_L);
}
if(Speed&(1<<15)){
Speed = -(Speed&~(1<<15));
}
return Speed;
}
int SCSCL::ReadLoad(int ID)
{
int Load = -1;
if(ID==-1){
Load = Mem[SCSCL_PRESENT_LOAD_L-SCSCL_PRESENT_POSITION_L];
Load <<= 8;
Load |= Mem[SCSCL_PRESENT_LOAD_H-SCSCL_PRESENT_POSITION_L];
}else{
Load = readWord(ID, SCSCL_PRESENT_LOAD_L);
}
if(Load&(1<<10)){
Load = -(Load&~(1<<10));
}
return Load;
}
int SCSCL::ReadVoltage(int ID)
{
int Voltage = -1;
if(ID==-1){
Voltage = Mem[SCSCL_PRESENT_VOLTAGE-SCSCL_PRESENT_POSITION_L];
}else{
Voltage = readByte(ID, SCSCL_PRESENT_VOLTAGE);
}
return Voltage;
}
int SCSCL::ReadTemper(int ID)
{
int Temper = -1;
if(ID==-1){
Temper = Mem[SCSCL_PRESENT_TEMPERATURE-SCSCL_PRESENT_POSITION_L];
}else{
Temper = readByte(ID, SCSCL_PRESENT_TEMPERATURE);
}
return Temper;
}
int SCSCL::ReadMove(int ID)
{
int Move = -1;
if(ID==-1){
Move = Mem[SCSCL_MOVING-SCSCL_PRESENT_POSITION_L];
}else{
Move = readByte(ID, SCSCL_MOVING);
}
return Move;
}
int SCSCL::ReadCurrent(int ID)
{
int Current = -1;
if(ID==-1){
Current = Mem[SCSCL_PRESENT_CURRENT_L-SCSCL_PRESENT_POSITION_L];
Current <<= 8;
Current |= Mem[SCSCL_PRESENT_CURRENT_H-SCSCL_PRESENT_POSITION_L];
}else{
Current = readWord(ID, SCSCL_PRESENT_CURRENT_L);
}
if(Current&(1<<15)){
Current = -(Current&~(1<<15));
}
return Current;
}
int SCSCL::PWMMode(u8 ID)
{
u8 bBuf[4];
bBuf[0] = 0;
bBuf[1] = 0;
bBuf[2] = 0;
bBuf[3] = 0;
return genWrite(ID, SCSCL_MIN_ANGLE_LIMIT_L, bBuf, 4);
}
int SCSCL::WritePWM(u8 ID, s16 pwmOut)
{
if(pwmOut<0){
pwmOut = -pwmOut;
pwmOut |= (1<<10);
}
u8 bBuf[2];
Host2SCS(bBuf+0, bBuf+1, pwmOut);
return genWrite(ID, SCSCL_GOAL_TIME_L, bBuf, 2);
}

77
src/SCSCL.h Normal file
View File

@ -0,0 +1,77 @@
/*
* SCSCL.h
* 飞特SCSCL系列串行舵机应用层程序
* 日期: 2024.4.2
* 作者:
*/
#ifndef _SCSCL_H
#define _SCSCL_H
//内存表定义
//-------EPROM(只读)--------
#define SCSCL_VERSION_L 3
#define SCSCL_VERSION_H 4
//-------EPROM(读写)--------
#define SCSCL_ID 5
#define SCSCL_BAUD_RATE 6
#define SCSCL_MIN_ANGLE_LIMIT_L 9
#define SCSCL_MIN_ANGLE_LIMIT_H 10
#define SCSCL_MAX_ANGLE_LIMIT_L 11
#define SCSCL_MAX_ANGLE_LIMIT_H 12
#define SCSCL_CW_DEAD 26
#define SCSCL_CCW_DEAD 27
//-------SRAM(读写)--------
#define SCSCL_TORQUE_ENABLE 40
#define SCSCL_GOAL_POSITION_L 42
#define SCSCL_GOAL_POSITION_H 43
#define SCSCL_GOAL_TIME_L 44
#define SCSCL_GOAL_TIME_H 45
#define SCSCL_GOAL_SPEED_L 46
#define SCSCL_GOAL_SPEED_H 47
#define SCSCL_LOCK 48
//-------SRAM(只读)--------
#define SCSCL_PRESENT_POSITION_L 56
#define SCSCL_PRESENT_POSITION_H 57
#define SCSCL_PRESENT_SPEED_L 58
#define SCSCL_PRESENT_SPEED_H 59
#define SCSCL_PRESENT_LOAD_L 60
#define SCSCL_PRESENT_LOAD_H 61
#define SCSCL_PRESENT_VOLTAGE 62
#define SCSCL_PRESENT_TEMPERATURE 63
#define SCSCL_MOVING 66
#define SCSCL_PRESENT_CURRENT_L 69
#define SCSCL_PRESENT_CURRENT_H 70
#include "SCSerial.h"
class SCSCL : public SCSerial
{
public:
SCSCL();
SCSCL(u8 End);
SCSCL(u8 End, u8 Level);
int WritePos(u8 ID, u16 Position, u16 Time, u16 Speed = 0);//普通写单个舵机位置指令
int RegWritePos(u8 ID, u16 Position, u16 Time, u16 Speed = 0);//异步写单个舵机位置指令(RegWriteAction生效)
void SyncWritePos(u8 ID[], u8 IDN, u16 Position[], u16 Time[], u16 Speed[]);//同步写多个舵机位置指令
int PWMMode(u8 ID);//PWM模式
int WritePWM(u8 ID, s16 pwmOut);//PWM输出模式指令
int EnableTorque(u8 ID, u8 Enable);//扭矩控制指令
int unLockEprom(u8 ID);//eprom解锁
int LockEprom(u8 ID);//eprom加锁
int FeedBack(int ID);//反馈舵机信息
int ReadPos(int ID);//读位置
int ReadSpeed(int ID);//读速度
int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
int ReadVoltage(int ID);//读电压
int ReadTemper(int ID);//读温度
int ReadMove(int ID);//读移动状态
int ReadCurrent(int ID);//读电流
private:
u8 Mem[SCSCL_PRESENT_CURRENT_H-SCSCL_PRESENT_POSITION_L+1];
};
#endif

99
src/SCSerial.cpp Normal file
View File

@ -0,0 +1,99 @@
/*
* SCSerial.h
* 飞特串行舵机硬件接口层程序
* 日期: 2024.4.2
* 作者:
*/
#include "SCSerial.h"
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, unsigned long TimeOut)
{
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++;
}
if(Size>=nLen){
break;
}
t_user = millis() - t_begin;
if(t_user>TimeOut){
break;
}
}
return Size;
}
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()
{
}

38
src/SCSerial.h Normal file
View File

@ -0,0 +1,38 @@
/*
* SCSerial.h
* 飞特串行舵机硬件接口层程序
* 日期: 2024.11.22
* 作者: txl
*/
#ifndef _SCSERIAL_H
#define _SCSERIAL_H
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include "SCS.h"
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 readSCS(unsigned char *nDat, int nLen, unsigned long TimeOut);
int writeSCS(unsigned char bDat);//输出1字节
void rFlushSCS();//
void wFlushSCS();//
public:
unsigned long IOTimeOut;//输入输出超时
HardwareSerial *pSerial;//串口指针
};
#endif

15
src/SCServo.h Normal file
View File

@ -0,0 +1,15 @@
/*
* SCServo.h
* 飞特串行舵机接口
* 日期: 2024.11.24
* 作者: txl
*/
#ifndef _SCSERVO_H
#define _SCSERVO_H
#include "SCSCL.h"
#include "SMS_STS.h"
#include "HLSCL.h"
#endif

249
src/SMS_STS.cpp Normal file
View File

@ -0,0 +1,249 @@
/*
* SMS_STS.cpp
* 飞特SMS_STS系列串行舵机应用层程序
* 日期: 2024.11.21
* 作者: txl
*/
#include "SMS_STS.h"
SMS_STS::SMS_STS()
{
End = 0;
}
SMS_STS::SMS_STS(u8 End):SCSerial(End)
{
}
SMS_STS::SMS_STS(u8 End, u8 Level):SCSerial(End, Level)
{
}
int SMS_STS::WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC)
{
if(Position<0){
Position = -Position;
Position |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, Position);
Host2SCS(bBuf+3, bBuf+4, 0);
Host2SCS(bBuf+5, bBuf+6, Speed);
return genWrite(ID, SMS_STS_ACC, bBuf, 7);
}
int SMS_STS::RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC)
{
if(Position<0){
Position = -Position;
Position |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, Position);
Host2SCS(bBuf+3, bBuf+4, 0);
Host2SCS(bBuf+5, bBuf+6, Speed);
return regWrite(ID, SMS_STS_ACC, bBuf, 7);
}
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);
}
void SMS_STS::SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[])
{
u8 offbuf[7*IDN];
for(u8 i = 0; i<IDN; i++){
if(Speed[i]<0){
Speed[i] = -Speed[i];
Speed[i] |= (1<<15);
}
if(ACC){
offbuf[i*7] = ACC[i];
}else{
offbuf[i*7] = 0;
}
Host2SCS(offbuf+i*7+1, offbuf+i*7+2, 0);
Host2SCS(offbuf+i*7+3, offbuf+i*7+4, 0);
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, Speed[i]);
}
syncWrite(ID, IDN, SMS_STS_ACC, offbuf, 7);
}
int SMS_STS::WheelMode(u8 ID)
{
return writeByte(ID, SMS_STS_MODE, 1);
}
int SMS_STS::WriteSpe(u8 ID, s16 Speed, u8 ACC)
{
if(Speed<0){
Speed = -Speed;
Speed |= (1<<15);
}
u8 bBuf[7];
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, 0);
Host2SCS(bBuf+3, bBuf+4, 0);
Host2SCS(bBuf+5, bBuf+6, Speed);
return genWrite(ID, SMS_STS_ACC, bBuf, 7);
}
int SMS_STS::EnableTorque(u8 ID, u8 Enable)
{
return writeByte(ID, SMS_STS_TORQUE_ENABLE, Enable);
}
int SMS_STS::unLockEprom(u8 ID)
{
return writeByte(ID, SMS_STS_LOCK, 0);
}
int SMS_STS::LockEprom(u8 ID)
{
return writeByte(ID, SMS_STS_LOCK, 1);
}
int SMS_STS::CalibrationOfs(u8 ID)
{
return writeByte(ID, SMS_STS_TORQUE_ENABLE, 128);
}
int SMS_STS::FeedBack(int ID)
{
int nLen = Read(ID, SMS_STS_PRESENT_POSITION_L, Mem, sizeof(Mem));
if(nLen!=sizeof(Mem)){
return -1;
}
return nLen;
}
int SMS_STS::ReadPos(int ID)
{
int Pos = -1;
if(ID==-1){
Pos = Mem[SMS_STS_PRESENT_POSITION_H-SMS_STS_PRESENT_POSITION_L];
Pos <<= 8;
Pos |= Mem[SMS_STS_PRESENT_POSITION_L-SMS_STS_PRESENT_POSITION_L];
}else{
Pos = readWord(ID, SMS_STS_PRESENT_POSITION_L);
}
if(Pos&(1<<15)){
Pos = -(Pos&~(1<<15));
}
return Pos;
}
int SMS_STS::ReadSpeed(int ID)
{
int Speed = -1;
if(ID==-1){
Speed = Mem[SMS_STS_PRESENT_SPEED_H-SMS_STS_PRESENT_POSITION_L];
Speed <<= 8;
Speed |= Mem[SMS_STS_PRESENT_SPEED_L-SMS_STS_PRESENT_POSITION_L];
}else{
Speed = readWord(ID, SMS_STS_PRESENT_SPEED_L);
}
if(Speed&(1<<15)){
Speed = -(Speed&~(1<<15));
}
return Speed;
}
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{
Load = readWord(ID, SMS_STS_PRESENT_LOAD_L);
}
if(Load&(1<<10)){
Load = -(Load&~(1<<10));
}
return Load;
}
int SMS_STS::ReadVoltage(int ID)
{
int Voltage = -1;
if(ID==-1){
Voltage = Mem[SMS_STS_PRESENT_VOLTAGE-SMS_STS_PRESENT_POSITION_L];
}else{
Voltage = readByte(ID, SMS_STS_PRESENT_VOLTAGE);
}
return Voltage;
}
int SMS_STS::ReadTemper(int ID)
{
int Temper = -1;
if(ID==-1){
Temper = Mem[SMS_STS_PRESENT_TEMPERATURE-SMS_STS_PRESENT_POSITION_L];
}else{
Temper = readByte(ID, SMS_STS_PRESENT_TEMPERATURE);
}
return Temper;
}
int SMS_STS::ReadMove(int ID)
{
int Move = -1;
if(ID==-1){
Move = Mem[SMS_STS_MOVING-SMS_STS_PRESENT_POSITION_L];
}else{
Move = readByte(ID, SMS_STS_MOVING);
}
return Move;
}
int SMS_STS::ReadCurrent(int ID)
{
int Current = -1;
if(ID==-1){
Current = Mem[SMS_STS_PRESENT_CURRENT_H-SMS_STS_PRESENT_POSITION_L];
Current <<= 8;
Current |= Mem[SMS_STS_PRESENT_CURRENT_L-SMS_STS_PRESENT_POSITION_L];
}else{
Current = readWord(ID, SMS_STS_PRESENT_CURRENT_L);
}
if(Current&(1<<15)){
Current = -(Current&~(1<<15));
}
return Current;
}
int SMS_STS::ServoMode(u8 ID)
{
return writeByte(ID, SMS_STS_MODE, 0);
}

86
src/SMS_STS.h Normal file
View File

@ -0,0 +1,86 @@
/*
* SMS_STS.h
* 飞特SMS/STS系列串行舵机应用层程序
* 日期: 2024.11.21
* 作者: txl
*/
#ifndef _SMS_STS_H
#define _SMS_STS_H
//内存表定义
//-------EPROM(只读)--------
#define SMS_STS_MODEL_L 3
#define SMS_STS_MODEL_H 4
//-------EPROM(读写)--------
#define SMS_STS_ID 5
#define SMS_STS_BAUD_RATE 6
#define SMS_STS_MIN_ANGLE_LIMIT_L 9
#define SMS_STS_MIN_ANGLE_LIMIT_H 10
#define SMS_STS_MAX_ANGLE_LIMIT_L 11
#define SMS_STS_MAX_ANGLE_LIMIT_H 12
#define SMS_STS_CW_DEAD 26
#define SMS_STS_CCW_DEAD 27
#define SMS_STS_OFS_L 31
#define SMS_STS_OFS_H 32
#define SMS_STS_MODE 33
//-------SRAM(读写)--------
#define SMS_STS_TORQUE_ENABLE 40
#define SMS_STS_ACC 41
#define SMS_STS_GOAL_POSITION_L 42
#define SMS_STS_GOAL_POSITION_H 43
#define SMS_STS_GOAL_TIME_L 44
#define SMS_STS_GOAL_TIME_H 45
#define SMS_STS_GOAL_SPEED_L 46
#define SMS_STS_GOAL_SPEED_H 47
#define SMS_STS_TORQUE_LIMIT_L 48
#define SMS_STS_TORQUE_LIMIT_H 49
#define SMS_STS_LOCK 55
//-------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
#include "SCSerial.h"
class SMS_STS : public SCSerial
{
public:
SMS_STS();
SMS_STS(u8 End);
SMS_STS(u8 End, u8 Level);
int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//普通写单个舵机位置指令
int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//异步写单个舵机位置指令(RegWriteAction生效)
void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]);//同步写多个舵机位置指令
void SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[]);//同步写多个舵机速度指令
int ServoMode(u8 ID);//Servo模式
int WheelMode(u8 ID);//恒速模式
int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0);//恒速模式控制指令
int EnableTorque(u8 ID, u8 Enable);//扭力控制指令
int unLockEprom(u8 ID);//eprom解锁
int LockEprom(u8 ID);//eprom加锁
int CalibrationOfs(u8 ID);//中位校准
int FeedBack(int ID);//反馈舵机信息
int ReadPos(int ID);//读位置
int ReadSpeed(int ID);//读速度
int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
int ReadVoltage(int ID);//读电压
int ReadTemper(int ID);//读温度
int ReadMove(int ID);//读移动状态
int ReadCurrent(int ID);//读电流
private:
u8 Mem[SMS_STS_PRESENT_CURRENT_H-SMS_STS_PRESENT_POSITION_L+1];
};
#endif

131
src/main.cpp Normal file
View File

@ -0,0 +1,131 @@
#include <Arduino.h>
#include "SCServo.h"
SMS_STS sms_sts;
void test_ping();
void lire_tous_les_registres(int servo_id);
char registre_description[70][100]={
"Version majeure du firmware",
"Version mineure du firmware",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
"",
""
};
// the setup routine runs once when you press reset:
void setup() {
// initialize the digital pin as an output.
pinMode(LED_BUILTIN, OUTPUT);
Serial.begin(115200);//sts舵机波特率1000000
Serial1.begin(1000000);//sts舵机波特率1000000
sms_sts.pSerial = &Serial1;
}
// the loop routine runs over and over again forever:
void loop() {
digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
test_ping();
digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
test_ping();
}
void test_ping()
{
int ID = sms_sts.Ping(1);
static int found = 0;
if(ID!=-1){
Serial.print("Servo ID:");
Serial.println(ID);
if(found == 0){
found = 1;
lire_tous_les_registres(ID);
}
delay(100);
}else{
Serial.print("Ping servo ID error!\n");
found = 0;
delay(2000);
}
}
int lire_registre(int servo_id, int registre_adresse){
return sms_sts.readByte(servo_id, registre_adresse); // Resolution à 1
}
void lire_tous_les_registres(int servo_id){
char message[200]="";
for (int i=0; i<0x46; i++){
sprintf(message, "registre 0x%x: %d\t\t%s\n", i, lire_registre(servo_id, i), registre_description[i]);
Serial.print(message);
}
}