优化了平衡，尤其对于向后倾倒的几种极限情况进行优化
This commit is contained in:
myperworld committed
1 parent
10aba2664d
commit
fcb8af8163
693 files changed
+2427
-70
No files matched your search
@@ -0,0 +1,40 @@
|
||||
/*
|
||||
* INST.h
|
||||
* 飞特串行舵机协议指令定义
|
||||
* 日期: 2021.3.11
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#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;
|
||||
|
||||
#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 _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
|
||||
@@ -0,0 +1,372 @@
|
||||
/*
|
||||
* SCS.cpp
|
||||
* 飞特串行舵机通信层协议程序
|
||||
* 日期: 2019.12.18
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#include <stddef.h>
|
||||
#include "SCS.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，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();
|
||||
if(!checkHead()){
|
||||
return 0;
|
||||
}
|
||||
u8 bBuf[4];
|
||||
Error = 0;
|
||||
if(readSCS(bBuf, 3)!=3){
|
||||
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;
|
||||
}
|
||||
|
||||
//读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();
|
||||
Error = 0;
|
||||
if(!checkHead()){
|
||||
return -1;
|
||||
}
|
||||
u8 bBuf[4];
|
||||
if(readSCS(bBuf, 4)!=4){
|
||||
return -1;
|
||||
}
|
||||
if(bBuf[0]!=ID && ID!=0xfe){
|
||||
return -1;
|
||||
}
|
||||
if(bBuf[1]!=2){
|
||||
return -1;
|
||||
}
|
||||
u8 calSum = ~(bBuf[0]+bBuf[1]+bBuf[2]);
|
||||
if(calSum!=bBuf[3]){
|
||||
return -1;
|
||||
}
|
||||
Error = bBuf[2];
|
||||
return bBuf[0];
|
||||
}
|
||||
|
||||
int SCS::checkHead()
|
||||
{
|
||||
u8 bDat;
|
||||
u8 bBuf[2] = {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)
|
||||
{
|
||||
Error = 0;
|
||||
if(ID!=0xfe && Level){
|
||||
if(!checkHead()){
|
||||
return 0;
|
||||
}
|
||||
u8 bBuf[4];
|
||||
if(readSCS(bBuf, 4)!=4){
|
||||
return 0;
|
||||
}
|
||||
if(bBuf[0]!=ID){
|
||||
return 0;
|
||||
}
|
||||
if(bBuf[1]!=2){
|
||||
return 0;
|
||||
}
|
||||
u8 calSum = ~(bBuf[0]+bBuf[1]+bBuf[2]);
|
||||
if(calSum!=bBuf[3]){
|
||||
return 0;
|
||||
}
|
||||
Error = bBuf[2];
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
int SCS::syncReadPacketTx(u8 ID[], u8 IDN, u8 MemAddr, u8 nLen)
|
||||
{
|
||||
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);
|
||||
return nLen;
|
||||
}
|
||||
|
||||
int SCS::syncReadPacketRx(u8 ID, u8 *nDat)
|
||||
{
|
||||
syncReadRxPacket = nDat;
|
||||
syncReadRxPacketIndex = 0;
|
||||
u8 bBuf[4];
|
||||
if(!checkHead()){
|
||||
return 0;
|
||||
}
|
||||
if(readSCS(bBuf, 3)!=3){
|
||||
return 0;
|
||||
}
|
||||
if(bBuf[0]!=ID){
|
||||
return 0;
|
||||
}
|
||||
if(bBuf[1]!=(syncReadRxPacketLen+2)){
|
||||
return 0;
|
||||
}
|
||||
Error = bBuf[2];
|
||||
if(readSCS(nDat, syncReadRxPacketLen)!=syncReadRxPacketLen){
|
||||
return 0;
|
||||
}
|
||||
return syncReadRxPacketLen;
|
||||
}
|
||||
|
||||
int SCS::syncReadRxPacketToByte()
|
||||
{
|
||||
if(syncReadRxPacketIndex>=syncReadRxPacketLen){
|
||||
return -1;
|
||||
}
|
||||
return syncReadRxPacket[syncReadRxPacketIndex++];
|
||||
}
|
||||
|
||||
int SCS::syncReadRxPacketToWrod(u8 negBit)
|
||||
{
|
||||
if((syncReadRxPacketIndex+1)>=syncReadRxPacketLen){
|
||||
return -1;
|
||||
}
|
||||
int Word = SCS2Host(syncReadRxPacket[syncReadRxPacketIndex], syncReadRxPacket[syncReadRxPacketIndex+1]);
|
||||
syncReadRxPacketIndex += 2;
|
||||
if(negBit){
|
||||
if(Word&(1<<negBit)){
|
||||
Word = -(Word & ~(1<<negBit));
|
||||
}
|
||||
}
|
||||
return Word;
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
/*
|
||||
* SCS.h
|
||||
* 飞特串行舵机通信层协议程序
|
||||
* 日期: 2019.12.18
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#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表示无方向
|
||||
public:
|
||||
u8 Level;//舵机返回等级
|
||||
u8 End;//处理器大小端结构
|
||||
u8 Error;//舵机状态
|
||||
u8 syncReadRxPacketIndex;
|
||||
u8 syncReadRxPacketLen;
|
||||
u8 *syncReadRxPacket;
|
||||
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();//帧头检测
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,281 @@
|
||||
/*
|
||||
* SCSCL.cpp
|
||||
* 飞特SCSCL系列串行舵机应用层程序
|
||||
* 日期: 2020.6.17
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#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::WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC)
|
||||
{
|
||||
ACC = 0;
|
||||
u16 Time = 0;
|
||||
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);
|
||||
}
|
||||
|
||||
int SCSCL::CalibrationOfs(u8 ID){
|
||||
return -1;
|
||||
}
|
||||
|
||||
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::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);
|
||||
}
|
||||
|
||||
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)){
|
||||
Err = 1;
|
||||
return -1;
|
||||
}
|
||||
Err = 0;
|
||||
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{
|
||||
Err = 0;
|
||||
Pos = readWord(ID, SCSCL_PRESENT_POSITION_L);
|
||||
if(Pos==-1){
|
||||
Err = 1;
|
||||
}
|
||||
}
|
||||
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{
|
||||
Err = 0;
|
||||
Speed = readWord(ID, SCSCL_PRESENT_SPEED_L);
|
||||
if(Speed==-1){
|
||||
Err = 1;
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
if(!Err && (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{
|
||||
Err = 0;
|
||||
Load = readWord(ID, SCSCL_PRESENT_LOAD_L);
|
||||
if(Load==-1){
|
||||
Err = 1;
|
||||
}
|
||||
}
|
||||
if(!Err && (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{
|
||||
Err = 0;
|
||||
Voltage = readByte(ID, SCSCL_PRESENT_VOLTAGE);
|
||||
if(Voltage==-1){
|
||||
Err = 1;
|
||||
}
|
||||
}
|
||||
return Voltage;
|
||||
}
|
||||
|
||||
int SCSCL::ReadTemper(int ID)
|
||||
{
|
||||
int Temper = -1;
|
||||
if(ID==-1){
|
||||
Temper = Mem[SCSCL_PRESENT_TEMPERATURE-SCSCL_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Err = 0;
|
||||
Temper = readByte(ID, SCSCL_PRESENT_TEMPERATURE);
|
||||
if(Temper==-1){
|
||||
Err = 1;
|
||||
}
|
||||
}
|
||||
return Temper;
|
||||
}
|
||||
|
||||
int SCSCL::ReadMove(int ID)
|
||||
{
|
||||
int Move = -1;
|
||||
if(ID==-1){
|
||||
Move = Mem[SCSCL_MOVING-SCSCL_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Err = 0;
|
||||
Move = readByte(ID, SCSCL_MOVING);
|
||||
if(Move==-1){
|
||||
Err = 1;
|
||||
}
|
||||
}
|
||||
return Move;
|
||||
}
|
||||
|
||||
int SCSCL::ReadMode(int ID)
|
||||
{
|
||||
int ValueRead = -1;
|
||||
ValueRead = readWord(ID, SCSCL_MIN_ANGLE_LIMIT_L);
|
||||
if(ValueRead == 0){
|
||||
return 3;
|
||||
}
|
||||
else if(ValueRead > 0){
|
||||
return 0;
|
||||
}
|
||||
// int Mode = -1;
|
||||
// if(ID==-1){
|
||||
// Mode = Mem[SMS_STS_MODE-SMS_STS_PRESENT_POSITION_L];
|
||||
// }else{
|
||||
// Err = 0;
|
||||
// Mode = readByte(ID, SMS_STS_MODE);
|
||||
// if(Mode==-1){
|
||||
// Err = 1;
|
||||
// }
|
||||
// }
|
||||
return ValueRead;
|
||||
}
|
||||
|
||||
int SCSCL::ReadInfoValue(int ID, int AddInput)
|
||||
{
|
||||
int ValueRead = -1;
|
||||
ValueRead = readWord(ID, AddInput);
|
||||
return ValueRead;
|
||||
}
|
||||
|
||||
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{
|
||||
Err = 0;
|
||||
Current = readWord(ID, SCSCL_PRESENT_CURRENT_L);
|
||||
if(Current==-1){
|
||||
Err = 1;
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
if(!Err && (Current&(1<<15))){
|
||||
Current = -(Current&~(1<<15));
|
||||
}
|
||||
return Current;
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
* SCSCL.h
|
||||
* 飞特SCSCL系列串行舵机应用层程序
|
||||
* 日期: 2021.3.11
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#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);
|
||||
virtual int WritePos(u8 ID, u16 Position, u16 Time, u16 Speed);//普通写单个舵机位置指令
|
||||
virtual int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC);//单个舵机位置指令
|
||||
virtual int RegWritePos(u8 ID, u16 Position, u16 Time, u16 Speed = 0);//异步写单个舵机位置指令(RegWriteAction生效)
|
||||
virtual void SyncWritePos(u8 ID[], u8 IDN, u16 Position[], u16 Time[], u16 Speed[]);//同步写多个舵机位置指令
|
||||
virtual int PWMMode(u8 ID);//PWM输出模式
|
||||
virtual int WritePWM(u8 ID, s16 pwmOut);//PWM输出模式指令
|
||||
virtual int EnableTorque(u8 ID, u8 Enable);//扭矩控制指令
|
||||
virtual int unLockEprom(u8 ID);//eprom解锁
|
||||
virtual int LockEprom(u8 ID);//eprom加锁
|
||||
virtual int FeedBack(int ID);//反馈舵机信息
|
||||
virtual int ReadPos(int ID);//读位置
|
||||
virtual int ReadSpeed(int ID);//读速度
|
||||
virtual int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
|
||||
virtual int ReadVoltage(int ID);//读电压
|
||||
virtual int ReadTemper(int ID);//读温度
|
||||
virtual int ReadMove(int ID);//读移动状态
|
||||
virtual int ReadCurrent(int ID);//读电流
|
||||
virtual int ReadMode(int ID);
|
||||
virtual int CalibrationOfs(u8 ID);
|
||||
virtual int ReadInfoValue(int ID, int AddInput);
|
||||
private:
|
||||
u8 Mem[SCSCL_PRESENT_CURRENT_H-SCSCL_PRESENT_POSITION_L+1];
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,75 @@
|
||||
/*
|
||||
* SCSerial.h
|
||||
* 飞特串行舵机硬件接口层程序
|
||||
* 日期: 2019.4.27
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
|
||||
#include "SCSerial.h"
|
||||
|
||||
SCSerial::SCSerial()
|
||||
{
|
||||
IOTimeOut = 100;
|
||||
pSerial = NULL;
|
||||
}
|
||||
|
||||
SCSerial::SCSerial(u8 End):SCS(End)
|
||||
{
|
||||
IOTimeOut = 100;
|
||||
pSerial = NULL;
|
||||
}
|
||||
|
||||
SCSerial::SCSerial(u8 End, u8 Level):SCS(End, Level)
|
||||
{
|
||||
IOTimeOut = 100;
|
||||
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()
|
||||
{
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
/*
|
||||
* SCSerial.h
|
||||
* 飞特串行舵机硬件接口层程序
|
||||
* 日期: 2019.4.27
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#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:
|
||||
virtual int writeSCS(unsigned char *nDat, int nLen);//输出nLen字节
|
||||
virtual int readSCS(unsigned char *nDat, int nLen);//输入nLen字节
|
||||
virtual int writeSCS(unsigned char bDat);//输出1字节
|
||||
virtual void rFlushSCS();//
|
||||
virtual void wFlushSCS();//
|
||||
public:
|
||||
unsigned long int IOTimeOut;//输入输出超时
|
||||
HardwareSerial *pSerial;//串口指针
|
||||
int Err;
|
||||
public:
|
||||
virtual int getErr(){ return Err; }
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,14 @@
|
||||
/*
|
||||
* SCServo.h
|
||||
* 飞特串行舵机接口
|
||||
* 日期: 2021.3.11
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#ifndef _SCSERVO_H
|
||||
#define _SCSERVO_H
|
||||
|
||||
#include "SCSCL.h"
|
||||
#include "SMS_STS.h"
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,24 @@
|
||||
通信层:SCS
|
||||
----------------------------
|
||||
硬件接口层:SCSerial
|
||||
----------------------------
|
||||
应用层:SMS_STS SCSCL分别对应飞特三个系列舵机
|
||||
|
||||
|
||||
|
||||
SMS_STS sms_sts;//定义SMSBL/SMSCL/STSCL系列舵机
|
||||
SCSCL sc;//定义SCSCL系列舵机
|
||||
|
||||
|
||||
|
||||
INST.h---指令定义头文件
|
||||
SCS.h/SCS.cpp---通信层程序
|
||||
SCSerial.h/SCSerial.cpp---硬件接口程序
|
||||
SMS_STS.h/SMS_STS.cpp---SMSBL/SMSCL/STSCL应用层程序
|
||||
SCSCL.h/SCSCL.cpp---SCSCL应用层程序
|
||||
(内存表定义于应用层程序头文件SMS_STS.h\SCSCL.h中不同系列舵内存表定义存在差异)
|
||||
|
||||
|
||||
SCS类<---SCSerial类<---SMS_STS类/SCSCL类
|
||||
|
||||
arduino使用Atmega2560测试
|
||||
@@ -0,0 +1,271 @@
|
||||
/*
|
||||
* SMS_STS.cpp
|
||||
* 飞特SMS_STS系列串行舵机应用层程序
|
||||
* 日期: 2020.6.17
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#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);
|
||||
}
|
||||
|
||||
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[2];
|
||||
bBuf[0] = ACC;
|
||||
genWrite(ID, SMS_STS_ACC, bBuf, 1);
|
||||
Host2SCS(bBuf+0, bBuf+1, Speed);
|
||||
|
||||
return genWrite(ID, SMS_STS_GOAL_SPEED_L, bBuf, 2);
|
||||
}
|
||||
|
||||
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)){
|
||||
Err = 1;
|
||||
return -1;
|
||||
}
|
||||
Err = 0;
|
||||
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{
|
||||
Err = 0;
|
||||
Pos = readWord(ID, SMS_STS_PRESENT_POSITION_L);
|
||||
if(Pos==-1){
|
||||
Err = 1;
|
||||
}
|
||||
}
|
||||
if(!Err && (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{
|
||||
Err = 0;
|
||||
Speed = readWord(ID, SMS_STS_PRESENT_SPEED_L);
|
||||
if(Speed==-1){
|
||||
Err = 1;
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
if(!Err && (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{
|
||||
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;
|
||||
}
|
||||
|
||||
int SMS_STS::ReadVoltage(int ID)
|
||||
{
|
||||
int Voltage = -1;
|
||||
if(ID==-1){
|
||||
Voltage = Mem[SMS_STS_PRESENT_VOLTAGE-SMS_STS_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Err = 0;
|
||||
Voltage = readByte(ID, SMS_STS_PRESENT_VOLTAGE);
|
||||
if(Voltage==-1){
|
||||
Err = 1;
|
||||
}
|
||||
}
|
||||
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{
|
||||
Err = 0;
|
||||
Temper = readByte(ID, SMS_STS_PRESENT_TEMPERATURE);
|
||||
if(Temper==-1){
|
||||
Err = 1;
|
||||
}
|
||||
}
|
||||
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{
|
||||
Err = 0;
|
||||
Move = readByte(ID, SMS_STS_MOVING);
|
||||
if(Move==-1){
|
||||
Err = 1;
|
||||
}
|
||||
}
|
||||
return Move;
|
||||
}
|
||||
|
||||
int SMS_STS::ReadMode(int ID)
|
||||
{
|
||||
int Mode = -1;
|
||||
if(ID==-1){
|
||||
Mode = Mem[SMS_STS_MODE-SMS_STS_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Err = 0;
|
||||
Mode = readByte(ID, SMS_STS_MODE);
|
||||
if(Mode==-1){
|
||||
Err = 1;
|
||||
}
|
||||
}
|
||||
return Mode;
|
||||
}
|
||||
|
||||
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{
|
||||
Err = 0;
|
||||
Current = readWord(ID, SMS_STS_PRESENT_CURRENT_L);
|
||||
if(Current==-1){
|
||||
Err = 1;
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
if(!Err && (Current&(1<<15))){
|
||||
Current = -(Current&~(1<<15));
|
||||
}
|
||||
return Current;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
* SMS_STS.h
|
||||
* 飞特SMS/STS系列串行舵机应用层程序
|
||||
* 日期: 2021.3.11
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#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);
|
||||
virtual int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//普通写单个舵机位置指令
|
||||
virtual int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//异步写单个舵机位置指令(RegWriteAction生效)
|
||||
virtual void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]);//同步写多个舵机位置指令
|
||||
virtual int WheelMode(u8 ID);//恒速模式
|
||||
virtual int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0);//恒速模式控制指令
|
||||
virtual int EnableTorque(u8 ID, u8 Enable);//扭力控制指令
|
||||
virtual int unLockEprom(u8 ID);//eprom解锁
|
||||
virtual int LockEprom(u8 ID);//eprom加锁
|
||||
virtual int CalibrationOfs(u8 ID);//中位校准
|
||||
virtual int FeedBack(int ID);//反馈舵机信息
|
||||
virtual int ReadPos(int ID);//读位置
|
||||
virtual int ReadSpeed(int ID);//读速度
|
||||
virtual int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
|
||||
virtual int ReadVoltage(int ID);//读电压
|
||||
virtual int ReadTemper(int ID);//读温度
|
||||
virtual int ReadMove(int ID);//读移动状态
|
||||
virtual int ReadCurrent(int ID);//读电流
|
||||
virtual int ReadMode(int ID);
|
||||
private:
|
||||
u8 Mem[SMS_STS_PRESENT_CURRENT_H-SMS_STS_PRESENT_POSITION_L+1];
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,32 @@
|
||||
/*
|
||||
The broadcast write example was tested in SCS15,
|
||||
and if testing other models of SCS series rudders please change the
|
||||
appropriate position, speed and delay parameters.
|
||||
*/
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SCSCL sc;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
sc.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
sc.WritePos(0xfe, 1000, 0, 1500);//All servos move at the max speed=1500, move to Position=1000
|
||||
delay(754);//[(P1-P0)/V]*1000+100
|
||||
|
||||
sc.WritePos(0xfe, 20, 0, 1500);//All servos move at the max speed=1500, move to Position=20
|
||||
delay(754);//[(P1-P0)/V]*1000+100
|
||||
}
|
||||
@@ -0,0 +1,122 @@
|
||||
/*
|
||||
回读所有舵机反馈参数:位置、速度、负载、电压、温度、移动状态；
|
||||
FeedBack函数回读舵机参数于缓冲区，Readxxx(-1)函数返回缓冲区中相应的舵机状态；
|
||||
函数Readxxx(ID)，ID=-1返回FeedBack缓冲区参数；ID>=0，通过读指令直接返回指定ID舵机状态,
|
||||
无需调用FeedBack函数。
|
||||
Read back all feedback parameters: position, speed, load, voltage, temperature, movement status;
|
||||
The FeedBack function reads back the servo parameters in the buffer, and the Readxxx (-1) function returns the corresponding servo state in the buffer;
|
||||
Function Readxxx (ID), ID=1 returns the FeedBack buffer parameter; ID > 0, and directly returns the specified ID rudder state by reading the instruction.
|
||||
There is no need to call the FeedBack function.
|
||||
*/
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SCSCL sc;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
Serial.begin(115200);
|
||||
sc.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
int Pos;
|
||||
int Speed;
|
||||
int Load;
|
||||
int Voltage;
|
||||
int Temper;
|
||||
int Move;
|
||||
if(sc.FeedBack(1)!=-1){
|
||||
Pos = sc.ReadPos(-1);
|
||||
Speed = sc.ReadSpeed(-1);
|
||||
Load = sc.ReadLoad(-1);
|
||||
Voltage = sc.ReadVoltage(-1);
|
||||
Temper = sc.ReadTemper(-1);
|
||||
Move = sc.ReadMove(-1);
|
||||
Serial.print("Position:");
|
||||
Serial.println(Pos);
|
||||
Serial.print("Speed:");
|
||||
Serial.println(Speed);
|
||||
Serial.print("Load:");
|
||||
Serial.println(Load);
|
||||
Serial.print("Voltage:");
|
||||
Serial.println(Voltage);
|
||||
Serial.print("Temper:");
|
||||
Serial.println(Temper);
|
||||
Serial.print("Move:");
|
||||
Serial.println(Move);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("FeedBack err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Pos = sc.ReadPos(1);
|
||||
if(Pos!=-1){
|
||||
Serial.print("Servo position:");
|
||||
Serial.println(Pos, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read position err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Voltage = sc.ReadVoltage(1);
|
||||
if(Voltage!=-1){
|
||||
Serial.print("Servo Voltage:");
|
||||
Serial.println(Voltage, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read Voltage err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Temper = sc.ReadTemper(1);
|
||||
if(Temper!=-1){
|
||||
Serial.print("Servo temperature:");
|
||||
Serial.println(Temper, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read temperature err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Speed = sc.ReadSpeed(1);
|
||||
if(Speed!=-1){
|
||||
Serial.print("Servo Speed:");
|
||||
Serial.println(Speed, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read Speed err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Load = sc.ReadLoad(1);
|
||||
if(Load!=-1){
|
||||
Serial.print("Servo Load:");
|
||||
Serial.println(Load, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read Load err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Move = sc.ReadMove(1);
|
||||
if(Move!=-1){
|
||||
Serial.print("Servo Move:");
|
||||
Serial.println(Move, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read Move err");
|
||||
delay(500);
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
/*
|
||||
Ping() to test a servo on the bus is ready.
|
||||
*/
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SCSCL sc;
|
||||
|
||||
int TEST_ID = 3;
|
||||
|
||||
int LEDpin = 13;
|
||||
void setup()
|
||||
{
|
||||
pinMode(LEDpin,OUTPUT);
|
||||
digitalWrite(LEDpin, HIGH);
|
||||
Serial.begin(115200);
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
sc.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
int ID = sc.Ping(TEST_ID);
|
||||
if(ID!=-1){
|
||||
digitalWrite(LEDpin, LOW);
|
||||
Serial.print("Servo ID:");
|
||||
Serial.println(ID, DEC);
|
||||
delay(100);
|
||||
}else{
|
||||
Serial.println("Ping servo ID error!");
|
||||
digitalWrite(LEDpin, HIGH);
|
||||
delay(2000);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
example for changing ID.
|
||||
*/
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SCSCL sc;
|
||||
|
||||
int ID_ChangeFrom = 1;
|
||||
int ID_Changeto = 2;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
sc.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
|
||||
sc.unLockEprom(ID_ChangeFrom);//unlock EPROM-SAFE
|
||||
sc.writeByte(ID_ChangeFrom, SCSCL_ID, ID_Changeto);//ID
|
||||
sc.LockEprom(ID_Changeto);// EPROM-SAFE locked
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
The RegWritePos example passed the test in SCS15,
|
||||
and if testing other models of SCS series servos
|
||||
please change the appropriate position, speed and delay parameters.
|
||||
*/
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SCSCL sc;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
sc.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
sc.RegWritePos(1, 1000, 0, 1500);//Servo(ID1) moves at max speed=1500, moves to position=1000.
|
||||
sc.RegWritePos(2, 1000, 0, 1500);//Servo(ID2) moves at max speed=1500, moves to position=1000.
|
||||
sc.RegWriteAction();//Start moving.
|
||||
delay(754);//[(P1-P0)/V]*1000+100
|
||||
|
||||
sc.RegWritePos(1, 20, 0, 1500);//Servo(ID1) moves at max speed=1500, moves to position=20.
|
||||
sc.RegWritePos(2, 20, 0, 1500);//Servo(ID2) moves at max speed=1500, moves to position=20.
|
||||
sc.RegWriteAction();//Start moving.
|
||||
delay(754);//[(P1-P0)/V]*1000+100
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
/*
|
||||
The SyncWritePos example passed the test in SCS15,
|
||||
and if testing other models of SCS series servos
|
||||
please change the appropriate position, speed and delay parameters.
|
||||
*/
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SCSCL sc;
|
||||
|
||||
byte ID[2];
|
||||
u16 Position[2];
|
||||
u16 Speed[2];
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
sc.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
ID[0] = 1;
|
||||
ID[1] = 2;
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
Position[0] = 1000;
|
||||
Position[1] = 1000;
|
||||
Speed[0] = 1500;
|
||||
Speed[1] = 1500;
|
||||
sc.SyncWritePos(ID, 2, Position, 0, Speed);//Servo((ID1/ID2)) moves at max speed=1500, moves to position=1000.
|
||||
delay(754);//[(P1-P0)/V]*1000+100
|
||||
|
||||
Position[0] = 20;
|
||||
Position[1] = 20;
|
||||
Speed[0] = 1500;
|
||||
Speed[1] = 1500;
|
||||
sc.SyncWritePos(ID, 2, Position, 0, Speed);//Servo((ID1/ID2)) moves at max speed=1500, moves to position=20.
|
||||
delay(754);//[(P1-P0)/V]*1000+100
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
The normal write example passed the test in SCS15,
|
||||
and if testing other models of SCS series servos
|
||||
please change the appropriate position, speed and delay parameters.
|
||||
*/
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SCSCL sc;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
sc.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
sc.WritePos(1, 1000, 0, 1500);//Servo(ID1) moves at max speed=1500, moves to position=1000.
|
||||
delay(754);//[(P1-P0)/V]*1000+100
|
||||
|
||||
sc.WritePos(1, 20, 0, 1500);//Servo(ID1) moves at max speed=1500, moves to position=20.
|
||||
delay(754);//[(P1-P0)/V]*1000+100
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
/*
|
||||
set the current position as middle.
|
||||
*/
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SMS_STS st;
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
st.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
st.CalibrationOfs(1);
|
||||
while(1);
|
||||
}
|
||||
@@ -0,0 +1,135 @@
|
||||
/*
|
||||
回读所有舵机反馈参数:位置、速度、负载、电压、温度、移动状态、电流；
|
||||
FeedBack函数回读舵机参数于缓冲区，Readxxx(-1)函数返回缓冲区中相应的舵机状态；
|
||||
函数Readxxx(ID)，ID=-1返回FeedBack缓冲区参数；ID>=0，通过读指令直接返回指定ID舵机状态,
|
||||
无需调用FeedBack函数。
|
||||
Read back all feedback parameters: position, speed, load, voltage, temperature, movement status;
|
||||
The FeedBack function reads back the servo parameters in the buffer, and the Readxxx (-1) function returns the corresponding servo state in the buffer;
|
||||
Function Readxxx (ID), ID=1 returns the FeedBack buffer parameter; ID > 0, and directly returns the specified ID rudder state by reading the instruction.
|
||||
There is no need to call the FeedBack function.
|
||||
*/
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
#include <SCServo.h>
|
||||
|
||||
SMS_STS sms_sts;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
Serial.begin(115200);
|
||||
sms_sts.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
int Pos;
|
||||
int Speed;
|
||||
int Load;
|
||||
int Voltage;
|
||||
int Temper;
|
||||
int Move;
|
||||
int Current;
|
||||
if(sms_sts.FeedBack(1)!=-1){
|
||||
Pos = sms_sts.ReadPos(-1);
|
||||
Speed = sms_sts.ReadSpeed(-1);
|
||||
Load = sms_sts.ReadLoad(-1);
|
||||
Voltage = sms_sts.ReadVoltage(-1);
|
||||
Temper = sms_sts.ReadTemper(-1);
|
||||
Move = sms_sts.ReadMove(-1);
|
||||
Current = sms_sts.ReadCurrent(-1);
|
||||
Serial.print("Position:");
|
||||
Serial.println(Pos);
|
||||
Serial.print("Speed:");
|
||||
Serial.println(Speed);
|
||||
Serial.print("Load:");
|
||||
Serial.println(Load);
|
||||
Serial.print("Voltage:");
|
||||
Serial.println(Voltage);
|
||||
Serial.print("Temper:");
|
||||
Serial.println(Temper);
|
||||
Serial.print("Move:");
|
||||
Serial.println(Move);
|
||||
Serial.print("Current:");
|
||||
Serial.println(Current);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("FeedBack err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Pos = sms_sts.ReadPos(1);
|
||||
if(Pos!=-1){
|
||||
Serial.print("Servo position:");
|
||||
Serial.println(Pos, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read position err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Voltage = sms_sts.ReadVoltage(1);
|
||||
if(Voltage!=-1){
|
||||
Serial.print("Servo Voltage:");
|
||||
Serial.println(Voltage, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read Voltage err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Temper = sms_sts.ReadTemper(1);
|
||||
if(Temper!=-1){
|
||||
Serial.print("Servo temperature:");
|
||||
Serial.println(Temper, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read temperature err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Speed = sms_sts.ReadSpeed(1);
|
||||
if(Speed!=-1){
|
||||
Serial.print("Servo Speed:");
|
||||
Serial.println(Speed, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read Speed err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Load = sms_sts.ReadLoad(1);
|
||||
if(Load!=-1){
|
||||
Serial.print("Servo Load:");
|
||||
Serial.println(Load, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read Load err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Current = sms_sts.ReadCurrent(1);
|
||||
if(Current!=-1){
|
||||
Serial.print("Servo Current:");
|
||||
Serial.println(Current, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read Current err");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Move = sms_sts.ReadMove(1);
|
||||
if(Move!=-1){
|
||||
Serial.print("Servo Move:");
|
||||
Serial.println(Move, DEC);
|
||||
delay(10);
|
||||
}else{
|
||||
Serial.println("read Move err");
|
||||
delay(500);
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
Ping the servo to check if it is ready.
|
||||
*/
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SMS_STS sms_sts;
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
int TEST_ID = 3;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
sms_sts.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
int ID = sms_sts.Ping(TEST_ID);
|
||||
if(ID!=-1){
|
||||
Serial.print("Servo ID:");
|
||||
Serial.println(ID, DEC);
|
||||
delay(100);
|
||||
}else{
|
||||
Serial.println("Ping servo ID error!");
|
||||
delay(2000);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
/*
|
||||
example for changing ID.
|
||||
*/
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SMS_STS sms_sts;
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
int ID_ChangeFrom = 1;
|
||||
int ID_Changeto = 2;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
sms_sts.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
sms_sts.unLockEprom(1);//unlock EPROM-SAFE
|
||||
sms_sts.writeByte(ID_ChangeFrom, SMS_STS_ID, ID_Changeto);//ID
|
||||
sms_sts.LockEprom(ID_Changeto);//EPROM-SAFE locked
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
/*
|
||||
The RegWritePos example passed the test in ST3215 Servo,
|
||||
and if testing other models of ST series servos
|
||||
please change the appropriate position, speed and delay parameters.
|
||||
*/
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SMS_STS st;
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
st.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
st.RegWritePosEx(1, 4095, 3400, 50);//servo(ID1) speed=3400，acc=50，move to position=4095.
|
||||
st.RegWritePosEx(2, 4095, 3400, 50);//servo(ID2) speed=3400，acc=50，move to position=4095.
|
||||
st.RegWriteAction();
|
||||
delay(3000);
|
||||
|
||||
st.RegWritePosEx(1, 0, 3400, 50);//servo(ID1) speed=3400，acc=50，move to position=0.
|
||||
st.RegWritePosEx(2, 0, 3400, 50);//servo(ID2) speed=3400，acc=50，move to position=0.
|
||||
st.RegWriteAction();
|
||||
delay(3000);
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
/*
|
||||
The SyncWritePos example passed the test in ST3215 Servo,
|
||||
and if testing other models of ST series servos
|
||||
please change the appropriate position, speed and delay parameters.
|
||||
*/
|
||||
|
||||
#include <SCServo.h>
|
||||
SMS_STS st;
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
byte ID[2];
|
||||
s16 Position[2];
|
||||
u16 Speed[2];
|
||||
byte ACC[2];
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
st.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
ID[0] = 1;
|
||||
ID[1] = 2;
|
||||
Speed[0] = 3400;
|
||||
Speed[1] = 3400;
|
||||
ACC[0] = 50;
|
||||
ACC[1] = 50;
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
Position[0] = 3000;
|
||||
Position[1] = 3000;
|
||||
st.SyncWritePosEx(ID, 2, Position, Speed, ACC);//servo(ID1/ID2) speed=3400，acc=50，move to position=3000.
|
||||
delay(2000);
|
||||
|
||||
Position[0] = 100;
|
||||
Position[1] = 100;
|
||||
st.SyncWritePosEx(ID, 2, Position, Speed, ACC);//servo(ID1/ID2) speed=3400，acc=50，move to position=100.
|
||||
delay(2000);
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
The normal write example passed the test in ST3215 Servo,
|
||||
and if testing other models of ST series servos
|
||||
please change the appropriate position, speed and delay parameters.
|
||||
*/
|
||||
|
||||
#include <SCServo.h>
|
||||
|
||||
SMS_STS st;
|
||||
|
||||
// the uart used to control servos.
|
||||
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
|
||||
#define S_RXD 18
|
||||
#define S_TXD 19
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
|
||||
st.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
st.WritePosEx(1, 4095, 3400, 50);//servo(ID1) speed=3400，acc=50，move to position=4095.
|
||||
delay(2000);
|
||||
|
||||
st.WritePosEx(1, 2000, 1500, 50);//servo(ID1) speed=3400，acc=50，move to position=2000.
|
||||
delay(2000);
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
name=SCSservo
|
||||
version=1.0.1
|
||||
author=FT&WS
|
||||
maintainer=FT&WS
|
||||
sentence=SCSservo ctrl for ESP32 and Arduino
|
||||
paragraph=SCSservo ctrl for ESP32 and Arduino
|
||||
category=Other
|
||||
url=https://github.com/waveshare/
|
||||
architectures=*
|
||||
Reference in new issue
Block a user