-
Notifications
You must be signed in to change notification settings - Fork 7
/
hexapod.go
215 lines (175 loc) · 5.78 KB
/
hexapod.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
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
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
package hexapod
import (
"fmt"
"net/http"
"time"
"github.com/Sirupsen/logrus"
"github.com/adammck/dynamixel/iface"
"github.com/adammck/dynamixel/network"
proto1 "github.com/adammck/dynamixel/protocol/v1"
"github.com/adammck/hexapod/math3d"
"github.com/adammck/hexapod/utils"
)
type State struct {
// The approximate number of frames per second which the main loop is
// currently running at. This can vary quite a bit depending on the load.
FPS int
// Components can set this to true to indicate that the hex should shut
// down. Components which need to clean up before being terminated (e.g.
// powering off servos) should check this value frequently.
Shutdown bool
// The actual pose at the origin, in the world coordinate space. This should
// be updated as accurately as possible as the hex walks around.
Pose math3d.Pose
// The offset from the actual home position which the feet should be
// positioned at.
Offset math3d.Vector3
// The target pose of the origin, in the world space. This can be set to
// instruct the legs to walk towards an arbitrary point, and the chassis to
// orient itself strangely.
Target math3d.Pose
// The point to aim the head (camera) at, in the world space. This is a
// pointer so it can be set to nil if there is no target.
LookAt *math3d.Vector3
// The index of the gait which should be used, mod however many gaits are
// available. (This doesn't really belong here, but is the simplest way to
// pass the selection from the controller to the chassis and I am lazy.)
GaitIndex int
// The increase (or decrease, if negative) from the default speed at which
// we should walk. There is no unit; more is just faster.
Speed int
}
// World returns a matrix to transform a vector in the coordinate space defined
// by the Position and Rotation attributes into the world space.
// TODO: Remove this method.
func (s *State) World() math3d.Matrix44 {
return s.Pose.Add(math3d.Pose{s.Offset, 0, 0, 0}).ToWorld()
}
// Local returns a matrix to transform a vector in the world coordinate space
// into the space defined by the state (using the Position and Rotation attrs).
// TODO: Remove this method.
func (s *State) Local() math3d.Matrix44 {
return s.Pose.Add(math3d.Pose{s.Offset, 0, 0, 0}).ToLocal()
}
type Hexapod struct {
Network *network.Network // TODO: Make this a io.ReadWriter
Components []Component
// Keep an unbound (i.e. having no particular servo ID) protocol for each
// version present on the Dynamixel network. This is just v1, so long as
// we're only using AX-12s.
Protocols []iface.Protocol
// Most components receive (and update) the state every tick, to instruct or
// react to instructions. This is more easily testable than passing around
// references to the Hexapod itself.
State *State
// The FPS which the main loop should try to run at.
TargetFPS int
// To count the number of times that Tick is called each second.
fc *utils.FrameCounter
// The time at which an FPS warning was last logged. To avoid flooding the
// logs if we're running too slowly.
prevWarnFPS time.Time
}
type Component interface {
Boot() error
Tick(time.Time, *State) error
}
// NewHexapod creates a new Hexapod object on the given Dynamixel network.
func NewHexapod(network *network.Network, targetFPS int) *Hexapod {
return &Hexapod{
Network: network,
Components: []Component{},
Protocols: []iface.Protocol{
proto1.New(network),
},
State: &State{
FPS: 0,
Pose: math3d.Pose{
Position: math3d.ZeroVector3,
Heading: 0,
},
Offset: math3d.Vector3{0, 0, 0},
Target: math3d.Pose{
Position: math3d.ZeroVector3,
Heading: 0,
},
LookAt: nil,
GaitIndex: 0,
Speed: 0,
},
TargetFPS: targetFPS,
fc: utils.NewFrameCounter(time.Second),
}
}
// Add registers a component to receive ticks every frame.
func (h *Hexapod) Add(c Component) {
h.Components = append(h.Components, c)
}
// Boot calls Boot on each component.
func (h *Hexapod) Boot() error {
for _, c := range h.Components {
err := c.Boot()
if err != nil {
return err
}
}
// Trigger any buffered instructions written during boot.
h.ActionInstruction()
return nil
}
var log = logrus.WithFields(logrus.Fields{
"pkg": "hex",
})
// Tick calls Tick on each component, then sends the ACTION instruction to
// trigger any buffered instructions.
func (h *Hexapod) Tick(now time.Time) error {
// Lock the network during tick. Any other goroutines wanting to hit the
// network (e.g. legs.waitForReady) must first acquire the lock.
h.Network.Lock()
defer h.Network.Unlock()
// Update the fps counter.
h.fc.Frame(now)
h.State.FPS = h.fc.Count()
// Send Tick to every component.
for _, c := range h.Components {
err := c.Tick(now, h.State)
if err != nil {
return fmt.Errorf("%T.Tick returned error: %v", c, err)
}
}
if h.State.FPS < h.TargetFPS {
if now.Sub(h.prevWarnFPS) > 5*time.Second {
log.Warnf("fps=%d, target=%d", h.State.FPS, h.TargetFPS)
h.prevWarnFPS = now
}
}
// Trigger any buffered instructions written during this tick.
h.ActionInstruction()
return nil
}
func (h *Hexapod) ActionInstruction() error {
for i := range h.Protocols {
err := h.Protocols[i].Action()
if err != nil {
return err
}
}
return nil
}
// TODO: Move this stuff to a separate package.
var log2 = logrus.WithFields(logrus.Fields{
"pkg": "http",
})
// Remote starts an HTTP server which can update the configuration. It blocks
// forever, so start it in a goroutine.
func (h *Hexapod) RunServer(port int) {
indexHTML := ""
http.HandleFunc("/", func(w http.ResponseWriter, r *http.Request) {
w.Header().Set("Content-Type", "text/html; charset=utf-8")
fmt.Fprintf(w, "<pre>%s</pre>", indexHTML)
})
addr := fmt.Sprintf(":%d", port)
log2.Infof("listening on %s", addr)
err := http.ListenAndServe(addr, nil)
panic(err)
}