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

Tree @7c5a15e (Download .tar.gz)

plan.nim @7c5a15eraw · history · blame

import basic2d
import hal
import machine
import math
import position

type
  MotionPlanner* = object of RootObj
    current: WallPosition
    hal: Hal

proc initMotionPlanner*(hal: Hal, start: WallPosition): MotionPlanner =
  return MotionPlanner(current: start.update(hal.current), hal: hal)

proc position*(mp: MotionPlanner): WallPosition = mp.current

proc enable*(mp: var MotionPlanner, left, right: bool) =
  mp.hal.enable(left, right)

proc relativeMove*(mp: var MotionPlanner, xy: Vector2d, vel: float) =
  let dist = xy.len
  let steps = math.ceil(dist / 0.10)
  let start = mp.current.rpos
  let target = mp.current.rpos + xy
  let time = (mp.current.rpos - target).len / vel / steps

  for i in 1..int(steps):
    let t = float(i) / steps
    let p = start * (1 - t) + target * t
    let r = vector2d(len(mp.current.pl - p), len(mp.current.pr - p))
    let m = radiusInterpolate(mp.current, r, time, mp.hal)
    let ms = mp.hal.sendMotion(m)
    mp.current = mp.current.update(ms)