saturn.js

/*

accel planning for networked controllers

*/

import {
  Hunkify,
  Input,
  Output,
  State
} from '../hunks.js'

import {
  vDist,
  vSum,
  vLen,
  vUnitBetween,
  vScalar,
  deg
} from '../../libs/smallvectors.js'

/*

indexing:
we should have a positions[n] of positions to get to,
and speeds[n] of speeds to be-at-when-there
p[0] and s[0] are always current state ... when we len > 1 we have werk 2 do

*/

let JD = (positions, speeds, deviation, accel, minSpeed) => {
  //console.log('positions', positions)
  let calcJunctionSpeed = (p0, p1, p2, jd, a) => {
    // junction speed at p1, arrival from p0 exit to p2
    let v0 = math.subtract(p1, p0)
    let v1 = math.subtract(p2, p1)
    //console.log('for\n', v0, v1)
    let dotprod = math.dot(v0, v1) / (vLen(v0) * vLen(v1))
    //console.log('dotprod', dotprod)
    let omega = Math.PI - Math.acos(dotprod)
    //console.log('angle between', deg(omega))
    let r = jd / (1 / Math.cos(Math.PI / 2 - omega / 2) - 1)
    //console.log('rad', r)
    let v = Math.sqrt(accel * r)
    //console.log('permissible', v)
    return v
  }
  // the ops,
  for (let m = 0; m < positions.length; m++) {
    if (m === 0) continue // noop for start: this is our current speed, should already be in speeds arr
    if (m === positions.length - 1) continue // noop for last move, nothing to junction into, exit should be minspeed
    let jd = calcJunctionSpeed(positions[m - 1], positions[m], positions[m + 1], deviation, accel)
    if(Number.isNaN(jd)){
      console.log(`NaN for ${m}`)
    }
    speeds.push(jd)
  }
  // finish at zero,
  speeds.push(0)
  // walk for minspeeds
  for (let s in speeds) {
    if (speeds[s] < minSpeed) speeds[s] = minSpeed
  }
  // that's it for us
  return speeds
}

let ReversePass = (positions, speeds, accel, minSpeed) => {
  // link, walking back from last
  let debug = false
  // this makes sure we can completely decelerate, through moves, to the last point at zero
  for (let i = positions.length - 2; i > 0; i--) {
    if(debug) console.log(`reverse pass for ${i}\n`, positions[i], positions[i + 1])
    if(debug) console.log(`current entrance to calculate is`, speeds[i])
    if(debug) console.log(`the constraining exit is`, speeds[i + 1])
    // given the constraing exit, how fast could we possibly start the block?
    let d = vLen(math.subtract(positions[i + 1], positions[i]))
    let maxEntrance = Math.sqrt(Math.pow(speeds[i + 1], 2) + 2 * accel * d)
    // set the entrance speed to the min of JD or our Max Entrance, but no lower than the minspeed
    let max = Math.max(minSpeed, Math.min(speeds[i], maxEntrance))
    // just for logging
    let temp = speeds[i]
    // stay safe w/ current state at zero
    if(i === 0){
      // only the future can be modified
    } else {
      speeds[i] = max
    }
    if(debug) console.log(`entrance was ${temp}, now ${speeds[i]}`)
  }
}

let ForwardPass = (positions, speeds, accel, minSpeed) => {
  // link, walk forwards: can we accel to these velocities in time?
  let debug = false
  for(let i = 0; i < positions.length - 2; i ++){
    if(debug) console.log(`forwards pass for ${i}\n`, positions[i], positions[i + 1])
    if(debug) console.log(`current exit to calculate is`, speeds[i + 1])
    if(debug) console.log(`the constraining entrance is`, speeds[i])
    let d = vLen(math.subtract(positions[i + 1], positions[i]))
    let maxExit = Math.sqrt(Math.pow(speeds[i], 2) + 2 * accel * d)
    let max = Math.max(minSpeed, Math.min(speeds[i + 1], maxExit))
    let temp = speeds[i + 1]
    if(i === positions.length - 2){
      // tail should always be minspeed, if not, trouble
      if(max > minSpeed) console.warn('trouble halting early')
    } else {
      speeds[i + 1] = max
    }
    if(debug) console.log(`exit was ${temp}, now ${speeds[i + 1]}`)
  }
  // link forwards, now making sure we can accel from our start speed up to the exit
  // here is assuming positions[0] is current position, for which speed is the current velocity
}

