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
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
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)
}
|
