JD to ramped segments

hunks/adhoc/saturn.js

@@ -15,6 +15,8 @@ import {
   vDist,
   vSum,
   vLen,
+  vUnitBetween,
+  vScalar,
   deg
 } from '../../libs/smallvectors.js'
 
@@ -112,6 +114,141 @@ let ForwardPass = (positions, speeds, accel, minSpeed) => {
   // 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)
 
@@ -190,7 +327,7 @@ export default function Saturn() {
     //if (allclear() && positions.length > 32) {
     // first we get all move final v's by jd:
     // we jd,
-    if (positions.length > 32) {
+    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')
@@ -202,7 +339,16 @@ export default function Saturn() {
       // 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')
     }

libs/smallvectors.js

@@ -24,8 +24,26 @@ let vLen = (v) => {
   return Math.sqrt(sum)
 }
 
+// from v1 to v2,
+let vUnitBetween = (v1, v2) => {
+  let dist = vDist(v1, v2)
+  let ret = []
+  for(let i = 0; i < v1.length; i ++){
+    ret[i] = (v2[i] - v1[i]) / dist
+  }
+  return ret
+}
+
+let vScalar = (v, s) => {
+  let ret = []
+  for(let i = 0; i < v.length; i ++){
+    ret[i] = v[i] * s
+  }
+  return ret 
+}
+
 let deg = (rad) => {
   return rad * (180 / Math.PI)
 }
 
-export { vDist, vSum, vLen, deg }
+export { vDist, vSum, vLen, vUnitBetween, vScalar, deg }