git.haldean.org wallbot / 35f2b22
move simulator into separate directory haldean 3 years ago
53 changed file(s) with 864 addition(s) and 864 deletion(s). Raw diff Collapse all Expand all
00 **/nimcache
1 wb/main
2 wb/test_position
1 sim/wb/main
2 sim/wb/test_position
libwin/csfml-audio-2.dll less more
Binary diff not shown
libwin/csfml-graphics-2.dll less more
Binary diff not shown
libwin/csfml-network-2.dll less more
Binary diff not shown
libwin/csfml-system-2.dll less more
Binary diff not shown
libwin/csfml-window-2.dll less more
Binary diff not shown
libwin/openal32.dll less more
Binary diff not shown
libwin/sfml-audio-2.dll less more
Binary diff not shown
libwin/sfml-audio-d-2.dll less more
Binary diff not shown
libwin/sfml-graphics-2.dll less more
Binary diff not shown
libwin/sfml-graphics-d-2.dll less more
Binary diff not shown
libwin/sfml-network-2.dll less more
Binary diff not shown
libwin/sfml-network-d-2.dll less more
Binary diff not shown
libwin/sfml-system-2.dll less more
Binary diff not shown
libwin/sfml-system-d-2.dll less more
Binary diff not shown
libwin/sfml-window-2.dll less more
Binary diff not shown
libwin/sfml-window-d-2.dll less more
Binary diff not shown
resources/InputMono-Regular.ttf less more
Binary diff not shown
+0
-9
runwin.bat less more
0 @echo off
1 setlocal EnableDelayedExpansion
2
3 set "LIBWIN=%cd%\libwin"
4 if "!path:%LIBWIN%=!" equ "%path%" (
5 set "PATH=%PATH%;%cd%\libwin"
6 )
7 nim compile --threads:on wb\main
8 wb\main
Binary diff not shown
0 @echo off
1 setlocal EnableDelayedExpansion
2
3 set "LIBWIN=%cd%\libwin"
4 if "!path:%LIBWIN%=!" equ "%path%" (
5 set "PATH=%PATH%;%cd%\libwin"
6 )
7 nim compile --threads:on wb\main
8 wb\main
0 import basic2d
1 import csfml
2 import deques
3 import machine
4 import math
5 import os
6 import position
7 import strfmt
8 import times
9
10 const frameRate* = 30
11
12 let targetFrameTime = 1.0 / float(frameRate)
13 const left = 40
14 const top = 160
15 const toothR = 30
16 const wheelR = 100
17 var w = 1800
18 var h = 900
19
20 let c0 = color(6, 12, 25)
21 let c1 = color(230, 255, 255)
22 let c2 = color(111, 195, 223)
23 let c3 = color(223, 116, 12)
24 let c4 = color(255, 230, 77)
25 let c5 = color(40, 108, 150)
26
27 type
28 DisplayException* = object of Exception
29 DisplayRequestType* = enum
30 UPDATE, QUIT
31 DisplayRequest* = object of RootObj
32 case kind*: DisplayRequestType
33 of UPDATE:
34 newstate*: MachineState
35 of QUIT:
36 discard
37 ControlRequest* = enum
38 UserQuit, Stop, Run, XUp, XDown, YUp, YDown, RLUp, RLDown, RRUp, RRDown,
39 MediaOn, MediaOff, RLOn, RLOff, RROn, RROff
40
41 var displayChannel: Channel[DisplayRequest]
42 var ctrlChannel: Channel[ControlRequest]
43 displayChannel.open(0)
44 ctrlChannel.open(0)
45
46 proc drawWheel(
47 window: RenderWindow, name: string, font: Font, x: int, y: int,
48 enable: bool, dir: Dir, pos: float) =
49
50 const thick = 30
51 var wheel = newCircleShape(wheelR - thick, 100)
52 wheel.origin = vec2(wheelR - thick, wheelR - thick)
53 wheel.outlineColor = if enable: c4 else: c3
54 wheel.outlineThickness = thick
55 wheel.position = vec2(wheelR + x, wheelR + y)
56 wheel.fillColor = Transparent
57 window.draw(wheel)
58
59 var tooth = newCircleShape(toothR)
60 tooth.origin = vec2(toothR, toothR)
61 tooth.fillColor = c0
62 for t in 0..6:
63 let theta = float(t) * math.PI / 3 + pos
64 let dx = int(math.sin(theta) * (wheelR - 0.5 * thick))
65 let dy = int(-math.cos(theta) * (wheelR - 0.5 * thick))
66 tooth.position = vec2(x + wheelR + dx, y + wheelR + dy)
67 window.draw(tooth)
68
69 var title = newText(name, font, 48)
70 title.color = c2
71 title.position = vec2(x + wheelR - 14, y + wheelR - 30)
72 window.draw(title)
73
74 let tableOrigin = vec2(x + 10, y + 2 * wheelR + 10)
75
76 var table = newText(
77 "θ={0:+06.0f} Δ={1:> 3s}".fmt(180 * pos / math.PI, $dir), font, 20)
78 table.color = c2
79 table.position = tableOrigin + vec2(wheelR - 108, 4)
80 window.draw(table)
81
82 proc drawPosition(
83 window: RenderWindow, font: Font, x: int, y: int, w: int, h: int,
84 wp: WallPosition, segment: Deque[Vector2d], trail: Deque[Deque[Vector2d]]) =
85 # Padding on bounding box of scene, in meters
86 const xpad = 1
87 const ypad = 0.5
88
89 var daspect = float(w) / float(h)
90
91 var xmin = min(wp.pl.x, wp.pr.x) - xpad
92 var ymin = -ypad
93 var xmax = max(wp.pl.x, wp.pr.x) + xpad
94 var ymax = max(wp.pl.y, wp.pr.y) + ypad
95 var paspect = (xmax - xmin) / (ymax - ymin)
96
97 if daspect < paspect:
98 ymax = (xmax - xmin) / daspect
99 else:
100 let xrange = ymax * daspect
101 let xcenter = 0.5 * (xmax + xmin)
102 xmin = xcenter - xrange / 2
103 xmax = xcenter + xrange / 2
104 paspect = (xmax - xmin) / (ymax - ymin)
105
106 let ppm = float(w) / (xmax - xmin)
107
108 proc pixelX(m: float): int = return int(float(x) + w / 2 + ppm * m)
109 proc pixelY(m: float): int = return int(float(y + h - 20) - ppm * m)
110 proc pixel(m: Vector2d): Vector2f = return vec2(pixelX(m.x), pixelY(m.y))
111
112 var varr = newVertexArray()
113 varr.append(Vertex(position: vec2(x, y), color: c1))
114 varr.append(Vertex(position: vec2(x + w, y), color: c1))
115 varr.append(Vertex(position: vec2(x + w, y), color: c1))
116 varr.append(Vertex(position: vec2(x + w, y + h), color: c1))
117 varr.append(Vertex(position: vec2(x + w, y + h), color: c1))
118 varr.append(Vertex(position: vec2(x, y + h), color: c1))
119 varr.append(Vertex(position: vec2(x, y + h), color: c1))
120 varr.append(Vertex(position: vec2(x, y), color: c1))
121
122 varr.append(Vertex(position: vec2(x + 1, pixelY(0)), color: c2))
123 varr.append(Vertex(position: vec2(x + w - 1, pixelY(0)), color: c2))
124
125 let xgridlo = int(math.floor(xmin - xpad))
126 let xgridhi = int(math.ceil(xmax + xpad))
127 for xm in xgridlo..xgridhi:
128 let xp = pixelX(float(xm))
129 if xp < x or xp > x + w:
130 continue
131 varr.append(Vertex(position: vec2(xp, y + 1), color: c5))
132 varr.append(Vertex(position: vec2(xp, y + h - 1), color: c5))
133
134 let ygridlo = int(math.floor(ymin - ypad))
135 let ygridhi = int(math.ceil(ymax + ypad))
136 for ym in ygridlo..ygridhi:
137 let yp = pixelY(float(ym))
138 if yp < y or yp > y + h:
139 continue
140 let color = if ym == 0: c2 else: c5
141 varr.append(Vertex(position: vec2(x + 1, yp), color: color))
142 varr.append(Vertex(position: vec2(x + w - 1, yp), color: color))
143
144 for tr in trail:
145 for i in 0..(len(tr) - 2):
146 varr.append(Vertex(position: pixel(tr[i + 0]), color: c4))
147 varr.append(Vertex(position: pixel(tr[i + 1]), color: c4))
148 for i in 0..(len(segment) - 2):
149 varr.append(Vertex(position: pixel(segment[i + 0]), color: c4))
150 varr.append(Vertex(position: pixel(segment[i + 1]), color: c4))
151
152 varr.append(Vertex(position: pixel(wp.rpos), color: c2))
153 varr.append(Vertex(position: pixel(wp.mpos), color: c1))
154 varr.primitiveType = PrimitiveType.Lines
155 window.draw(varr)
156
157 proc rlabel(r: float, pos: Vector2d) =
158 let s = newRectangleShape(size = vec2(60, 24))
159 s.position = pixel(pos) - vec2(32, 12)
160 s.fillColor = c0
161 s.outlineColor = c2
162 s.outlineThickness = 1
163 window.draw(s)
164 let t = newText(r.format("04.3f"), font, 18)
165 t.position = pixel(pos) - vec2(30, 12)
166 t.color = c2
167 window.draw(t)
168 rlabel(wp.rl, 0.5 * (wp.pl + wp.rpos))
169 rlabel(wp.rr, 0.5 * (wp.pr + wp.rpos))
170
171 proc circle(pos: Vector2d, color: Color, labelColor: Color, showLabel = true) =
172 let pc = newCircleShape(10)
173 pc.origin = vec2(10, 10)
174 pc.position = pixel(pos)
175 pc.outlineColor = color
176 pc.outlineThickness = 2
177 pc.fillColor = Transparent
178 window.draw(pc)
179
180 if showLabel:
181 let label = newText("{0:+04.2f}\n{1:+04.2f}".fmt(pos.x, pos.y), font, 18)
182 label.position = pc.position + vec2(10, 0)
183 label.color = labelColor
184 window.draw(label)
185
186 circle(wp.pl, c3, c2)
187 circle(wp.pr, c3, c2)
188 circle(wp.rpos, c2, c1, false)
189 circle(wp.mpos, c4, c1)
190
191 let ccc = newCircleShape(2)
192 ccc.origin = vec2(1, 1)
193 ccc.fillColor = White
194 for p in wp.pivots:
195 ccc.position = pixel(p)
196 window.draw(ccc)
197
198 varr.clear()
199 varr.primitiveType = PrimitiveType.LinesStrip
200 for p in wp.pivots:
201 varr.append(Vertex(position: pixel(p), color: c2))
202 window.draw(varr)
203
204 proc pushKey(key: KeyEvent) =
205 case key.code
206 of KeyCode.Space:
207 ctrlChannel.send(ControlRequest.Stop)
208 of KeyCode.Tilde:
209 ctrlChannel.send(ControlRequest.Run)
210 of KeyCode.Right:
211 ctrlChannel.send(ControlRequest.XUp)
212 of KeyCode.Left:
213 ctrlChannel.send(ControlRequest.XDown)
214 of KeyCode.Up:
215 ctrlChannel.send(ControlRequest.YUp)
216 of KeyCode.Down:
217 ctrlChannel.send(ControlRequest.YDown)
218 of KeyCode.W:
219 ctrlChannel.send(ControlRequest.RLUp)
220 of KeyCode.S:
221 ctrlChannel.send(ControlRequest.RLDown)
222 of KeyCode.E:
223 ctrlChannel.send(ControlRequest.RRUp)
224 of KeyCode.D:
225 ctrlChannel.send(ControlRequest.RRDown)
226 of KeyCode.M:
227 ctrlChannel.send(ControlRequest.MediaOn)
228 of KeyCode.N:
229 ctrlChannel.send(ControlRequest.MediaOff)
230 of KeyCode.Q:
231 ctrlChannel.send(ControlRequest.RLOn)
232 of KeyCode.A:
233 ctrlChannel.send(ControlRequest.RLOff)
234 of KeyCode.R:
235 ctrlChannel.send(ControlRequest.RROn)
236 of KeyCode.F:
237 ctrlChannel.send(ControlRequest.RROff)
238 else:
239 discard
240
241 proc displayRun*(startPos: WallPosition) {.thread.} =
242 var csettings = csfml.contextSettings(antialiasing = 1)
243 let mode = videoMode_getFullscreenModes()[0]
244 let window = csfml.newRenderWindow(
245 mode, "Wallplot Display", WindowStyle.None, csettings)
246 w = mode.width
247 h = mode.height
248
249 let font = newFont("resources/InputMono-Regular.ttf")
250 var header = newText("WALLBOT CONTROLLER", font, 48)
251 header.color = c0
252 header.position = vec2(left, top - 100)
253
254 var wp = startPos
255
256 var current: MachineState
257 block findFirstState:
258 while true:
259 let req = displayChannel.recv()
260 case req.kind
261 of UPDATE:
262 current = req.newstate
263 break findFirstState
264 of QUIT:
265 return
266 wp = wp.update(current)
267
268 # Each element of the trail is a fully-connected polyline. We start a new
269 # polyline every time we lift the pen.
270 var segment = initDeque[Vector2d]()
271 var trail = initDeque[Deque[Vector2d]]()
272
273 let loopStart = epochTime()
274 block mainLoop:
275 while true:
276 let frameStart = epochTime()
277 var event: csfml.Event
278 while window.pollEvent(event):
279 case event.kind
280 of EventType.Closed:
281 break mainLoop
282 of EventType.KeyPressed:
283 pushKey(event.key)
284 else:
285 discard
286
287 let (avail, req) = displayChannel.tryRecv()
288 if avail:
289 case req.kind
290 of QUIT:
291 break
292 of UPDATE:
293 current = req.newstate
294 wp = wp.update(current)
295
296 if wp.media == Active:
297 if segment.len == 0 or segment.peekFirst != wp.mpos:
298 segment.addFirst(wp.mpos)
299 elif segment.len != 0:
300 trail.addFirst(segment)
301 while segment.len != 0:
302 segment.popLast()
303
304 window.clear(c5)
305
306 var inset = newRectangleShape(size = window.size() - vec2(0, top - 40))
307 inset.fillColor = c0
308 inset.position = vec2(0, top - 40)
309 window.draw(inset)
310
311 window.draw(header)
312
313 window.drawWheel(
314 "L", font, left, top, current.lenable, current.ldir, current.lpos)
315 window.drawWheel(
316 "R", font, left, top + 2 * wheelR + 90, current.renable,
317 current.rdir, current.rpos)
318
319 var mediaLabel = newText("MEDIA:", font, 20)
320 mediaLabel.color = c2
321 mediaLabel.position = vec2(left, top + 4 * wheelR + 180)
322 window.draw(mediaLabel)
323
324 var media = newText(if wp.media == Active: "ACTIVE" else: "INACTIVE", font, 20)
325 media.color = if wp.media == Active: c4 else: c3
326 media.position = mediaLabel.position + vec2(if wp.media == Active: 126 else: 100, 0)
327 window.draw(media)
328
329 let posLeft = 2 * left + 2 * wheelR
330 window.drawPosition(
331 font, posLeft, top, w - posLeft - left, h - top - 40, wp,
332 segment, trail)
333
334 let totalTime = epochTime() - loopStart
335 var timeDisplay = newText(totalTime.format("+07.2f"), font, 18)
336 timeDisplay.color = c2
337 timeDisplay.position = vec2(w - 115, h - 32)
338 window.draw(timeDisplay)
339
340 window.display()
341 let frameTime = epochTime() - frameStart
342 if frameTime < targetFrameTime:
343 sleep(int(1000 * (targetFrameTime - frameTime)))
344
345 ctrlChannel.send(ControlRequest.UserQuit)
346 window.close()
347
348 var displayThread: system.Thread[WallPosition]
349 proc displayStart*(wp: WallPosition) =
350 displayThread.createThread(displayRun, wp)
351
352 proc displayPush*(req: DisplayRequest) =
353 displayChannel.send(req)
354
355 proc controlRequest*(): ControlRequest =
356 return ctrlChannel.recv()
357
358 proc displayWait*() =
359 displayThread.joinThread()
360
361 proc displayEnd*() =
362 displayChannel.send(DisplayRequest(kind: QUIT))
363 displayWait()
0 import display
1 import machine
2 import math
3
4 proc initHal*(stepdeg: float): Hal =
5 return Hal(
6 kind: Simulated,
7 steprads: stepdeg / 360 * math.PI,
8 current: MachineState(
9 lpos: 0, rpos: 0, ldir: CCW, rdir: CCW, lenable: false, renable: false,
10 media: Inactive))
11
12 proc enable*(hal: var Hal, left: bool, right: bool) =
13 case hal.kind
14 of Simulated:
15 hal.current.lenable = left
16 hal.current.renable = right
17 displayPush(DisplayRequest(kind: UPDATE, newstate: hal.current))
18 of Actual:
19 discard
20
21 proc direction*(hal: var Hal, left: Dir, right: Dir) =
22 case hal.kind
23 of Simulated:
24 hal.current.ldir = left
25 hal.current.rdir = right
26 displayPush(DisplayRequest(kind: UPDATE, newstate: hal.current))
27 of Actual:
28 discard
29
30 proc media*(hal: var Hal, media: MediaState) =
31 case hal.kind
32 of Simulated:
33 if media != hal.current.media:
34 hal.current.media = media
35 displayPush(DisplayRequest(kind: UPDATE, newstate: hal.current))
36 of Actual:
37 discard
38
39 proc sendMotion*(hal: var Hal, m: Motion): MachineState =
40 if m.ldir != hal.current.ldir or m.rdir != hal.current.rdir:
41 hal.direction(m.ldir, m.rdir)
42 let ltime = float(m.ln) / max(float(m.lhz), 1)
43 let rtime = float(m.rn) / max(float(m.rhz), 1)
44 let time = max(ltime, rtime)
45 let frames = int(ceil(time * frameRate))
46 let s = hal.current
47
48 case hal.kind
49 of Simulated:
50 for frame in 0..frames:
51 let t = frame / frameRate
52 let lticks = if s.lenable: min(m.ln, uint32(t * float(m.lhz))) else: 0
53 let rticks = if s.renable: min(m.rn, uint32(t * float(m.rhz))) else: 0
54 let ldist = (if s.ldir == CCW: 1.0 else: -1.0) * hal.steprads * float(lticks)
55 let rdist = (if s.rdir == CCW: 1.0 else: -1.0) * hal.steprads * float(rticks)
56
57 let step = MachineState(
58 lpos: s.lpos + ldist, rpos: s.rpos + rdist,
59 ldir: s.ldir, rdir: s.rdir,
60 lenable: s.lenable, renable: s.renable, media: s.media)
61 displayPush(DisplayRequest(kind: UPDATE, newstate: step))
62 hal.current = step
63 of Actual:
64 discard
65 return hal.current
0 import math
1 import strfmt
2
3 # The amount of chain that is moved through one full rotation, in meters
4 const gearCircumference* = math.PI * 0.10
5
6 type
7 Dir* = enum
8 CW, CCW
9
10 MediaState* = enum
11 Active
12 Inactive
13
14 MachineState* = object of RootObj
15 lpos*: float
16 rpos*: float
17 ldir*: Dir
18 rdir*: Dir
19 lenable*: bool
20 renable*: bool
21 media*: MediaState
22
23 HalType* = enum
24 Simulated
25 Actual
26
27 Hal* = object of RootObj
28 case kind*: HalType
29 of Simulated:
30 steprads*: float
31 current*: MachineState
32 of Actual:
33 discard
34
35 Motion* = object of RootObj
36 ln*: uint32
37 lhz*: uint32
38 rn*: uint32
39 rhz*: uint32
40 ldir*: Dir
41 rdir*: Dir
42
43 proc writeformat*(o: var Writer, x: Dir, fmt: Format) =
44 writeformat(o, $x, fmt)
45
46 proc `$`*(m: Motion): string =
47 "[left {0.ln}@{0.lhz}hz, right {0.rn}@{0.rhz}hz]".fmt(m)
48 proc `$`*(ms: MachineState): string =
49 return ("[left p={0.lpos} d={0.ldir} e={0.lenable} " &
50 "right p={0.rpos} d={0.rdir} e={0.renable}]").fmt(ms)
0 import basic2d
1 import display
2 import hal
3 import machine
4 import math
5 import plan
6 import position
7
8 var mp = initMotionPlanner(
9 initHal(stepdeg = 0.42),
10 WallPosition(
11 rl: 4, rr: 4, rl_zero: 4, rr_zero: 4, rpos: vector2d(0, 0),
12 pl: vector2d(-4.0, 5.0), pr: vector2d(2.0, 4.0)))
13
14 displayStart(mp.position)
15
16 mp.enable(left = TurnOn, right = TurnOn)
17
18 let step = 0.1
19 let vel = 0.25
20
21 while true:
22 let req = controlRequest()
23 case req
24 of UserQuit:
25 break;
26 of XUp:
27 mp.relR2(vector2d(step, 0), vel)
28 of XDown:
29 mp.relR2(vector2d(-step, 0), vel)
30 of YUp:
31 mp.relR2(vector2d(0, step), vel)
32 of YDown:
33 mp.relR2(vector2d(0, -step), vel)
34 of RLUp:
35 mp.relQ(vector2d(step, 0), step / vel)
36 of RLDown:
37 mp.relQ(vector2d(-step, 0), step / vel)
38 of RRUp:
39 mp.relQ(vector2d(0, step), step / vel)
40 of RRDown:
41 mp.relQ(vector2d(0, -step), step / vel)
42 of MediaOn:
43 mp.media(Active)
44 of MediaOff:
45 mp.media(Inactive)
46 of RLOn:
47 mp.enable(left = TurnOn, right = NoChange)
48 of RLOff:
49 mp.enable(left = TurnOff, right = NoChange)
50 of RROn:
51 mp.enable(left = NoChange, right = TurnOn)
52 of RROff:
53 mp.enable(left = NoChange, right = TurnOff)
54 else:
55 discard
56
57 displayWait()
0 import basic2d
1 import hal
2 import machine
3 import math
4 import position
5
6 type
7 ChangeEnable* = enum
8 TurnOn, TurnOff, NoChange
9 MotionPlanner* = object of RootObj
10 current: WallPosition
11 hal: Hal
12
13 proc initMotionPlanner*(hal: Hal, start: WallPosition): MotionPlanner =
14 return MotionPlanner(current: start.update(hal.current), hal: hal)
15
16 proc position*(mp: MotionPlanner): WallPosition = mp.current
17
18 proc enable*(mp: var MotionPlanner, left, right: ChangeEnable) =
19 let lb = case left
20 of TurnOn: true
21 of TurnOff: false
22 of NoChange: mp.hal.current.lenable
23 let rb = case right
24 of TurnOn: true
25 of TurnOff: false
26 of NoChange: mp.hal.current.renable
27 mp.hal.enable(lb, rb)
28
29 proc media*(mp: var MotionPlanner, state: MediaState) =
30 mp.hal.media(state)
31
32 proc absQ*(mp: var MotionPlanner, r: Vector2d, time: float) =
33 ## An absolute move in configuration space. Expressed against the
34 ## currently-configured zero, so may be a bit dangerous before calibration has
35 ## run. Speed is expressed as the amount of time that the move should take,
36 ## and both chains are moved such that their motions end concurrently.
37 let m = radiusInterpolate(mp.current, r, time, mp.hal)
38 let ms = mp.hal.sendMotion(m)
39 mp.current = mp.current.update(ms)
40
41 proc relQ*(mp: var MotionPlanner, dR: Vector2d, time: float) =
42 ## A relative move in configuration space. Speed is expressed as the amount of
43 ## time that the move should take, and both chains are moved such that their
44 ## motions end concurrently.
45 let targetR = vector2d(mp.current.rl, mp.current.rr) + dR
46 mp.absQ(targetR, time)
47
48 proc relR2*(mp: var MotionPlanner, xy: Vector2d, vel: float) =
49 ## A relative move along a vector in cartesian space
50 let dist = xy.len
51 let steps = math.ceil(dist / 0.10)
52 let start = mp.current.rpos
53 let target = mp.current.rpos + xy
54 let time = (mp.current.rpos - target).len / vel / steps
55
56 for i in 1..int(steps):
57 let t = float(i) / steps
58 let p = start * (1 - t) + target * t
59 let r = vector2d(len(mp.current.pl - p), len(mp.current.pr - p))
60 mp.absQ(r, time)
61
62 proc absR2*(mp: var MotionPlanner, xy: Vector2d, vel: float) =
63 ## An absolute move to a position in cartesian space
64 relR2(mp, xy - mp.current.rpos, vel)
0 import basic2d
1 import basic3d
2 import deques
3 import machine
4 import math
5
6 const chainLinkDist = 0.05 # m
7 const chainLinkMass = 0.000833 # kg
8 const robotMass = 10.0000
9
10 # The distance between the left and right gear centers. TODO: model the way the
11 # chain wraps around the sprockets
12 const robotPivotDist = 0.15 # m
13 const mediaTDist = 0.20 # m
14
15 type
16 Vec = Vector2d
17 WallPosition* = object of RootObj
18 rl*: float
19 rr*: float
20 rl_zero*: float
21 rr_zero*: float
22 # Position of the robot for control purposes: the midpoint between the two
23 # robot pivots
24 rpos*: Vec
25 # The position of the media, as calculated based on rpos and the robot's
26 # orientation
27 mpos*: Vec
28 # Positions of the two anchor points
29 pl*: Vec
30 pr*: Vec
31 media*: MediaState
32 pivots*: seq[Vec]
33
34 proc runLinkSim(wp: var WallPosition) =
35 # There are links and pivots in a chain. Links connect pivots to each other,
36 # and a pivot can either be fixed or free. Because of the fixed points at the
37 # end, there is one more pivot than link.
38 #
39 # link ID 0 1 2 3 4 5 6 7
40 # pivot ID 0 1 2 3 4 5 6 7 8
41 # PL --- L0 --- L1 --- L2 --- BL --- BR --- R1 --- R0 --- PR
42 # ^ ^ ^ ^
43 # | | | |
44 # | | | +- shortened link = lln - 1
45 # | | +- pivot 1
46 # | + link 0
47 # + fixed point (as is PR)
48 #
49 # In this example:
50 # lln is 4
51 # lpn is 3
52 # rln is 3
53 # rpn is 2
54 # ln is 8
55 # pn is 9
56 #
57 # The robot is modelled as a pair of pivots with a custom link distance
58 # between them.
59
60 # The number of links for the left and right chains
61 let lln = int(math.ceil(wp.rl / chainLinkDist)) - 1
62 let rln = int(math.ceil(wp.rr / chainLinkDist)) - 1
63 let lpn = lln - 1
64 let rpn = rln - 1
65
66 # Number of links and pivots. The extra link here is for the link between the
67 # left and right robot pivots.
68 let ln = lln + rln + 1
69 let pn = ln + 1
70
71 # The index of the left robot pivot
72 let rpli = lpn + 1
73 # The index of the right robot pivot
74 let rpri = lpn + 2
75 # The index of the robot link
76 let rli = lln
77
78 # L[i] is the length of the i'th link. Because we might have fractional chain
79 # links let out, not all links that represent chain pivot-to-pivot have the
80 # same length
81 var L: seq[float]
82
83 # The position of each of the pivots
84 var x: seq[Vec]
85
86 newSeq(L, ln)
87 newSeq(x, pn)
88
89 # Create initial guess for chain location based on previous rpos position
90 let guessRobotL = wp.rpos - vector2d(robotPivotDist / 2, 0)
91 let guessRobotR = wp.rpos + vector2d(robotPivotDist / 2, 0)
92 var ld = guessRobotL - wp.pl
93 var rd = guessRobotR - wp.pr
94 ld.normalize()
95 rd.normalize()
96
97 # Endpoints are fixed
98 x[0] = wp.pl
99 x[pn - 1] = wp.pr
100
101 # First lln - 1 links are all a fixed length
102 for i in 0..(lln - 2):
103 L[i] = chainLinkDist
104 # The last link is however long is needed to make up the distance from the robot to
105 # the last chain link that we placed
106 L[lln - 1] = wp.rl - float(lln) * chainLinkDist
107 for i in 1..lpn:
108 x[i] = ld * L[i - 1] + x[i - 1]
109
110 # The next link is the one between the robot pivots
111 L[rli] = robotPivotDist
112 # ...whose initial guess is to the left and right of the previous robot position
113 x[rpli] = guessRobotL
114 x[rpri] = guessRobotR
115
116 L[rli + 1] = wp.rr - float(rln) * chainLinkDist
117 for i in (rli + 2)..(ln - 1):
118 L[i] = chainLinkDist
119 for i in (rpri + 1)..(pn - 2):
120 x[i] = -rd * L[i - 1] + x[i - 1]
121
122 # Okay, now we're ready to start simulating. First step on each iteration is
123 # to calculate the force on each link
124 var F = newSeq[Vec](ln)
125 for i in 1..(ln - 1):
126 let m =
127 if i == rli: robotMass
128 else: chainLinkMass
129 F[i].x = 0
130 F[i].y = -m
131
132 # Copy result into the WallPosition struct
133 setlen(wp.pivots, pn)
134 for i in 0..(pn - 1):
135 wp.pivots[i] = x[i]
136
137 let robotL = x[lpn + 1]
138 let robotR = x[lpn + 2]
139 var edge = robotR - robotL
140 edge.normalize()
141 wp.rpos = robotL + 0.5 * robotPivotDist * edge
142
143 let edge3d = vector3d(edge.x, edge.y, 0)
144 let medge = edge3d.cross(vector3d(0, 0, 1))
145 wp.mpos = wp.rpos + mediaTDist * vector2d(medge.x, medge.y)
146
147 proc evalRpos(wp: WallPosition): tuple[f: float64, df: Vec, ddf: float64] =
148 let lv = wp.pl - wp.rpos
149 let rv = wp.pr - wp.rpos
150 let lerr = lv.len - wp.rl
151 let rerr = rv.len - wp.rr
152 let f = pow(lerr, 2) + pow(rerr, 2)
153 let df = -lerr * lv / lv.len - rerr * rv / rv.len
154 let ddf = lv.len + rv.len / 8
155 return (f: f, df: df, ddf: ddf)
156
157 proc update*(wp: WallPosition, ms: MachineState): WallPosition =
158 let rl = wp.rl_zero + gearCircumference * (ms.lpos / (2 * math.PI))
159 let rr = wp.rr_zero + gearCircumference * (ms.rpos / (2 * math.PI))
160 var res = WallPosition(
161 rl: rl, rr: rr, rl_zero: wp.rl_zero, rr_zero: wp.rr_zero,
162 rpos: wp.rpos, pl: wp.pl, pr: wp.pr, media: ms.media,
163 pivots: @[])
164 var (f, df, ddf) = evalRpos(res)
165 var iter = 10000
166 while f > 1e-7 and iter > 0:
167 res.rpos = res.rpos - df / ddf
168 (f, df, ddf) = evalRpos(res)
169 iter -= 1
170 runLinkSim(res)
171 return res
172
173 proc radiusInterpolate*(wp: WallPosition, targetR: Vec, time: float, hal: Hal): Motion =
174 ## Move to the target radial position in the given amount of time
175 var dR = targetR - vector2d(wp.rl, wp.rr)
176 # number of circumferences = delta-radius / circumference
177 # number of ticks = number of circumferences * number of ticks per circumference
178 let ticks = dR * (1.0/ gearCircumference * (2 * math.PI / hal.steprads))
179 let ldir = if ticks.x > 0: CCW else: CW
180 let rdir = if ticks.y > 0: CCW else: CW
181 let lhz = uint32(abs(ticks.x / time))
182 let rhz = uint32(abs(ticks.y / time))
183 return Motion(
184 ln: uint32(abs(ticks.x)),
185 lhz: lhz,
186 ldir: ldir,
187 rn: uint32(abs(ticks.y)),
188 rhz: rhz,
189 rdir: rdir,
190 )
0 import machine
1 import math
2 import neo
3 import position
4 import unittest
5
6 suite "forward position solver tests":
7 test "initial guess is perfect":
8 let rl = math.sqrt(float64(17))
9 let rr = math.sqrt(float64(34))
10 let initial = WallPosition(
11 rl: rl, rr: rr, rl_zero: rl, rr_zero: rr,
12 rpos: vector(0.0, 1.0),
13 pl: vector(-1.0, 5.0),
14 pr: vector(3.0, 6.0))
15 let result = initial.update(MachineState(
16 lpos: 0, rpos: 0, ldir: CW, rdir: CW, lenable: true, renable: true))
17 check(result.rpos[0] == initial.rpos[0])
18 check(result.rpos[1] == initial.rpos[1])
19
20 test "initial guess is zero":
21 let rl = math.sqrt(float64(17))
22 let rr = math.sqrt(float64(34))
23 let initial = WallPosition(
24 rl: rl, rr: rr, rl_zero: rl, rr_zero: rr,
25 rpos: vector(0.0, 0.0),
26 pl: vector(-1.0, 5.0),
27 pr: vector(3.0, 6.0))
28 let result = initial.update(MachineState(
29 lpos: 0, rpos: 0, ldir: CW, rdir: CW, lenable: true, renable: true))
30 check(abs(result.rpos[0] - 0.0) < 1e-3)
31 check(abs(result.rpos[1] - 1.0) < 1e-3)
32
33 test "initial guess is far away":
34 let rl = 6 * math.sqrt(float64(74))
35 let rr = math.sqrt(float64(797))
36 let initial = WallPosition(
37 rl: rl, rr: rr, rl_zero: rl, rr_zero: rr,
38 rpos: vector(0.0, 0.0),
39 pl: vector(-10.0, 52.0),
40 pr: vector(31.0, 36.0))
41 let result = initial.update(MachineState(
42 lpos: 0, rpos: 0, ldir: CW, rdir: CW, lenable: true, renable: true))
43 check(abs(result.rpos[0] - 20.0) < 1e-3)
44 check(abs(result.rpos[1] - 10.0) < 1e-3)
0 # Package
1
2 version = "0.1.0"
3 author = "haldean"
4 description = "Controller for the wallbot robotic system"
5 license = "GPLv3"
6 bin = @["wb/main"]
7
8 # Dependencies
9
10 requires "nim >= 0.17.2"
11 requires "strfmt >= 0.8.5"
12 requires "csfml >= 2.3.0"
+0
-364
wb/display.nim less more
0 import basic2d
1 import csfml
2 import deques
3 import machine
4 import math
5 import os
6 import position
7 import strfmt
8 import times
9
10 const frameRate* = 30
11
12 let targetFrameTime = 1.0 / float(frameRate)
13 const left = 40
14 const top = 160
15 const toothR = 30
16 const wheelR = 100
17 var w = 1800
18 var h = 900
19
20 let c0 = color(6, 12, 25)
21 let c1 = color(230, 255, 255)
22 let c2 = color(111, 195, 223)
23 let c3 = color(223, 116, 12)
24 let c4 = color(255, 230, 77)
25 let c5 = color(40, 108, 150)
26
27 type
28 DisplayException* = object of Exception
29 DisplayRequestType* = enum
30 UPDATE, QUIT
31 DisplayRequest* = object of RootObj
32 case kind*: DisplayRequestType
33 of UPDATE:
34 newstate*: MachineState
35 of QUIT:
36 discard
37 ControlRequest* = enum
38 UserQuit, Stop, Run, XUp, XDown, YUp, YDown, RLUp, RLDown, RRUp, RRDown,
39 MediaOn, MediaOff, RLOn, RLOff, RROn, RROff
40
41 var displayChannel: Channel[DisplayRequest]
42 var ctrlChannel: Channel[ControlRequest]
43 displayChannel.open(0)
44 ctrlChannel.open(0)
45
46 proc drawWheel(
47 window: RenderWindow, name: string, font: Font, x: int, y: int,
48 enable: bool, dir: Dir, pos: float) =
49
50 const thick = 30
51 var wheel = newCircleShape(wheelR - thick, 100)
52 wheel.origin = vec2(wheelR - thick, wheelR - thick)
53 wheel.outlineColor = if enable: c4 else: c3
54 wheel.outlineThickness = thick
55 wheel.position = vec2(wheelR + x, wheelR + y)
56 wheel.fillColor = Transparent
57 window.draw(wheel)
58
59 var tooth = newCircleShape(toothR)
60 tooth.origin = vec2(toothR, toothR)
61 tooth.fillColor = c0
62 for t in 0..6:
63 let theta = float(t) * math.PI / 3 + pos
64 let dx = int(math.sin(theta) * (wheelR - 0.5 * thick))
65 let dy = int(-math.cos(theta) * (wheelR - 0.5 * thick))
66 tooth.position = vec2(x + wheelR + dx, y + wheelR + dy)
67 window.draw(tooth)
68
69 var title = newText(name, font, 48)
70 title.color = c2
71 title.position = vec2(x + wheelR - 14, y + wheelR - 30)
72 window.draw(title)
73
74 let tableOrigin = vec2(x + 10, y + 2 * wheelR + 10)
75
76 var table = newText(
77 "θ={0:+06.0f} Δ={1:> 3s}".fmt(180 * pos / math.PI, $dir), font, 20)
78 table.color = c2
79 table.position = tableOrigin + vec2(wheelR - 108, 4)
80 window.draw(table)
81
82 proc drawPosition(
83 window: RenderWindow, font: Font, x: int, y: int, w: int, h: int,
84 wp: WallPosition, segment: Deque[Vector2d], trail: Deque[Deque[Vector2d]]) =
85 # Padding on bounding box of scene, in meters
86 const xpad = 1
87 const ypad = 0.5
88
89 var daspect = float(w) / float(h)
90
91 var xmin = min(wp.pl.x, wp.pr.x) - xpad
92 var ymin = -ypad
93 var xmax = max(wp.pl.x, wp.pr.x) + xpad
94 var ymax = max(wp.pl.y, wp.pr.y) + ypad
95 var paspect = (xmax - xmin) / (ymax - ymin)
96
97 if daspect < paspect:
98 ymax = (xmax - xmin) / daspect
99 else:
100 let xrange = ymax * daspect
101 let xcenter = 0.5 * (xmax + xmin)
102 xmin = xcenter - xrange / 2
103 xmax = xcenter + xrange / 2
104 paspect = (xmax - xmin) / (ymax - ymin)
105
106 let ppm = float(w) / (xmax - xmin)
107
108 proc pixelX(m: float): int = return int(float(x) + w / 2 + ppm * m)
109 proc pixelY(m: float): int = return int(float(y + h - 20) - ppm * m)
110 proc pixel(m: Vector2d): Vector2f = return vec2(pixelX(m.x), pixelY(m.y))
111
112 var varr = newVertexArray()
113 varr.append(Vertex(position: vec2(x, y), color: c1))
114 varr.append(Vertex(position: vec2(x + w, y), color: c1))
115 varr.append(Vertex(position: vec2(x + w, y), color: c1))
116 varr.append(Vertex(position: vec2(x + w, y + h), color: c1))
117 varr.append(Vertex(position: vec2(x + w, y + h), color: c1))
118 varr.append(Vertex(position: vec2(x, y + h), color: c1))
119 varr.append(Vertex(position: vec2(x, y + h), color: c1))
120 varr.append(Vertex(position: vec2(x, y), color: c1))
121
122 varr.append(Vertex(position: vec2(x + 1, pixelY(0)), color: c2))
123 varr.append(Vertex(position: vec2(x + w - 1, pixelY(0)), color: c2))
124
125 let xgridlo = int(math.floor(xmin - xpad))
126 let xgridhi = int(math.ceil(xmax + xpad))
127 for xm in xgridlo..xgridhi:
128 let xp = pixelX(float(xm))
129 if xp < x or xp > x + w:
130 continue
131 varr.append(Vertex(position: vec2(xp, y + 1), color: c5))
132 varr.append(Vertex(position: vec2(xp, y + h - 1), color: c5))
133
134 let ygridlo = int(math.floor(ymin - ypad))
135 let ygridhi = int(math.ceil(ymax + ypad))
136 for ym in ygridlo..ygridhi:
137 let yp = pixelY(float(ym))
138 if yp < y or yp > y + h:
139 continue
140 let color = if ym == 0: c2 else: c5
141 varr.append(Vertex(position: vec2(x + 1, yp), color: color))
142 varr.append(Vertex(position: vec2(x + w - 1, yp), color: color))
143
144 for tr in trail:
145 for i in 0..(len(tr) - 2):
146 varr.append(Vertex(position: pixel(tr[i + 0]), color: c4))
147 varr.append(Vertex(position: pixel(tr[i + 1]), color: c4))
148 for i in 0..(len(segment) - 2):
149 varr.append(Vertex(position: pixel(segment[i + 0]), color: c4))
150 varr.append(Vertex(position: pixel(segment[i + 1]), color: c4))
151
152 varr.append(Vertex(position: pixel(wp.rpos), color: c2))
153 varr.append(Vertex(position: pixel(wp.mpos), color: c1))
154 varr.primitiveType = PrimitiveType.Lines
155 window.draw(varr)
156
157 proc rlabel(r: float, pos: Vector2d) =
158 let s = newRectangleShape(size = vec2(60, 24))
159 s.position = pixel(pos) - vec2(32, 12)
160 s.fillColor = c0
161 s.outlineColor = c2
162 s.outlineThickness = 1
163 window.draw(s)
164 let t = newText(r.format("04.3f"), font, 18)
165 t.position = pixel(pos) - vec2(30, 12)
166 t.color = c2
167 window.draw(t)
168 rlabel(wp.rl, 0.5 * (wp.pl + wp.rpos))
169 rlabel(wp.rr, 0.5 * (wp.pr + wp.rpos))
170
171 proc circle(pos: Vector2d, color: Color, labelColor: Color, showLabel = true) =
172 let pc = newCircleShape(10)
173 pc.origin = vec2(10, 10)
174 pc.position = pixel(pos)
175 pc.outlineColor = color
176 pc.outlineThickness = 2
177 pc.fillColor = Transparent
178 window.draw(pc)
179
180 if showLabel:
181 let label = newText("{0:+04.2f}\n{1:+04.2f}".fmt(pos.x, pos.y), font, 18)
182 label.position = pc.position + vec2(10, 0)
183 label.color = labelColor
184 window.draw(label)
185
186 circle(wp.pl, c3, c2)
187 circle(wp.pr, c3, c2)
188 circle(wp.rpos, c2, c1, false)
189 circle(wp.mpos, c4, c1)
190
191 let ccc = newCircleShape(2)
192 ccc.origin = vec2(1, 1)
193 ccc.fillColor = White
194 for p in wp.pivots:
195 ccc.position = pixel(p)
196 window.draw(ccc)
197
198 varr.clear()
199 varr.primitiveType = PrimitiveType.LinesStrip
200 for p in wp.pivots:
201 varr.append(Vertex(position: pixel(p), color: c2))
202 window.draw(varr)
203
204 proc pushKey(key: KeyEvent) =
205 case key.code
206 of KeyCode.Space:
207 ctrlChannel.send(ControlRequest.Stop)
208 of KeyCode.Tilde:
209 ctrlChannel.send(ControlRequest.Run)
210 of KeyCode.Right:
211 ctrlChannel.send(ControlRequest.XUp)
212 of KeyCode.Left:
213 ctrlChannel.send(ControlRequest.XDown)
214 of KeyCode.Up:
215 ctrlChannel.send(ControlRequest.YUp)
216 of KeyCode.Down:
217 ctrlChannel.send(ControlRequest.YDown)
218 of KeyCode.W:
219 ctrlChannel.send(ControlRequest.RLUp)
220 of KeyCode.S:
221 ctrlChannel.send(ControlRequest.RLDown)
222 of KeyCode.E:
223 ctrlChannel.send(ControlRequest.RRUp)
224 of KeyCode.D:
225 ctrlChannel.send(ControlRequest.RRDown)
226 of KeyCode.M:
227 ctrlChannel.send(ControlRequest.MediaOn)
228 of KeyCode.N:
229 ctrlChannel.send(ControlRequest.MediaOff)
230 of KeyCode.Q:
231 ctrlChannel.send(ControlRequest.RLOn)
232 of KeyCode.A:
233 ctrlChannel.send(ControlRequest.RLOff)
234 of KeyCode.R:
235 ctrlChannel.send(ControlRequest.RROn)
236 of KeyCode.F:
237 ctrlChannel.send(ControlRequest.RROff)
238 else:
239 discard
240
241 proc displayRun*(startPos: WallPosition) {.thread.} =
242 var csettings = csfml.contextSettings(antialiasing = 1)
243 let mode = videoMode_getFullscreenModes()[0]
244 let window = csfml.newRenderWindow(
245 mode, "Wallplot Display", WindowStyle.None, csettings)
246 w = mode.width
247 h = mode.height
248
249 let font = newFont("resources/InputMono-Regular.ttf")
250 var header = newText("WALLBOT CONTROLLER", font, 48)
251 header.color = c0
252 header.position = vec2(left, top - 100)
253
254 var wp = startPos
255
256 var current: MachineState
257 block findFirstState:
258 while true:
259 let req = displayChannel.recv()
260 case req.kind
261 of UPDATE:
262 current = req.newstate
263 break findFirstState
264 of QUIT:
265 return
266 wp = wp.update(current)
267
268 # Each element of the trail is a fully-connected polyline. We start a new
269 # polyline every time we lift the pen.
270 var segment = initDeque[Vector2d]()
271 var trail = initDeque[Deque[Vector2d]]()
272
273 let loopStart = epochTime()
274 block mainLoop:
275 while true:
276 let frameStart = epochTime()
277 var event: csfml.Event
278 while window.pollEvent(event):
279 case event.kind
280 of EventType.Closed:
281 break mainLoop
282 of EventType.KeyPressed:
283 pushKey(event.key)
284 else:
285 discard
286
287 let (avail, req) = displayChannel.tryRecv()
288 if avail:
289 case req.kind
290 of QUIT:
291 break
292 of UPDATE:
293 current = req.newstate
294 wp = wp.update(current)
295
296 if wp.media == Active:
297 if segment.len == 0 or segment.peekFirst != wp.mpos:
298 segment.addFirst(wp.mpos)
299 elif segment.len != 0:
300 trail.addFirst(segment)
301 while segment.len != 0:
302 segment.popLast()
303
304 window.clear(c5)
305
306 var inset = newRectangleShape(size = window.size() - vec2(0, top - 40))
307 inset.fillColor = c0
308 inset.position = vec2(0, top - 40)
309 window.draw(inset)
310
311 window.draw(header)
312
313 window.drawWheel(
314 "L", font, left, top, current.lenable, current.ldir, current.lpos)
315 window.drawWheel(
316 "R", font, left, top + 2 * wheelR + 90, current.renable,
317 current.rdir, current.rpos)
318
319 var mediaLabel = newText("MEDIA:", font, 20)
320 mediaLabel.color = c2
321 mediaLabel.position = vec2(left, top + 4 * wheelR + 180)
322 window.draw(mediaLabel)
323
324 var media = newText(if wp.media == Active: "ACTIVE" else: "INACTIVE", font, 20)
325 media.color = if wp.media == Active: c4 else: c3
326 media.position = mediaLabel.position + vec2(if wp.media == Active: 126 else: 100, 0)
327 window.draw(media)
328
329 let posLeft = 2 * left + 2 * wheelR
330 window.drawPosition(
331 font, posLeft, top, w - posLeft - left, h - top - 40, wp,
332 segment, trail)
333
334 let totalTime = epochTime() - loopStart
335 var timeDisplay = newText(totalTime.format("+07.2f"), font, 18)
336 timeDisplay.color = c2
337 timeDisplay.position = vec2(w - 115, h - 32)
338 window.draw(timeDisplay)
339
340 window.display()
341 let frameTime = epochTime() - frameStart
342 if frameTime < targetFrameTime:
343 sleep(int(1000 * (targetFrameTime - frameTime)))
344
345 ctrlChannel.send(ControlRequest.UserQuit)
346 window.close()
347
348 var displayThread: system.Thread[WallPosition]
349 proc displayStart*(wp: WallPosition) =
350 displayThread.createThread(displayRun, wp)
351
352 proc displayPush*(req: DisplayRequest) =
353 displayChannel.send(req)
354
355 proc controlRequest*(): ControlRequest =
356 return ctrlChannel.recv()
357
358 proc displayWait*() =
359 displayThread.joinThread()
360
361 proc displayEnd*() =
362 displayChannel.send(DisplayRequest(kind: QUIT))
363 displayWait()
+0
-66
wb/hal.nim less more
0 import display
1 import machine
2 import math
3
4 proc initHal*(stepdeg: float): Hal =
5 return Hal(
6 kind: Simulated,
7 steprads: stepdeg / 360 * math.PI,
8 current: MachineState(
9 lpos: 0, rpos: 0, ldir: CCW, rdir: CCW, lenable: false, renable: false,
10 media: Inactive))
11
12 proc enable*(hal: var Hal, left: bool, right: bool) =
13 case hal.kind
14 of Simulated:
15 hal.current.lenable = left
16 hal.current.renable = right
17 displayPush(DisplayRequest(kind: UPDATE, newstate: hal.current))
18 of Actual:
19 discard
20
21 proc direction*(hal: var Hal, left: Dir, right: Dir) =
22 case hal.kind
23 of Simulated:
24 hal.current.ldir = left
25 hal.current.rdir = right
26 displayPush(DisplayRequest(kind: UPDATE, newstate: hal.current))
27 of Actual:
28 discard
29
30 proc media*(hal: var Hal, media: MediaState) =
31 case hal.kind
32 of Simulated:
33 if media != hal.current.media:
34 hal.current.media = media
35 displayPush(DisplayRequest(kind: UPDATE, newstate: hal.current))
36 of Actual:
37 discard
38
39 proc sendMotion*(hal: var Hal, m: Motion): MachineState =
40 if m.ldir != hal.current.ldir or m.rdir != hal.current.rdir:
41 hal.direction(m.ldir, m.rdir)
42 let ltime = float(m.ln) / max(float(m.lhz), 1)
43 let rtime = float(m.rn) / max(float(m.rhz), 1)
44 let time = max(ltime, rtime)
45 let frames = int(ceil(time * frameRate))
46 let s = hal.current
47
48 case hal.kind
49 of Simulated:
50 for frame in 0..frames:
51 let t = frame / frameRate
52 let lticks = if s.lenable: min(m.ln, uint32(t * float(m.lhz))) else: 0
53 let rticks = if s.renable: min(m.rn, uint32(t * float(m.rhz))) else: 0
54 let ldist = (if s.ldir == CCW: 1.0 else: -1.0) * hal.steprads * float(lticks)
55 let rdist = (if s.rdir == CCW: 1.0 else: -1.0) * hal.steprads * float(rticks)
56
57 let step = MachineState(
58 lpos: s.lpos + ldist, rpos: s.rpos + rdist,
59 ldir: s.ldir, rdir: s.rdir,
60 lenable: s.lenable, renable: s.renable, media: s.media)
61 displayPush(DisplayRequest(kind: UPDATE, newstate: step))
62 hal.current = step
63 of Actual:
64 discard
65 return hal.current
+0
-51
wb/machine.nim less more
0 import math
1 import strfmt
2
3 # The amount of chain that is moved through one full rotation, in meters
4 const gearCircumference* = math.PI * 0.10
5
6 type
7 Dir* = enum
8 CW, CCW
9
10 MediaState* = enum
11 Active
12 Inactive
13
14 MachineState* = object of RootObj
15 lpos*: float
16 rpos*: float
17 ldir*: Dir
18 rdir*: Dir
19 lenable*: bool
20 renable*: bool
21 media*: MediaState
22
23 HalType* = enum
24 Simulated
25 Actual
26
27 Hal* = object of RootObj
28 case kind*: HalType
29 of Simulated:
30 steprads*: float
31 current*: MachineState
32 of Actual:
33 discard
34
35 Motion* = object of RootObj
36 ln*: uint32
37 lhz*: uint32
38 rn*: uint32
39 rhz*: uint32
40 ldir*: Dir
41 rdir*: Dir
42
43 proc writeformat*(o: var Writer, x: Dir, fmt: Format) =
44 writeformat(o, $x, fmt)
45
46 proc `$`*(m: Motion): string =
47 "[left {0.ln}@{0.lhz}hz, right {0.rn}@{0.rhz}hz]".fmt(m)
48 proc `$`*(ms: MachineState): string =
49 return ("[left p={0.lpos} d={0.ldir} e={0.lenable} " &
50 "right p={0.rpos} d={0.rdir} e={0.renable}]").fmt(ms)
+0
-58
wb/main.nim less more
0 import basic2d
1 import display
2 import hal
3 import machine
4 import math
5 import plan
6 import position
7
8 var mp = initMotionPlanner(
9 initHal(stepdeg = 0.42),
10 WallPosition(
11 rl: 4, rr: 4, rl_zero: 4, rr_zero: 4, rpos: vector2d(0, 0),
12 pl: vector2d(-4.0, 5.0), pr: vector2d(2.0, 4.0)))
13
14 displayStart(mp.position)
15
16 mp.enable(left = TurnOn, right = TurnOn)
17
18 let step = 0.1
19 let vel = 0.25
20
21 while true:
22 let req = controlRequest()
23 case req
24 of UserQuit:
25 break;
26 of XUp:
27 mp.relR2(vector2d(step, 0), vel)
28 of XDown:
29 mp.relR2(vector2d(-step, 0), vel)
30 of YUp:
31 mp.relR2(vector2d(0, step), vel)
32 of YDown:
33 mp.relR2(vector2d(0, -step), vel)
34 of RLUp:
35 mp.relQ(vector2d(step, 0), step / vel)
36 of RLDown:
37 mp.relQ(vector2d(-step, 0), step / vel)
38 of RRUp:
39 mp.relQ(vector2d(0, step), step / vel)
40 of RRDown:
41 mp.relQ(vector2d(0, -step), step / vel)
42 of MediaOn:
43 mp.media(Active)
44 of MediaOff:
45 mp.media(Inactive)
46 of RLOn:
47 mp.enable(left = TurnOn, right = NoChange)
48 of RLOff:
49 mp.enable(left = TurnOff, right = NoChange)
50 of RROn:
51 mp.enable(left = NoChange, right = TurnOn)
52 of RROff:
53 mp.enable(left = NoChange, right = TurnOff)
54 else:
55 discard
56
57 displayWait()
+0
-65
wb/plan.nim less more
0 import basic2d
1 import hal
2 import machine
3 import math
4 import position
5
6 type
7 ChangeEnable* = enum
8 TurnOn, TurnOff, NoChange
9 MotionPlanner* = object of RootObj
10 current: WallPosition
11 hal: Hal
12
13 proc initMotionPlanner*(hal: Hal, start: WallPosition): MotionPlanner =
14 return MotionPlanner(current: start.update(hal.current), hal: hal)
15
16 proc position*(mp: MotionPlanner): WallPosition = mp.current
17
18 proc enable*(mp: var MotionPlanner, left, right: ChangeEnable) =
19 let lb = case left
20 of TurnOn: true
21 of TurnOff: false
22 of NoChange: mp.hal.current.lenable
23 let rb = case right
24 of TurnOn: true
25 of TurnOff: false
26 of NoChange: mp.hal.current.renable
27 mp.hal.enable(lb, rb)
28
29 proc media*(mp: var MotionPlanner, state: MediaState) =
30 mp.hal.media(state)
31
32 proc absQ*(mp: var MotionPlanner, r: Vector2d, time: float) =
33 ## An absolute move in configuration space. Expressed against the
34 ## currently-configured zero, so may be a bit dangerous before calibration has
35 ## run. Speed is expressed as the amount of time that the move should take,
36 ## and both chains are moved such that their motions end concurrently.
37 let m = radiusInterpolate(mp.current, r, time, mp.hal)
38 let ms = mp.hal.sendMotion(m)
39 mp.current = mp.current.update(ms)
40
41 proc relQ*(mp: var MotionPlanner, dR: Vector2d, time: float) =
42 ## A relative move in configuration space. Speed is expressed as the amount of
43 ## time that the move should take, and both chains are moved such that their
44 ## motions end concurrently.
45 let targetR = vector2d(mp.current.rl, mp.current.rr) + dR
46 mp.absQ(targetR, time)
47
48 proc relR2*(mp: var MotionPlanner, xy: Vector2d, vel: float) =
49 ## A relative move along a vector in cartesian space
50 let dist = xy.len
51 let steps = math.ceil(dist / 0.10)
52 let start = mp.current.rpos
53 let target = mp.current.rpos + xy
54 let time = (mp.current.rpos - target).len / vel / steps
55
56 for i in 1..int(steps):
57 let t = float(i) / steps
58 let p = start * (1 - t) + target * t
59 let r = vector2d(len(mp.current.pl - p), len(mp.current.pr - p))
60 mp.absQ(r, time)
61
62 proc absR2*(mp: var MotionPlanner, xy: Vector2d, vel: float) =
63 ## An absolute move to a position in cartesian space
64 relR2(mp, xy - mp.current.rpos, vel)
+0
-191
wb/position.nim less more
0 import basic2d
1 import basic3d
2 import deques
3 import machine
4 import math
5
6 const chainLinkDist = 0.05 # m
7 const chainLinkMass = 0.000833 # kg
8 const robotMass = 10.0000
9
10 # The distance between the left and right gear centers. TODO: model the way the
11 # chain wraps around the sprockets
12 const robotPivotDist = 0.15 # m
13 const mediaTDist = 0.20 # m
14
15 type
16 Vec = Vector2d
17 WallPosition* = object of RootObj
18 rl*: float
19 rr*: float
20 rl_zero*: float
21 rr_zero*: float
22 # Position of the robot for control purposes: the midpoint between the two
23 # robot pivots
24 rpos*: Vec
25 # The position of the media, as calculated based on rpos and the robot's
26 # orientation
27 mpos*: Vec
28 # Positions of the two anchor points
29 pl*: Vec
30 pr*: Vec
31 media*: MediaState
32 pivots*: seq[Vec]
33
34 proc runLinkSim(wp: var WallPosition) =
35 # There are links and pivots in a chain. Links connect pivots to each other,
36 # and a pivot can either be fixed or free. Because of the fixed points at the
37 # end, there is one more pivot than link.
38 #
39 # link ID 0 1 2 3 4 5 6 7
40 # pivot ID 0 1 2 3 4 5 6 7 8
41 # PL --- L0 --- L1 --- L2 --- BL --- BR --- R1 --- R0 --- PR
42 # ^ ^ ^ ^
43 # | | | |
44 # | | | +- shortened link = lln - 1
45 # | | +- pivot 1
46 # | + link 0
47 # + fixed point (as is PR)
48 #
49 # In this example:
50 # lln is 4
51 # lpn is 3
52 # rln is 3
53 # rpn is 2
54 # ln is 8
55 # pn is 9
56 #
57 # The robot is modelled as a pair of pivots with a custom link distance
58 # between them.
59
60 # The number of links for the left and right chains
61 let lln = int(math.ceil(wp.rl / chainLinkDist)) - 1
62 let rln = int(math.ceil(wp.rr / chainLinkDist)) - 1
63 let lpn = lln - 1
64 let rpn = rln - 1
65
66 # Number of links and pivots. The extra link here is for the link between the
67 # left and right robot pivots.
68 let ln = lln + rln + 1
69 let pn = ln + 1
70
71 # The index of the left robot pivot
72 let rpli = lpn + 1
73 # The index of the right robot pivot
74 let rpri = lpn + 2
75 # The index of the robot link
76 let rli = lln
77
78 # L[i] is the length of the i'th link. Because we might have fractional chain
79 # links let out, not all links that represent chain pivot-to-pivot have the
80 # same length
81 var L: seq[float]
82
83 # The position of each of the pivots
84 var x: seq[Vec]
85
86 newSeq(L, ln)
87 newSeq(x, pn)
88
89 # Create initial guess for chain location based on previous rpos position
90 let guessRobotL = wp.rpos - vector2d(robotPivotDist / 2, 0)
91 let guessRobotR = wp.rpos + vector2d(robotPivotDist / 2, 0)
92 var ld = guessRobotL - wp.pl
93 var rd = guessRobotR - wp.pr
94 ld.normalize()
95 rd.normalize()
96
97 # Endpoints are fixed
98 x[0] = wp.pl
99 x[pn - 1] = wp.pr
100
101 # First lln - 1 links are all a fixed length
102 for i in 0..(lln - 2):
103 L[i] = chainLinkDist
104 # The last link is however long is needed to make up the distance from the robot to
105 # the last chain link that we placed
106 L[lln - 1] = wp.rl - float(lln) * chainLinkDist
107 for i in 1..lpn:
108 x[i] = ld * L[i - 1] + x[i - 1]
109
110 # The next link is the one between the robot pivots
111 L[rli] = robotPivotDist
112 # ...whose initial guess is to the left and right of the previous robot position
113 x[rpli] = guessRobotL
114 x[rpri] = guessRobotR
115
116 L[rli + 1] = wp.rr - float(rln) * chainLinkDist
117 for i in (rli + 2)..(ln - 1):
118 L[i] = chainLinkDist
119 for i in (rpri + 1)..(pn - 2):
120 x[i] = -rd * L[i - 1] + x[i - 1]
121
122 # Okay, now we're ready to start simulating. First step on each iteration is
123 # to calculate the force on each link
124 var F = newSeq[Vec](ln)
125 for i in 1..(ln - 1):
126 let m =
127 if i == rli: robotMass
128 else: chainLinkMass
129 F[i].x = 0
130 F[i].y = -m
131
132 # Copy result into the WallPosition struct
133 setlen(wp.pivots, pn)
134 for i in 0..(pn - 1):
135 wp.pivots[i] = x[i]
136
137 let robotL = x[lpn + 1]
138 let robotR = x[lpn + 2]
139 var edge = robotR - robotL
140 edge.normalize()
141 wp.rpos = robotL + 0.5 * robotPivotDist * edge
142
143 let edge3d = vector3d(edge.x, edge.y, 0)
144 let medge = edge3d.cross(vector3d(0, 0, 1))
145 wp.mpos = wp.rpos + mediaTDist * vector2d(medge.x, medge.y)
146
147 proc evalRpos(wp: WallPosition): tuple[f: float64, df: Vec, ddf: float64] =
148 let lv = wp.pl - wp.rpos
149 let rv = wp.pr - wp.rpos
150 let lerr = lv.len - wp.rl
151 let rerr = rv.len - wp.rr
152 let f = pow(lerr, 2) + pow(rerr, 2)
153 let df = -lerr * lv / lv.len - rerr * rv / rv.len
154 let ddf = lv.len + rv.len / 8
155 return (f: f, df: df, ddf: ddf)
156
157 proc update*(wp: WallPosition, ms: MachineState): WallPosition =
158 let rl = wp.rl_zero + gearCircumference * (ms.lpos / (2 * math.PI))
159 let rr = wp.rr_zero + gearCircumference * (ms.rpos / (2 * math.PI))
160 var res = WallPosition(
161 rl: rl, rr: rr, rl_zero: wp.rl_zero, rr_zero: wp.rr_zero,
162 rpos: wp.rpos, pl: wp.pl, pr: wp.pr, media: ms.media,
163 pivots: @[])
164 var (f, df, ddf) = evalRpos(res)
165 var iter = 10000
166 while f > 1e-7 and iter > 0:
167 res.rpos = res.rpos - df / ddf
168 (f, df, ddf) = evalRpos(res)
169 iter -= 1
170 runLinkSim(res)
171 return res
172
173 proc radiusInterpolate*(wp: WallPosition, targetR: Vec, time: float, hal: Hal): Motion =
174 ## Move to the target radial position in the given amount of time
175 var dR = targetR - vector2d(wp.rl, wp.rr)
176 # number of circumferences = delta-radius / circumference
177 # number of ticks = number of circumferences * number of ticks per circumference
178 let ticks = dR * (1.0/ gearCircumference * (2 * math.PI / hal.steprads))
179 let ldir = if ticks.x > 0: CCW else: CW
180 let rdir = if ticks.y > 0: CCW else: CW
181 let lhz = uint32(abs(ticks.x / time))
182 let rhz = uint32(abs(ticks.y / time))
183 return Motion(
184 ln: uint32(abs(ticks.x)),
185 lhz: lhz,
186 ldir: ldir,
187 rn: uint32(abs(ticks.y)),
188 rhz: rhz,
189 rdir: rdir,
190 )
+0
-45
wb/test_position.nim less more
0 import machine
1 import math
2 import neo
3 import position
4 import unittest
5
6 suite "forward position solver tests":
7 test "initial guess is perfect":
8 let rl = math.sqrt(float64(17))
9 let rr = math.sqrt(float64(34))
10 let initial = WallPosition(
11 rl: rl, rr: rr, rl_zero: rl, rr_zero: rr,
12 rpos: vector(0.0, 1.0),
13 pl: vector(-1.0, 5.0),
14 pr: vector(3.0, 6.0))
15 let result = initial.update(MachineState(
16 lpos: 0, rpos: 0, ldir: CW, rdir: CW, lenable: true, renable: true))
17 check(result.rpos[0] == initial.rpos[0])
18 check(result.rpos[1] == initial.rpos[1])
19
20 test "initial guess is zero":
21 let rl = math.sqrt(float64(17))
22 let rr = math.sqrt(float64(34))
23 let initial = WallPosition(
24 rl: rl, rr: rr, rl_zero: rl, rr_zero: rr,
25 rpos: vector(0.0, 0.0),
26 pl: vector(-1.0, 5.0),
27 pr: vector(3.0, 6.0))
28 let result = initial.update(MachineState(
29 lpos: 0, rpos: 0, ldir: CW, rdir: CW, lenable: true, renable: true))
30 check(abs(result.rpos[0] - 0.0) < 1e-3)
31 check(abs(result.rpos[1] - 1.0) < 1e-3)
32
33 test "initial guess is far away":
34 let rl = 6 * math.sqrt(float64(74))
35 let rr = math.sqrt(float64(797))
36 let initial = WallPosition(
37 rl: rl, rr: rr, rl_zero: rl, rr_zero: rr,
38 rpos: vector(0.0, 0.0),
39 pl: vector(-10.0, 52.0),
40 pr: vector(31.0, 36.0))
41 let result = initial.update(MachineState(
42 lpos: 0, rpos: 0, ldir: CW, rdir: CW, lenable: true, renable: true))
43 check(abs(result.rpos[0] - 20.0) < 1e-3)
44 check(abs(result.rpos[1] - 10.0) < 1e-3)
+0
-13
wb.nimble less more
0 # Package
1
2 version = "0.1.0"
3 author = "haldean"
4 description = "Controller for the wallbot robotic system"
5 license = "GPLv3"
6 bin = @["wb/main"]
7
8 # Dependencies
9
10 requires "nim >= 0.17.2"
11 requires "strfmt >= 0.8.5"
12 requires "csfml >= 2.3.0"