move simulator into separate directory
haldean
4 years ago
| 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
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
Binary diff not shown
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 | 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) |