151 lines
3.6 KiB
C++
151 lines
3.6 KiB
C++
// -----------------------------------------------------------------------------
|
|
// Copyright (c) 2024 Mu Shibo
|
|
//
|
|
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
// of this software and associated documentation files (the "Software"), to deal
|
|
// in the Software without restriction, including without limitation the rights
|
|
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
// copies of the Software, and to permit persons to whom the Software is
|
|
// furnished to do so, subject to the following conditions:
|
|
//
|
|
// The above copyright notice and this permission notice shall be included in
|
|
// all copies or substantial portions of the Software.
|
|
//
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
|
// THE SOFTWARE.
|
|
// -----------------------------------------------------------------------------
|
|
|
|
//飞特STS3032舵机同步控制指令
|
|
|
|
#include "Servo_STS3032.h"
|
|
#include <stddef.h>
|
|
|
|
//飞特串行舵机通信层协议程序
|
|
SCS::SCS()
|
|
{
|
|
Level = 1;//除广播指令所有指令返回应答
|
|
Error = 0;
|
|
}
|
|
SCS::SCS(u8 End)
|
|
{
|
|
Level = 1;
|
|
this->End = End;
|
|
Error = 0;
|
|
}
|
|
SCS::SCS(u8 End, u8 Level)
|
|
{
|
|
this->Level = Level;
|
|
this->End = End;
|
|
Error = 0;
|
|
}
|
|
//1个16位数拆分为2个8位数
|
|
//DataL为低位，DataH为高位
|
|
void SCS::Host2SCS(u8 *DataL, u8* DataH, u16 Data)
|
|
{
|
|
if(End){
|
|
*DataL = (Data>>8);
|
|
*DataH = (Data&0xff);
|
|
}else{
|
|
*DataH = (Data>>8);
|
|
*DataL = (Data&0xff);
|
|
}
|
|
}
|
|
//同步写指令
|
|
//舵机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();
|
|
}
|
|
|
|
//飞特串行舵机硬件接口层程序
|
|
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::writeSCS(unsigned char *nDat, int nLen)
|
|
{
|
|
if(nDat==NULL){
|
|
return 0;
|
|
}
|
|
return pSerial->write(nDat, nLen);
|
|
}
|
|
int SCSerial::writeSCS(unsigned char bDat)
|
|
{
|
|
return pSerial->write(&bDat, 1);
|
|
}
|
|
void SCSerial::rFlushSCS()
|
|
{
|
|
while(pSerial->read()!=-1);
|
|
}
|
|
void SCSerial::wFlushSCS()
|
|
{
|
|
}
|
|
|
|
//飞特SMS_STS系列串行舵机应用层程序
|
|
void SMS_STS::SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[])
|
|
{
|
|
u8 offbuf[7*IDN];
|
|
for(u8 i = 0; i<IDN; i++){
|
|
if(Position[i]<0){
|
|
Position[i] = -Position[i];
|
|
Position[i] |= (1<<15);
|
|
}
|
|
u16 V;
|
|
if(Speed){
|
|
V = Speed[i];
|
|
}else{
|
|
V = 0;
|
|
}
|
|
if(ACC){
|
|
offbuf[i*7] = ACC[i];
|
|
}else{
|
|
offbuf[i*7] = 0;
|
|
}
|
|
Host2SCS(offbuf+i*7+1, offbuf+i*7+2, Position[i]);
|
|
Host2SCS(offbuf+i*7+3, offbuf+i*7+4, 0);
|
|
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, V);
|
|
}
|
|
syncWrite(ID, IDN, SMS_STS_ACC, offbuf, 7);
|
|
}
|
|
|