From afde6c3829656022f7dc738870178bf9e48b0119 Mon Sep 17 00:00:00 2001 From: jet tsang zeon-git Date: Wed, 5 Aug 2020 15:38:23 +0800 Subject: Initial scaffolding Signed-off-by: jet tsang zeon-git --- LICENSE | 22 ++++++++ lobot.go | 7 +++ protocal/checksum.go | 15 ++++++ protocal/proto.go | 141 +++++++++++++++++++++++++++++++++++++++++++++++++++ utils/utils.go | 43 ++++++++++++++++ 5 files changed, 228 insertions(+) create mode 100644 LICENSE create mode 100644 lobot.go create mode 100644 protocal/checksum.go create mode 100644 protocal/proto.go create mode 100644 utils/utils.go 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) +} -- cgit v1.2.3