forked from foxis/EasyRobot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
servos.go
75 lines (62 loc) · 1.58 KB
/
servos.go
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
package servos
//go:generate protoc -I=./ -I=${GOPATH}/pkg/mod/ -I=${GOPATH}/src --proto_path=../../ --gogofaster_out=./ pb/types.proto
//go:generate go run ../../../../cmd/codegen -i pb/types.pb.go -c ../../proto/proto.json -m re
import (
"errors"
"io"
"github.com/itohio/EasyRobot/pkg/core/options"
"github.com/itohio/EasyRobot/pkg/robot/actuator"
"github.com/itohio/EasyRobot/pkg/robot/actuator/servos/pb"
"github.com/itohio/EasyRobot/pkg/robot/kinematics"
"github.com/itohio/EasyRobot/pkg/robot/transport"
)
const (
// Device ID for robot.transport
ID = 0x00000100
)
type Motor = pb.Motor
type Config = pb.Config
type State = kinematics.State
type servosClient struct {
wr io.Writer
}
var (
errNotSupported = errors.New("not supported")
)
func NewMotorConfig(opts ...options.Option) Motor {
cfg := Motor{
Pin: 0,
Min: -90,
Max: 90,
Default: 0,
Scale: 500 / 90.0,
Offset: 1500,
}
options.ApplyOptions(&cfg, opts...)
return cfg
}
func New(writer io.Writer) actuator.Actuator {
return &servosClient{wr: writer}
}
func (s *servosClient) Configure(opts ...actuator.ConfigureOption) error {
for _, opt := range opts {
dataType, data := opt()
if err := transport.WritePacket(ID, dataType, s.wr, data); err != nil {
return err
}
}
return nil
}
func (s *servosClient) Get() ([]float32, error) {
return nil, errNotSupported
}
func (s *servosClient) Set(params []float32) error {
cfg := State{
Params: params,
}
data, err := cfg.Marshal()
if err != nil {
return err
}
return transport.WritePacket(ID, transport.PacketSetState, s.wr, data)
}