FTServo_Arduino/src/SCS.h at main · spaceactech/FTServo_Arduino · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
/*
 * SCS.h
 * Feetech serial servo communication layer protocol program
 * Date: 2024.11.21
 * Author: 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);             // Standard write instruction
  int  regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen);             // Asynchronous write instruction
  int  RegWriteAction(u8 ID = 0xfe);                               // Asynchronous write execute instruction
  void syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen);  // Sync write instruction
  int  writeByte(u8 ID, u8 MemAddr, u8 bDat);                      // Write 1 byte
  int  writeWord(u8 ID, u8 MemAddr, u16 wDat);                     // Write 2 bytes
  int  Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen);                // Read instruction
  int  readByte(u8 ID, u8 MemAddr);                                // Read 1 byte
  int  readWord(u8 ID, u8 MemAddr);                                // Read 2 bytes
  int  Ping(u8 ID);                                                // Ping instruction
  int  syncReadPacketTx(u8 ID[], u8 IDN, u8 MemAddr, u8 nLen);     // Sync read packet transmit
  int  syncReadPacketRx(u8 ID, u8 *nDat);                          // Decode sync read return packet, returns byte count on success, 0 on failure
  int  syncReadRxPacketToByte();                                   // Decode one byte
  int  syncReadRxPacketToWrod(u8 negBit = 0);                      // Decode two bytes, negBit is direction bit, negBit=0 means no direction
  void syncReadBegin(u8 IDN, u8 rxLen, u32 TimeOut);               // Begin sync read
  void syncReadEnd();                                              // End sync read
  int  Reset(u8 ID);                                               // Reset servo state
  int  Recal(u8 ID);                                               // Reset servo center position
  u8   getState() { return u8Status; }
  u8   getLastError() { return u8Error; }

public:
  u8  Level;     // Servo response level
  u8  End;       // Processor endianness
  u8  u8Status;  // Servo status
  u8  u8Error;   // Communication status
  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);  // Split one 16-bit value into two 8-bit values
  u16  SCS2Host(u8 DataL, u8 DataH);              // Combine two 8-bit values into one 16-bit value
  int  Ack(u8 ID);                                // Return acknowledgement
  int  checkHead();                               // Frame header detection
};
#endif