git.haldean.org wallbot / 7c5a15e wb / position.nim
7c5a15e

Tree @7c5a15e (Download .tar.gz)

position.nim @7c5a15e — raw · history · blame 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
import basic2d
import machine
import math

type
  Vec = Vector2d
  WallPosition* = object of RootObj
    rl*: float
    rr*: float
    rl_zero*: float
    rr_zero*: float
    rpos*: Vec
    # Positions of the two anchor points
    pl*: Vec
    pr*: Vec

proc evalRpos(wp: WallPosition): tuple[f: float64, df: Vec, ddf: float64] =
  let lv = wp.pl - wp.rpos
  let rv = wp.pr - wp.rpos
  let lerr = lv.len - wp.rl
  let rerr = rv.len - wp.rr
  let f = pow(lerr, 2) + pow(rerr, 2)
  let df = -lerr * lv / lv.len - rerr * rv / rv.len
  let ddf = lv.len + rv.len / 8
  return (f: f, df: df, ddf: ddf)

proc update*(wp: WallPosition, ms: MachineState): WallPosition =
  let rl = wp.rl_zero + gearCircumference * (ms.lpos / (2 * math.PI))
  let rr = wp.rr_zero + gearCircumference * (ms.rpos / (2 * math.PI))
  var res = WallPosition(
    rl: rl, rr: rr, rl_zero: wp.rl_zero, rr_zero: wp.rr_zero,
    rpos: wp.rpos, pl: wp.pl, pr: wp.pr)
  var (f, df, ddf) = evalRpos(res)
  while f > 1e-7:
    res.rpos = res.rpos - df / ddf
    (f, df, ddf) = evalRpos(res)
  return res

proc radiusInterpolate*(wp: WallPosition, targetR: Vec, time: float, hal: Hal): Motion =
  var dR = targetR - vector2d(wp.rl, wp.rr)
  # number of circumferences = delta-radius / circumference
  # number of ticks = number of circumferences * number of ticks per circumference
  let ticks = dR * (1.0/ gearCircumference * (2 * math.PI / hal.steprads))
  let ldir = if ticks.x > 0: CCW else: CW
  let rdir = if ticks.y > 0: CCW else: CW
  let lhz = uint32(abs(ticks.x / time))
  let rhz = uint32(abs(ticks.y / time))
  return Motion(
    ln: uint32(abs(ticks.x)),
    lhz: lhz,
    ldir: ldir,
    rn: uint32(abs(ticks.y)),
    rhz: rhz,
    rdir: rdir,
    )