From 2b79ab623163a057b036c93eb6b6ca726d728545 Mon Sep 17 00:00:00 2001
From: Jake Read <jake.read@cba.mit.edu>
Date: Wed, 30 Oct 2019 09:04:17 -0400
Subject: [PATCH] add notes to saturn, some clean, add network check pass to
 revpass

---
 hunks/adhoc/saturn.js | 161 +++++++++++++++++-------------------------
 libs/smallvectors.js  |   2 +-
 scratch/rampcheck.js  |  26 +++++++
 3 files changed, 93 insertions(+), 96 deletions(-)
 create mode 100644 scratch/rampcheck.js

diff --git a/hunks/adhoc/saturn.js b/hunks/adhoc/saturn.js
index 276a52f..22cc0bb 100644
--- a/hunks/adhoc/saturn.js
+++ b/hunks/adhoc/saturn.js
@@ -52,10 +52,10 @@ export default function Saturn() {
   // 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)
+  let minSpeed = 0.01 // conspicuous, to debug for tails (indexing)
 
   // current states,
-  let feed = 10 // target, (units/s)
+  let cruise = 10 // target, (units/s)
   let speed = minSpeed // currently
   let posUpdated = false
 
@@ -91,25 +91,12 @@ export default function Saturn() {
     // walk for minspeeds
     for (let s in speeds) {
       if (speeds[s] < minSpeed) speeds[s] = minSpeed
-      if (speeds[s] > feed) speeds[s] = feed
+      if (speeds[s] > cruise) speeds[s] = cruise
     }
     // that's it for us
     return speeds
   }
 
-  let PeriodPass = (speeds, debug) => {
-    for (let i = positions.length - 2; i > 0; i--) {
-      let d = vLen(math.subtract(positions[i + 1], positions[i]))
-      let sLimit = d / (period * 3)
-      if(debug) console.log('mgp', sLimit)
-      if(sLimit < speeds[i] || sLimit < speeds[i + 1]){
-        speeds[i] = sLimit / 2
-        speeds[i + 1] = sLimit / 2
-        if(debug) console.warn('bad small boy at', i)
-      }
-    }
-  }
-
   let ReversePass = (speeds, debug) => {
     // link, walking back from last
     // this makes sure we can completely decelerate, through moves, to the last point at zero
@@ -117,12 +104,19 @@ export default function Saturn() {
       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?
+
+      // to calcluate the maximum entrance, given our exit, with pure acceleration:
       let d = vLen(math.subtract(positions[i + 1], positions[i]))
       // with given period, how fast can we possibly go in one period? for small moves this is constraining
-      let maxEntrance = Math.sqrt(Math.pow(speeds[i + 1], 2) + 2 * accel * d)
+      let maxEntranceByAccel = Math.sqrt(Math.pow(speeds[i + 1], 2) + 2 * accel * d)
+
+      // to calculate the maximum entrance, given our exit, to make within one period of time:
+      // let t = 2 * d / (speeds[i] + speeds[i + 1]) // current t ...
+      // v2, where t = period
+      let maxEntranceByPeriod = period / (2 * d) - speeds[i]
+
       // 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))
+      let max = Math.max(minSpeed, Math.min(speeds[i], maxEntranceByAccel, maxEntranceByPeriod))
       // just for logging
       let temp = speeds[i]
       // stay safe w/ current state at zero
@@ -164,7 +158,7 @@ export default function Saturn() {
       let pi = positions[i]
       let pf = positions[i + 1]
       let vi = speeds[i]
-      if (vi > feed) console.warn(`vi at ${i} > feed during RampPass`)
+      if (vi > cruise) console.warn(`vi at ${i} > cruise during RampPass`)
       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 * accel * d)
@@ -182,7 +176,7 @@ export default function Saturn() {
           pi: pi,
           pf: pf
         })
-        if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
+        if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
       } else if (maxEntry <= vi) {
         if (debug) console.log('\\')
         ramps.push({
@@ -192,8 +186,8 @@ export default function Saturn() {
           pi: pi,
           pf: pf
         })
-        if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
-      } else if (vi === feed && vf === feed) {
+        if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
+      } else if (vi === cruise && vf === cruise) {
         if (debug) console.log('--')
         ramps.push({
           vi: vi,
@@ -202,54 +196,54 @@ export default function Saturn() {
           pi: pi,
           pf: pf
         })
-        if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
-      } else if (vi === feed) {
+        if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
+      } else if (vi === cruise) {
         if (debug) console.log('--\\')
-        let dcDist = (Math.pow(feed, 2) - Math.pow(vf, 2)) / (2 * accel)
+        let dcDist = (Math.pow(cruise, 2) - Math.pow(vf, 2)) / (2 * accel)
         let pInter = math.add(pf, vScalar(vUnitBetween(pf, pi), dcDist))
         // seg1
         ramps.push({
           vi: vi,
-          vf: feed,
-          t: (d - dcDist) / feed,
+          vf: cruise,
+          t: (d - dcDist) / cruise,
           pi: pi,
           pf: pInter
         })
-        if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
+        if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
         // seg 2,
         ramps.push({
-          vi: feed,
+          vi: cruise,
           vf: vf,
-          t: (feed - vf) / accel,
+          t: (cruise - vf) / accel,
           pi: pInter,
           pf: pf
         })
-        if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
-      } else if (vf === feed) {
+        if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
+      } else if (vf === cruise) {
         if (debug) console.log('/--')
-        let acDist = (Math.pow(feed, 2) - Math.pow(vi, 2)) / (2 * accel)
+        let acDist = (Math.pow(cruise, 2) - Math.pow(vi, 2)) / (2 * accel)
         let pInter = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist))
         // seg1
         ramps.push({
           vi: vi,
-          vf: feed,
-          t: (feed - vi) / accel,
+          vf: cruise,
+          t: (cruise - vi) / accel,
           pi: pi,
           pf: pInter
         })
-        if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
+        if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
         // seg2
         ramps.push({
-          vi: feed,
+          vi: cruise,
           vf: vf,
-          t: (d - acDist) / feed,
+          t: (d - acDist) / cruise,
           pi: pInter,
           pf: pf
         })
-        if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
+        if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
       } else {
-        let dcDist = (Math.pow(feed, 2) - Math.pow(vf, 2)) / (2 * accel)
-        let acDist = (Math.pow(feed, 2) - Math.pow(vi, 2)) / (2 * accel)
+        let dcDist = (Math.pow(cruise, 2) - Math.pow(vf, 2)) / (2 * accel)
+        let acDist = (Math.pow(cruise, 2) - Math.pow(vi, 2)) / (2 * accel)
         if (acDist + dcDist < d) {
           if (debug) console.log('/--\\')
           let pa = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist))
@@ -257,24 +251,24 @@ export default function Saturn() {
           // 3 segs
           ramps.push({
             vi: vi,
-            vf: feed,
-            t: (feed - vi) / accel,
+            vf: cruise,
+            t: (cruise - vi) / accel,
             pi: pi,
             pf: pa
           })
           if (ramps[ramps.length - 1].t < period) console.warn('trouble seg1/3', ramps[ramps.length - 1])
           ramps.push({
-            vi: feed,
-            vf: feed,
-            t: (d - acDist - dcDist) / feed,
+            vi: cruise,
+            vf: cruise,
+            t: (d - acDist - dcDist) / cruise,
             pi: pa,
             pf: pb
           })
           if (ramps[ramps.length - 1].t < period) console.warn('trouble seg2/3', ramps[ramps.length - 1])
           ramps.push({
-            vi: feed,
+            vi: cruise,
             vf: vf,
-            t: (feed - vf) / accel,
+            t: (cruise - vf) / accel,
             pi: pb,
             pf: pf
           })
@@ -291,7 +285,7 @@ export default function Saturn() {
             pi: pi,
             pf: pInter
           })
-          if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
+          if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
           ramps.push({
             vi: vPeak,
             vf: vf,
@@ -299,7 +293,7 @@ export default function Saturn() {
             pi: pInter,
             pf: pf
           })
-          if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
+          if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
         }
       } // end BIGSWITCH
     } // end for-over-positions
