diff --git a/hunks/adhoc/saturn.js b/hunks/adhoc/saturn.js index 6641e9c9ffc24ceb35f91b91468ee1cb2c923b48..7c6bb583d663fe2c7c7374c97fd2d7854ccc769c 100644 --- a/hunks/adhoc/saturn.js +++ b/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') } diff --git a/libs/smallvectors.js b/libs/smallvectors.js index 4a8855635680aa45c2c85003390098783f2371f3..890d853dfafd97d9dc144b2efc4a3f10bba162dc 100644 --- a/libs/smallvectors.js +++ b/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 }