let RampPass = (positions, speeds, ramps, a, cruise) => {
  let debug = false
  for(let i = 0; i < positions.length - 2; i ++){
    if(debug) console.log(`ramp pass for ${i}`)
    let pi = positions[i]
    let pf = positions[i + 1]
    let vi = speeds[i]
    let vf = speeds[i+1]
    let d = vLen(math.subtract(positions[i + 1], positions[i]))
    let maxEntry = Math.sqrt(Math.pow(speeds[i + 1], 2) + 2 * a * d)
    let maxExit = Math.sqrt(Math.pow(speeds[i], 2) + 2 * a * d)
    if(debug) console.log(`entrance speed is ${vi}`)
    if(debug) console.log(`exit speed is ${vf}`)
    if(debug) console.log(`d is ${d}, maxEntry ${maxEntry}, maxExit ${maxExit}`)
    // big switch
    if(maxExit <= vf){
      if(debug) console.log(`/`)
      ramps.push({
        vi: vi,
        vf: vf,
        t: (vf-vi) / a,
        pi: pi,
        pf: pf
      })
    } else if (maxEntry <= vi){
      if(debug) console.log('\\')
      ramps.push({
        vi: vi,
        vf: vf,
        t: (vi-vf) / a,
        pi: pi,
        pf: pf
      })
    } else if (vi === cruise && vf === cruise){
      if(debug) console.log('--')
      ramps.push({
        vi: vi,
        vf: vf,
        t: d / vi,
        pi: pi,
        pf: pf
      })
    } else if (vi === cruise) {
      if(debug) console.log('--\\')
      let dcDist = (Math.pow(cruise, 2) - Math.pow(vf, 2)) / (2 * a)
      let pInter = math.add(pf, vScalar(vUnitBetween(pf, pi), dcDist))
      // seg1
      ramps.push({
        vi: vi,
        vf: cruise,
        t: (d - dcDist) / cruise,
        pi: pi,
        pf: pInter
      })
      // seg 2,
      ramps.push({
        vi: cruise,
        vf: vf,
        t: (cruise - vf) / a,
        pi: pInter,
        pf: pf
      })
    } else if (vf === cruise){
      if(debug) console.log('/--')
      let acDist = (Math.pow(cruise, 2) - Math.pow(vi, 2)) / (2 * a)
      let pInter = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist))
      // seg1
      ramps.push({
        vi: vi,
        vf: cruise,
        t: (cruise - vi) / a,
        pi: pi,
        pf: pInter
      })
      // seg2
      ramps.push({
        vi: cruise,
        vf: vf,
        t: (d - acDist) / cruise,
        pi: pInter,
        pf: pf
      })
    } else {
      let dcDist = (Math.pow(cruise, 2) - Math.pow(vf, 2)) / (2 * a)
      let acDist = (Math.pow(cruise, 2) - Math.pow(vi, 2)) / (2 * a)
      if(acDist + dcDist > d){
        if(debug) console.log('/--\\')
        let pa = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist))
        let pb = math.add(pf, vScalar(vUnitBetween(pf, pi), dcDist))
        // 3 segs
        ramps.push({
          vi: vi,
          vf: cruise,
          t: (cruise - vi) / a,
          pi: pi,
          pf: pa
        })
        ramps.push({
          vi: cruise,
          vf: cruise,
          t: (d - acDist - dcDist) / cruise,
          pi: pa,
          pf: pb
        })
        ramps.push({
          vi: cruise,
          vf: vf,
          t: (cruise - vf) / a,
          pi: pb,
          pf: pf
        })
      } else {
        if(debug) console.log('/\\')
        let vPeak = Math.sqrt(((2 * a * d + Math.pow(vi, 2) + Math.pow(vf, 2)) / 2))
        let acDist = (Math.pow(vPeak, 2) - Math.pow(vi, 2)) / (2 * a)
        let pInter = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist))
        ramps.push({
          vi: vi,
          vf: vPeak,
          t: (vPeak - vi) / a,
          pi: pi,
          pf: pInter
        })
        ramps.push({
          vi: vPeak,
          vf: vf,
          t: (vPeak - vf) / a,
          pi: pInter,
          pf: pf
        })
      }
    } // end BIGSWITCH
  }
}

