bugfix not checking for speeds > cruise speed

hunks/adhoc/saturn.js

@@ -29,7 +29,7 @@ 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) => {
+let JD = (positions, speeds, deviation, accel, minSpeed, cruise) => {
   //console.log('positions', positions)
   let calcJunctionSpeed = (p0, p1, p2, jd, a) => {
     // junction speed at p1, arrival from p0 exit to p2
@@ -61,14 +61,14 @@ let JD = (positions, speeds, deviation, accel, minSpeed) => {
   // walk for minspeeds
   for (let s in speeds) {
     if (speeds[s] < minSpeed) speeds[s] = minSpeed
+    if (speeds[s] > cruise) speeds[s] = cruise
   }
   // that's it for us
   return speeds
 }
 
-let ReversePass = (positions, speeds, accel, minSpeed) => {
+let ReversePass = (positions, speeds, accel, minSpeed, debug) => {
   // 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])
@@ -91,9 +91,8 @@ let ReversePass = (positions, speeds, accel, minSpeed) => {
   }
 }
 
-let ForwardPass = (positions, speeds, accel, minSpeed) => {
+let ForwardPass = (positions, speeds, accel, minSpeed, debug) => {
   // 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])
@@ -114,13 +113,13 @@ 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
+let RampPass = (positions, speeds, ramps, a, cruise, debug) => {
   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]
+    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 * a * d)
@@ -138,6 +137,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
         pi: pi,
         pf: pf
       })
+      if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
     } else if (maxEntry <= vi){
       if(debug) console.log('\\')
       ramps.push({
@@ -147,6 +147,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
         pi: pi,
         pf: pf
       })
+      if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
     } else if (vi === cruise && vf === cruise){
       if(debug) console.log('--')
       ramps.push({
@@ -156,6 +157,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
         pi: pi,
         pf: pf
       })
+      if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
     } else if (vi === cruise) {
       if(debug) console.log('--\\')
       let dcDist = (Math.pow(cruise, 2) - Math.pow(vf, 2)) / (2 * a)
@@ -168,6 +170,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
         pi: pi,
         pf: pInter
       })
+      if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
       // seg 2,
       ramps.push({
         vi: cruise,
@@ -176,6 +179,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
         pi: pInter,
         pf: pf
       })
+      if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
     } else if (vf === cruise){
       if(debug) console.log('/--')
       let acDist = (Math.pow(cruise, 2) - Math.pow(vi, 2)) / (2 * a)
@@ -188,6 +192,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
         pi: pi,
         pf: pInter
       })
+      if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
       // seg2
       ramps.push({
         vi: cruise,
@@ -196,10 +201,11 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
         pi: pInter,
         pf: pf
       })
+      if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
     } 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(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))
@@ -211,6 +217,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
           pi: pi,
           pf: pa
         })
+        if(ramps[ramps.length - 1].t < 0) console.warn('trouble seg1/3', ramps[ramps.length - 1])
         ramps.push({
           vi: cruise,
           vf: cruise,
@@ -218,6 +225,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
           pi: pa,
           pf: pb
         })
+        if(ramps[ramps.length - 1].t < 0) console.warn('trouble seg2/3', ramps[ramps.length - 1])
         ramps.push({
           vi: cruise,
           vf: vf,
@@ -225,6 +233,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
           pi: pb,
           pf: pf
         })
+        if(ramps[ramps.length - 1].t < 0) console.warn('trouble seg3/3', ramps[ramps.length - 1])
       } else {
         if(debug) console.log('/\\')
         let vPeak = Math.sqrt(((2 * a * d + Math.pow(vi, 2) + Math.pow(vf, 2)) / 2))
@@ -237,6 +246,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
           pi: pi,
           pf: pInter
         })
+        if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
         ramps.push({
           vi: vPeak,
           vf: vf,
@@ -244,6 +254,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
           pi: pInter,
           pf: pf
         })
+        if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
       }
     } // end BIGSWITCH
   }
@@ -333,12 +344,13 @@ export default function Saturn() {
       console.time('lookahead')
       // we can incorporate this update when we rewrite the loop accordingly
       let speeds = [speed]
-      JD(positions, speeds, deviation, accel, minSpeed)
+      JD(positions, speeds, deviation, accel, minSpeed, feed)
       //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)
+      // 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
@@ -346,9 +358,13 @@ export default function Saturn() {
       // 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)
+      RampPass(positions, speeds, ramps, accel, feed, true)
       console.timeEnd('lookahead')
       console.log(`have ${ramps.length} ramps for ${positions.length} positions`)
+      console.log('times')
+      for(let r of ramps){
+        console.log(r.t)
+      }
       // run once,
       throw new Error('halt')
     }