fallensigh/craic
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

WIP: snapshot before repo migration

Browse Source
This commit is contained in:
FallenSigh
2026-05-27 02:14:33 +08:00
parent aa7ed7cf51
commit ec41ed8a48
51 changed files with 3119 additions and 2015 deletions
Download Patch File Download Diff File Expand all files Collapse all files

260
jxbeye/lib/FTServo/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
jxbeye/lib/FTServo/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
jxbeye/lib/FTServo/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
jxbeye/lib/FTServo/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
jxbeye/lib/FTServo/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
jxbeye/lib/FTServo/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
jxbeye/lib/FTServo/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
jxbeye/lib/FTServo/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
jxbeye/lib/FTServo/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
jxbeye/lib/FTServo/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
jxbeye/lib/FTServo/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
jxbeye/lib/FTServo/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