@@ -320,7 +314,7 @@ export default function Saturn() {
       // easy increment is just this vector * rate / period
       let timeSeg = [0, 0, 0]
       timeSeg.forEach((axis, i) => {
-        timeSeg[i] = vect[i] * (feed / period)
+        timeSeg[i] = vect[i] * (cruise / period)
       })
       // now, is this a step-over ? some fast-slow-math for this:
       let np = vSum(position, timeSeg)
@@ -356,60 +350,37 @@ export default function Saturn() {
     // 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...
+      // ok, here's the lookahead routine:
       console.time('lookahead')
-      // we can incorporate this update when we rewrite the loop accordingly
+      // positions[] is global, speeds is generated now
+      // speed[0], matching positions[0], are our current situations
       let speeds = [speed]
+      // JD runs an algorithm that calculates maximum allowable
+      // instantaneous accelerations at corners
       JD(speeds)
-      //console.log('jd writes speeds', speeds)
-      //console.log(`have ${speeds.length} speeds and ${positions.length} positions`)
-      PeriodPass(speeds, true)
-      // now we need to link these together,
+      // we occasionally (rather, often) need to start decelerating (or accelerating) a few segments before
+      // the actual constraint: these passes link segments to one another by ensuring that a pass through
+      // the whole path does not require any accelerations outside of our range.
+      // the reversepass also sets minimum speeds such that we don't find any moves impossible to execute
+      // within one network period
       ReversePass(speeds)
       ForwardPass(speeds)
-      // check about our minperiod issue,
-
-      // are any of these non-permissible?
       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
-      // have 'ideal' entrance / exit speeds,
-
-      // 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.
+      // now have allowable maximum speeds at the junctions - the corners - between segments
+      // but we still need to consider how much acceleration we can do in between these minimum speeds
+      // i.e. we will now generate the individual accel- deccel- and cruise phases of each segment
+      // *now* since we are operating on a period basis, we also tune these segments such that
+      // (1) none are smaller than one period of time and (2) all take some integer division of periods to complete
       let ramps = RampPass(speeds)
+      // now we're done,
       console.timeLog('lookahead')
       console.log(`have ${ramps.length} ramps for ${positions.length} positions`)
       let debug = false
-      for(let i = 0; i < ramps.length; i ++){
-        // try a forward walk through these things. adjust so that distance is equal for each,
-        let r = ramps[i]
-        // first, given current d, entrance and exit speeds, the time:
-        let d = vDist(r.pi, r.pf)
-        let t = 2 * d / (r.vi + r.vf)
-        // round to some degree, but not to zero...
-        let count = t / period
-        let integer = Math.round(count)
-        if(integer < 1){
-          console.warn(`small ramp at ${i}`, r)
-          integer = 1
-        }
-        // so, given vi, want to decrease vf such that t = this period,
-        let nt = t * (integer / count)
-        let nvf = (2 * d) / nt - r.vi
-        if(nvf < 0) console.warn('negative speed', r)
-        if(debug) console.log(`adjusts by ratio ${(integer / count).toFixed(4)}, old vf ${r.vf.toFixed(3)} to nvf ${nvf.toFixed(3)}, t is ${nt.toFixed(3)}`)
-        r.vf = nvf
-        r.t = nt
-        if(ramps[i + 1]) ramps[i + 1].vi = nvf
-      }
       console.timeLog('lookahead')
-      // final check, if anything has fallen under speed...
-      for(let r of ramps){
-        if(r.vi < minSpeed || r.vf < minSpeed || r.vi > cruise || r.vf > cruise){
-          console.warn('trouble speed', r.vi.toFixed(3), r.vf.toFixed(3))
+      // we do one last check:
+      for (let r of ramps) {
+        if (r.vi < minSpeed || r.vf < minSpeed || r.vi > cruise || r.vf > cruise || r.t < period) {
+          console.warn(`troublesome ramp found on final check`, r)
         }
       }
       // run once,
diff --git a/libs/smallvectors.js b/libs/smallvectors.js
index 890d853..547f142 100644
--- a/libs/smallvectors.js
+++ b/libs/smallvectors.js
@@ -39,7 +39,7 @@ let vScalar = (v, s) => {
   for(let i = 0; i < v.length; i ++){
     ret[i] = v[i] * s
   }
-  return ret 
+  return ret
 }
 
 let deg = (rad) => {
diff --git a/scratch/rampcheck.js b/scratch/rampcheck.js
new file mode 100644
index 0000000..47e1a9b
--- /dev/null
+++ b/scratch/rampcheck.js
@@ -0,0 +1,26 @@
+
+// scratch notes
+let fn = () => {
+  for (let i = 0; i < ramps.length; i++) {
+    // try a forward walk through these things. adjust so that distance is equal for each,
+    let r = ramps[i]
+    // first, given current d, entrance and exit speeds, the time:
+    let d = vDist(r.pi, r.pf)
+    let t = 2 * d / (r.vi + r.vf)
+    // round to some degree, but not to zero...
+    let count = t / period
+    let integer = Math.round(count)
+    if (integer < 1) {
+      console.warn(`small ramp at ${i}`, r)
+      integer = 1
+    }
+    // so, given vi, want to decrease vf such that t = this period,
+    let nt = t * (integer / count)
+    let nvf = (2 * d) / nt - r.vi
+    if (nvf < 0) console.warn('negative speed', r)
+    if (debug) console.log(`adjusts by ratio ${(integer / count).toFixed(4)}, old vf ${r.vf.toFixed(3)} to nvf ${nvf.toFixed(3)}, t is ${nt.toFixed(3)}`)
+    r.vf = nvf
+    r.t = nt
+    if (ramps[i + 1]) ramps[i + 1].vi = nvf
+  }
+}
-- 
GitLab