export default function Saturn() {
  Hunkify(this)

  let inpts = this.input('array', 'posn')
  let outx = this.output('number', 'outx')
  let outy = this.output('number', 'outy')
  let outz = this.output('number', 'outz')
  let outp = this.output('array', 'posn')
  let period = 0.050 // his.state('number', 'period', 50) // in ms,

  let allclear = () => {
    return (!outx.io() && !outy.io() && !outz.io())
  }

  // our positions (and num segs to plan over)
  let positionsBufferSize = 64
  let positions = [
    [0, 0, 0]
  ] // should always have p[0] (current) and p[1] (one target) when running, at standstill have p[0] only

  // settings,
  let deviation = 0.1 // virtual radius to junction about
  let accel = 10 // units/s/s
  let minSpeed = 0.333 // conspicuous, to debug for tails (indexing)

  // current states,
  let feed = 10 // target, (units/s)
  let speed = minSpeed // currently
  let posUpdated = false

  /*
    let calculateNextIncrement = () => {
      // for now, straightforward: current position w/r/t positions,
      // increment is this posn -> next target, at rate ...
      // for simplicity, we can assume that we're always going from where we're at to the [1]th point in the positions,
      let vect = [0, 0, 0]
      let dist = vDist(position, positions[0])
      vect.forEach((axis, i) => {
        vect[i] = (positions[0][i] - position[i]) / dist
      })
      // easy increment is just this vector * rate / period
      let timeSeg = [0, 0, 0]
      timeSeg.forEach((axis, i) => {
        timeSeg[i] = vect[i] * (feed / period)
      })
      // now, is this a step-over ? some fast-slow-math for this:
      let np = vSum(position, timeSeg)
      if (vDist(np, position) > vDist(position, positions[0])) {
        // ts is just to get us exactly to it ..
        timeSeg.forEach((axis, i) => {
          timeSeg[i] = positions[0][i] - position[i]
        })
        positions.shift()
      }
      return timeSeg
    }
  */

  this.loop = () => {
    // loading new points,
    if (positions.length < positionsBufferSize && inpts.io()) {
      let np = inpts.get()
      // reject the baddies
      try {
        if(vLen(math.subtract(np, positions[positions.length - 1])) === 0){
          // dunk on 'em
          console.warn('zero length appendage rejected by planner')
        } else {
          positions.push(np) // end of queue
          //console.log('new pt\t', positions.length, np)
        }
      } catch (err) {
        console.warn('error caught at saturn input', err)
      }
    }
    //if (allclear() && positions.length > 32) {
    // first we get all move final v's by jd:
    // we jd,
    if (positions.length > 63) {
      // at the moment, for jd, we'll assume positions[0] is our current position.
      // we should time this...
      console.time('lookahead')
      // we can incorporate this update when we rewrite the loop accordingly
      let speeds = [speed]
      JD(positions, speeds, deviation, accel, minSpeed)
      //console.log('jd writes speeds', speeds)
      //console.log(`have ${speeds.length} speeds and ${positions.length} positions`)
      // now we need to link these together,
      ReversePass(positions, speeds, accel, minSpeed)
      ForwardPass(positions, speeds, accel, minSpeed)
      console.timeLog('lookahead')
      // that's kinda tough (25ms), means we need some double-loop action (can't do this every time segmment)
      // now that we have this, we need to break it into motion packets
      // ah: yes - ok, we can now write this thing that will return a list of positions, speeds that's
      // inside of single-slope segments: i.e. have a start velocity, end velocity, and distance.
      // then we can do another pass through to adjust these times to suit our period. ok.
      let ramps = [] // an arr of objs
      RampPass(positions, speeds, ramps, accel, feed)
      console.timeEnd('lookahead')
      console.log(`have ${ramps.length} ramps for ${positions.length} positions`)
      // run once,
      throw new Error('halt')
    }
    /*
      // this is our major timestep, and should happen only once-every-segment
      // positions length reads @ 26, but NaN ??
      // do check ... if anything is NaN, print all positions, OK. maybe reference problem?
      console.log(positions.length)
      // we have items in the positions,
      // we should calculate an ideal trajectory from our current position and speed
      // probably recalculating the whole thing, and then send that out...
      let ts = calculateNextIncrement()
      //console.log('time seg\t', ts)
      position = vSum(position, ts)
      posUpdated = true
      // send 'em
      outx.put(ts[0])
      outy.put(ts[1])
      outz.put(ts[2])
    }
    if (!outp.io() && posUpdated) {
      outp.put(position)
      posUpdated = false
    }
    */
    //}
  }
}