165 lines
3.7 KiB
C++
165 lines
3.7 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);
|
|
}
|