all lookahead passes for saturn

hunks/adhoc/saturn.js

@@ -18,10 +18,17 @@ import {
   deg
 } from '../../libs/smallvectors.js'
 
-let JD = (position, speed, accel, moves) => {
-  //console.log('moves', moves)
-  let deviation = 0.1 // virtual junction size
-  let minSpeed = 1
+/*
+
+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)
@@ -38,21 +45,71 @@ let JD = (position, speed, accel, moves) => {
     return v
   }
   // the ops,
-  let endSpeeds = [speed] // start speed is the current speed
-  for(let m = 0; m < moves.length; m ++){
-    if(m === 0) continue // noop for start: this is our current speed
-    if(m === moves.length - 1) continue // noop for last move, nothing to junction into
-    let jd = calcJunctionSpeed(moves[m - 1], moves[m], moves[m+1], deviation, accel)
-    endSpeeds.push(jd)
+  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,
-  endSpeeds.push(0)
+  speeds.push(0)
   // walk for minspeeds
-  for(let s in endSpeeds){
-    if(endSpeeds[s] < minSpeed) endSpeeds[s] = minSpeed
+  for (let s in speeds) {
+    if (speeds[s] < minSpeed) speeds[s] = minSpeed
   }
   // that's it for us
-  return endSpeeds
+  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
 }
 
 export default function Saturn() {
@@ -69,25 +126,31 @@ export default function Saturn() {
     return (!outx.io() && !outy.io() && !outz.io())
   }
 
-  // our path (and num segs to plan over)
-  let pathBufferSize = 64
-  let path = []
+  // 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 // (units/s)
-  let lt = [] // last transform (since we know time, this is our speed data)
-  let position = [0, 0, 0] // we track position...
-  let speed = 0
+  let feed = 10 // target, (units/s)
+  let speed = minSpeed // currently
   let posUpdated = false
 
+  /*
     let calculateNextIncrement = () => {
-    // for now, straightforward: current position w/r/t path,
+      // 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 path,
+      // 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, path[0])
+      let dist = vDist(position, positions[0])
       vect.forEach((axis, i) => {
-      vect[i] = (path[0][i] - position[i]) / dist
+        vect[i] = (positions[0][i] - position[i]) / dist
       })
       // easy increment is just this vector * rate / period
       let timeSeg = [0, 0, 0]
@@ -96,33 +159,59 @@ export default function Saturn() {
       })
       // now, is this a step-over ? some fast-slow-math for this:
       let np = vSum(position, timeSeg)
-    if (vDist(np, position) > vDist(position, path[0])) {
+      if (vDist(np, position) > vDist(position, positions[0])) {
         // ts is just to get us exactly to it ..
         timeSeg.forEach((axis, i) => {
-        timeSeg[i] = path[0][i] - position[i]
+          timeSeg[i] = positions[0][i] - position[i]
         })
-      path.shift()
+        positions.shift()
       }
       return timeSeg
     }
+  */
 
   this.loop = () => {
-    if (path.length < pathBufferSize && inpts.io()) {
+    // loading new points,
+    if (positions.length < positionsBufferSize && inpts.io()) {
       let np = inpts.get()
-      console.log('new pt\t', path.length, np)
-      path.push(np)
+      // 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() && path.length > 0) {
+    //if (allclear() && positions.length > 32) {
+    // first we get all move final v's by jd:
     // we jd,
-      if(path.length > 32){
-        console.log('JD', JD(position, speed, 10, path))
+    if (positions.length > 32) {
+      // 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.timeEnd('lookahead')
+      // run once,
       throw new Error('halt')
     }
+    /*
       // this is our major timestep, and should happen only once-every-segment
-      // path length reads @ 26, but NaN ??
-      // do check ... if anything is NaN, print all path, OK. maybe reference problem?
-      console.log(path.length)
-      // we have items in the path,
+      // 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()
@@ -138,5 +227,7 @@ export default function Saturn() {
       outp.put(position)
       posUpdated = false
     }
+    */
+    //}
   }
 }