git.haldean.org wallbot / 7c5a15e
create motion planner with linear pathfinding Haldean Brown 4 years ago
5 changed file(s) with 66 addition(s) and 25 deletion(s). Raw diff Collapse all Expand all
1717 hal.current.rdir = right
1818 displayPush(DisplayRequest(kind: UPDATE, newstate: hal.current))
1919
20 proc sendMotion*(hal: var Hal, m: Motion) =
20 proc sendMotion*(hal: var Hal, m: Motion): MachineState =
21 if m.ldir != hal.current.ldir or m.rdir != hal.current.rdir:
22 hal.direction(m.ldir, m.rdir)
2123 let ltime = float(m.ln) / float(m.lhz)
2224 let rtime = float(m.rn) / float(m.rhz)
2325 let time = max(ltime, rtime)
3739 lenable: s.lenable, renable: s.renable)
3840 displayPush(DisplayRequest(kind: UPDATE, newstate: step))
3941 hal.current = step
42 return hal.current
2424 lhz*: uint32
2525 rn*: uint32
2626 rhz*: uint32
27 ldir*: Dir
28 rdir*: Dir
2729
2830 proc writeformat*(o: var Writer, x: Dir, fmt: Format) =
2931 writeformat(o, $x, fmt)
22 import hal
33 import machine
44 import math
5 import plan
56 import position
67
7 let wpStart = WallPosition(
8 rl: 4, rr: 4, rl_zero: 4, rr_zero: 4, rpos: vector2d(0, 0),
9 pl: vector2d(-4.0, 5.0), pr: vector2d(2.0, 4.0))
10 displayStart(wpStart)
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)))
1113
12 var s = initHal(stepdeg = 0.42)
13 s.enable(left = true, right = true)
14 s.direction(left = CW, right = CCW)
15 s.sendMotion(Motion(ln: 10000, lhz: 1000, rn: 5000, rhz: 500))
16 s.enable(left = true, right = false)
14 displayStart(mp.position)
15
16 mp.enable(left = true, right = true)
17 mp.relativeMove(vector2d(0, 2), 0.5)
18 mp.relativeMove(vector2d(2, 0), 0.5)
19 mp.relativeMove(vector2d(0, -2), 0.5)
20 mp.relativeMove(vector2d(-2, 0), 0.5)
1721
1822 displayWait()
0 import basic2d
1 import hal
2 import machine
3 import math
4 import position
5
6 type
7 MotionPlanner* = object of RootObj
8 current: WallPosition
9 hal: Hal
10
11 proc initMotionPlanner*(hal: Hal, start: WallPosition): MotionPlanner =
12 return MotionPlanner(current: start.update(hal.current), hal: hal)
13
14 proc position*(mp: MotionPlanner): WallPosition = mp.current
15
16 proc enable*(mp: var MotionPlanner, left, right: bool) =
17 mp.hal.enable(left, right)
18
19 proc relativeMove*(mp: var MotionPlanner, xy: Vector2d, vel: float) =
20 let dist = xy.len
21 let steps = math.ceil(dist / 0.10)
22 let start = mp.current.rpos
23 let target = mp.current.rpos + xy
24 let time = (mp.current.rpos - target).len / vel / steps
25
26 for i in 1..int(steps):
27 let t = float(i) / steps
28 let p = start * (1 - t) + target * t
29 let r = vector2d(len(mp.current.pl - p), len(mp.current.pr - p))
30 let m = radiusInterpolate(mp.current, r, time, mp.hal)
31 let ms = mp.hal.sendMotion(m)
32 mp.current = mp.current.update(ms)
3535 (f, df, ddf) = evalRpos(res)
3636 return res
3737
38 proc evalRadii(wp: WallPosition, target: Vec, dR: Vec): tuple[f: float64, df: Vec, ddf: float64] =
39 let rl = wp.rl + dR.x
40 let rr = wp.rr + dR.y
41
42 proc linearMove*(wp: WallPosition, xy: Vec, vel: float, hal: Hal): Motion =
43 let target = wp.rpos + xy
44 var dR = vector2d(0, 0)
45 var (f, df, ddf) = evalRadii(wp, target, dR)
46 while f > 1e-7:
47 dR = dR - df / ddf
48 var (f, df, ddf) = evalRadii(wp, target, dR)
49 let time = xy.len / vel
38 proc radiusInterpolate*(wp: WallPosition, targetR: Vec, time: float, hal: Hal): Motion =
39 var dR = targetR - vector2d(wp.rl, wp.rr)
5040 # number of circumferences = delta-radius / circumference
5141 # number of ticks = number of circumferences * number of ticks per circumference
5242 let ticks = dR * (1.0/ gearCircumference * (2 * math.PI / hal.steprads))
53 let lhz = ticks.x / time
54 let rhz = ticks.y / time
55 return Motion(ln: ticks.x, lhz: lhz, rn: ticks.y, rhz: rhz)
43 let ldir = if ticks.x > 0: CCW else: CW
44 let rdir = if ticks.y > 0: CCW else: CW
45 let lhz = uint32(abs(ticks.x / time))
46 let rhz = uint32(abs(ticks.y / time))
47 return Motion(
48 ln: uint32(abs(ticks.x)),
49 lhz: lhz,
50 ldir: ldir,
51 rn: uint32(abs(ticks.y)),
52 rhz: rhz,
53 rdir: rdir,
54 )