1 files changed, 141 insertions, 0 deletions
diff --git a/protocal/proto.go b/protocal/proto.go
new file mode 100644
index 0000000..05a2d90
--- /dev/null
+++ b/protocal/proto.go
@@ -0,0 +1,141 @@
+// +build !windows
+
+/* SPDX-License-Identifier: MIT
+ *
+ * Copyright (C) 2020 jet tsang zeon-git. All Rights Reserved.
+ */
+package protocal
+
+import (
+	"bytes"
+	"fmt"
+	"io"
+)
+
+const (
+	FrameHeader byte = 0x55
+	// Send an instruction to all servos
+	BroadcastIdent int = 0xFE // 254
+)
+
+type Proto struct {
+	Network io.ReadWriter
+}
+
+func New(network io.ReadWriter) *Proto {
+	return &Proto{
+		Network: network,
+	}
+}
+
+func (p *Proto) writeInstruction(ident int, instruction byte, params []byte) error {
+	buf := new(bytes.Buffer)
+	id := byte(ident & 0xFF)
+	pLen := len(params) + 3
+
+	// +------+------+----+---------------+-------------+--------+-----+--------+----------+
+	// | 0x55 | 0x55 | ID |      LEN      |     INST    | Param1 | ... | ParamN |    CKS   |
+	// +------+------+----+---------------+-------------+--------+-----+--------+----------+
+	// |    Header   | ID | Packet Length | Instruction |       Parameter       | Checksum |
+	// +-------------+----+---------------+-------------+-----------------------+----------+
+
+	buf.Write([]byte{
+		FrameHeader, // Header
+		FrameHeader, // Header
+		id,          // target ID
+		byte(pLen),  // Length
+		instruction, // instruction type (see const section)
+	})
+
+	// append n params
+	buf.Write(params)
+
+	// calculate checksum
+	b := CheckSum(buf.Bytes()[2:]) // Checksum = ~ (ID + Length + INST + Prm1 + ... PrmN)
+	buf.WriteByte(byte(b))
+
+	// write to port
+	_, err := buf.WriteTo(p.Network)
+	if err != nil {
+		return err
+	}
+
+	return nil
+}
+
+func (p *Proto) readPacket(expID int) ([]byte, error) {
+
+	// +------+------+----+---------------+-------------+--------+-----+--------+----------+
+	// | 0x55 | 0x55 | ID |      LEN      |     INST    | Param1 | ... | ParamN |    CKS   |
+	// +------+------+----+---------------+-------------+--------+-----+--------+----------+
+	// |    Header   | ID | Packet Length | Instruction |       Parameter       | Checksum |
+	// +-------------+----+---------------+-------------+-----------------------+----------+
+	// Read the first nine bytes (up to Error), which should always be present.
+
+	buf := make([]byte, 5)
+	n, err := p.Network.Read(buf)
+	if err != nil {
+		return nil, fmt.Errorf("reading packet header: %s", err)
+	}
+	if n != 9 {
+		return nil, fmt.Errorf("reading packet header: expected %d bytes, got %d", 9, n)
+	}
+
+	if buf[0] != FrameHeader || buf[1] != FrameHeader {
+		return nil, fmt.Errorf("bad status packet header: 0x%02X 0x%02X ", buf[0], buf[1])
+	}
+
+	actID := int(buf[2])
+	plen := int(buf[4]) - 3
+
+	// Now read the params, if there are any. We must do this before checking
+	// for errors, to avoid leaving junk in the buffer.
+
+	var pbuf []byte
+	if plen > 0 {
+		pbuf = make([]byte, plen)
+		_, err = p.Network.Read(pbuf)
+		if err != nil {
+			return nil, fmt.Errorf("reading %d params: %s", plen, err)
+		}
+	}
+
+	// Read the checksum, which is always two bytes.
+
+	buf = make([]byte, 1)
+	n, err = p.Network.Read(buf)
+	if err != nil {
+		return nil, fmt.Errorf("reading checksum: %s", err)
+	}
+	if n != 2 {
+		return nil, fmt.Errorf("reading checksum: expected %d bytes, got %d", 2, n)
+	}
+
+	if actID != expID {
+		return nil, fmt.Errorf("expected status packet for %v, but got %v", expID, actID)
+	}
+
+	return pbuf, nil
+}
+
+// ReadData reads a slice of n bytes from the control table of the given servo
+// ID. Use the bytesToInt function to convert the output to something more
+// useful.
+func (p *Proto) ReadData(ident int, instruction byte) ([]byte, error) {
+
+	err := p.writeInstruction(ident, instruction, nil)
+	if err != nil {
+		return []byte{}, err
+	}
+
+	buf, err := p.readPacket(ident)
+	if err != nil {
+		return buf, err
+	}
+
+	return buf, nil
+}
+
+func (p *Proto) WriteData(ident int, instruction byte, data []byte) error {
+	return p.writeInstruction(ident, instruction, data)
+}