Initial scaffolding

Signed-off-by: jet tsang zeon-git <zeon-git@jettsang.com>

5 files changed, 228 insertions, 0 deletions

diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..ee29e48
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,22 @@
+The MIT License (MIT)
+
+Copyright (c) 2020 Jet Tsang Zeon-git
+
+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.
+
diff --git a/lobot.go b/lobot.go
new file mode 100644
index 0000000..1b4cd71
--- /dev/null
+++ b/lobot.go
@@ -0,0 +1,7 @@
+// +build !windows
+
+/* SPDX-License-Identifier: MIT
+ *
+ * Copyright (C) 2020 jet tsang zeon-git. All Rights Reserved.
+ */
+package lobot
diff --git a/protocal/checksum.go b/protocal/checksum.go
new file mode 100644
index 0000000..ce096ff
--- /dev/null
+++ b/protocal/checksum.go
@@ -0,0 +1,15 @@
+// +build !windows
+
+/* SPDX-License-Identifier: MIT
+ *
+ * Copyright (C) 2020 jet tsang zeon-git. All Rights Reserved.
+ */
+package protocal
+
+func CheckSum(checksum []byte) uint8 {
+	var sum uint16 = 0
+	for _, v := range checksum {
+		sum += uint16(v)
+	}
+	return (uint8)(^sum)
+}
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)
+}
diff --git a/utils/utils.go b/utils/utils.go
new file mode 100644
index 0000000..4e16ba3
--- /dev/null
+++ b/utils/utils.go
@@ -0,0 +1,43 @@
+package utils
+
+import (
+	"fmt"
+)
+
+// BytesToInt converts a slice of bytes to an int. Only 8- and 16-bit uints are
+// supported, because those are the only thing the control tables contain.
+func BytesToInt(b []byte) (int, error) {
+
+	switch len(b) {
+	case 1:
+		return int(b[0]), nil
+
+	case 2:
+		return int(b[0]) | int(b[1])<<8, nil
+
+	default:
+		return 0, fmt.Errorf("invalid read length %d", len(b))
+
+	}
+}
+
+// BoolToInt converts a bool to an int.
+func BoolToInt(b bool) int {
+	if b {
+		return 1
+	}
+	return 0
+}
+
+// IntToBool converts an int to a bool.
+func IntToBool(v int) bool {
+	return (v != 0)
+}
+
+func Low(i uint16) byte {
+	return byte(i & 0xFF)
+}
+
+func High(i uint16) byte {
+	return Low(i >> 8)
+}