mirror of
https://github.com/m5stack/StackChan.git
synced 2026-04-28 11:27:59 +00:00
Forairaaaaa
370 lines
7.6 KiB
C++
370 lines
7.6 KiB
C++
/*
|
|
* SCS.cpp
|
|
* FIT serial servo communication layer protocol program
|
|
*/
|
|
|
|
#include <stddef.h>
|
|
#include "SCS.h"
|
|
|
|
SCS::SCS()
|
|
{
|
|
Level = 1; // All commands except broadcast commands return responses
|
|
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;
|
|
}
|
|
|
|
// Split a 16-bit number into two 8-bit numbers
|
|
// DataL is the low bit, DataH is the high bit
|
|
void SCS::Host2SCS(u8 *DataL, u8 *DataH, u16 Data)
|
|
{
|
|
if (End) {
|
|
*DataL = (Data >> 8);
|
|
*DataH = (Data & 0xff);
|
|
} else {
|
|
*DataH = (Data >> 8);
|
|
*DataL = (Data & 0xff);
|
|
}
|
|
}
|
|
|
|
// 8-bit numbers are combined into a 16-bit number
|
|
// DataL is the low bit, DataH is the high bit
|
|
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);
|
|
}
|
|
|
|
// Normal write command
|
|
// Servo ID, MemAddr memory table address, write data, write length
|
|
int SCS::genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
|
|
{
|
|
rFlushSCS();
|
|
writeBuf(ID, MemAddr, nDat, nLen, INST_WRITE);
|
|
wFlushSCS();
|
|
return Ack(ID);
|
|
}
|
|
|
|
// Asynchronous write command
|
|
// Servo ID, MemAddr memory table address, write data, write length
|
|
int SCS::regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
|
|
{
|
|
rFlushSCS();
|
|
writeBuf(ID, MemAddr, nDat, nLen, INST_REG_WRITE);
|
|
wFlushSCS();
|
|
return Ack(ID);
|
|
}
|
|
|
|
// Asynchronous write execution command
|
|
// Servo ID
|
|
int SCS::RegWriteAction(u8 ID)
|
|
{
|
|
rFlushSCS();
|
|
writeBuf(ID, 0, NULL, 0, INST_REG_ACTION);
|
|
wFlushSCS();
|
|
return Ack(ID);
|
|
}
|
|
|
|
// Synchronous write command
|
|
// Servo ID[] array, IDN array length, MemAddr memory table address, write data, write length
|
|
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);
|
|
}
|
|
|
|
// Read command
|
|
// Servo ID, MemAddr memory table address, return data nData, data length 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;
|
|
}
|
|
|
|
// Read 1 byte, return -1 if timeout
|
|
int SCS::readByte(u8 ID, u8 MemAddr)
|
|
{
|
|
u8 bDat;
|
|
int Size = Read(ID, MemAddr, &bDat, 1);
|
|
if (Size != 1) {
|
|
return -1;
|
|
} else {
|
|
return bDat;
|
|
}
|
|
}
|
|
|
|
// Read 2 bytes, return -1 if timeout
|
|
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 command, return the servo ID, timeout returns -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;
|
|
}
|