diff --git a/hunks/adhoc/saturn.js b/hunks/adhoc/saturn.js index f809d6bdbb9a34d6052dee89e26d1f252707fd36..276a52f831bbea89512115f504fdbb3919849208 100644 --- a/hunks/adhoc/saturn.js +++ b/hunks/adhoc/saturn.js @@ -29,266 +29,283 @@ 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, cruise) => { - //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) - let v1 = math.subtract(p2, p1) - //console.log('for\n', v0, v1) - let dotprod = math.dot(v0, v1) / (vLen(v0) * vLen(v1)) - //console.log('dotprod', dotprod) - let omega = Math.PI - Math.acos(dotprod) - //console.log('angle between', deg(omega)) - let r = jd / (1 / Math.cos(Math.PI / 2 - omega / 2) - 1) - //console.log('rad', r) - let v = Math.sqrt(accel * r) - //console.log('permissible', v) - return v +export default function Saturn() { + Hunkify(this) + + let inpts = this.input('array', 'posn') + let outx = this.output('number', 'outx') + let outy = this.output('number', 'outy') + let outz = this.output('number', 'outz') + let outp = this.output('array', 'posn') + let period = 0.050 // his.state('number', 'period', 50) // in ms, + + let allclear = () => { + return (!outx.io() && !outy.io() && !outz.io()) } - // the ops, - 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}`) + + // 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 // target, (units/s) + let speed = minSpeed // currently + let posUpdated = false + + let JD = (speeds, debug) => { + //console.log('positions', positions) + let calcJunctionSpeed = (p0, p1, p2, jd) => { + // junction speed at p1, arrival from p0 exit to p2 + let v0 = math.subtract(p1, p0) + let v1 = math.subtract(p2, p1) + //console.log('for\n', v0, v1) + let dotprod = math.dot(v0, v1) / (vLen(v0) * vLen(v1)) + //console.log('dotprod', dotprod) + let omega = Math.PI - Math.acos(dotprod) + //console.log('angle between', deg(omega)) + let r = jd / (1 / Math.cos(Math.PI / 2 - omega / 2) - 1) + //console.log('rad', r) + let v = Math.sqrt(accel * r) + //console.log('permissible', v) + return v + } + // the ops, + 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, + speeds.push(0) + // walk for minspeeds + for (let s in speeds) { + if (speeds[s] < minSpeed) speeds[s] = minSpeed + if (speeds[s] > feed) speeds[s] = feed } - speeds.push(jd) + // that's it for us + return speeds } - // finish at zero, - speeds.push(0) - // walk for minspeeds - for (let s in speeds) { - if (speeds[s] < minSpeed) speeds[s] = minSpeed - if (speeds[s] > cruise) speeds[s] = cruise + + 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) + } + } } - // that's it for us - return speeds -} -let ReversePass = (positions, speeds, accel, minSpeed, debug) => { - // link, walking back from last - // 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 + let ReversePass = (speeds, debug) => { + // link, walking back from last + // 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])) + // 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) + // 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]}`) } - if(debug) console.log(`entrance was ${temp}, now ${speeds[i]}`) } -} -let ForwardPass = (positions, speeds, accel, minSpeed, debug) => { - // link, walk forwards: can we accel to these velocities in time? - 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 + let ForwardPass = (speeds, debug) => { + // link, walk forwards: can we accel to these velocities in time? + 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]}`) } - 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 } - // 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 -} -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) - 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 - }) - if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1]) - } else if (maxEntry <= vi){ - if(debug) console.log('\\') - ramps.push({ - vi: vi, - vf: vf, - t: (vi-vf) / a, - 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({ - vi: vi, - vf: vf, - t: d / vi, - 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) - 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 - }) - if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1]) - // seg 2, - ramps.push({ - vi: cruise, - vf: vf, - t: (cruise - vf) / a, - 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) - 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 - }) - if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1]) - // seg2 - ramps.push({ - vi: cruise, - vf: vf, - t: (d - acDist) / 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(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 + let RampPass = (speeds, debug) => { + let ramps = [] + 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 > feed) console.warn(`vi at ${i} > feed 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) + let maxExit = Math.sqrt(Math.pow(speeds[i], 2) + 2 * accel * 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: cruise, - t: (cruise - vi) / a, + vf: vf, + t: (vf - vi) / accel, pi: pi, - pf: pa + pf: pf }) - if(ramps[ramps.length - 1].t < 0) console.warn('trouble seg1/3', ramps[ramps.length - 1]) + if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1]) + } else if (maxEntry <= vi) { + if (debug) console.log('\\') ramps.push({ - vi: cruise, - vf: cruise, - t: (d - acDist - dcDist) / cruise, - pi: pa, - pf: pb + vi: vi, + vf: vf, + t: (vi - vf) / accel, + pi: pi, + pf: pf }) - if(ramps[ramps.length - 1].t < 0) console.warn('trouble seg2/3', ramps[ramps.length - 1]) + if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1]) + } else if (vi === feed && vf === feed) { + if (debug) console.log('--') ramps.push({ - vi: cruise, + vi: vi, vf: vf, - t: (cruise - vf) / a, - pi: pb, + t: d / vi, + pi: pi, 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)) - let acDist = (Math.pow(vPeak, 2) - Math.pow(vi, 2)) / (2 * a) + if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1]) + } else if (vi === feed) { + if (debug) console.log('--\\') + let dcDist = (Math.pow(feed, 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, + pi: pi, + pf: pInter + }) + if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1]) + // seg 2, + ramps.push({ + vi: feed, + vf: vf, + t: (feed - 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 (debug) console.log('/--') + let acDist = (Math.pow(feed, 2) - Math.pow(vi, 2)) / (2 * accel) let pInter = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist)) + // seg1 ramps.push({ vi: vi, - vf: vPeak, - t: (vPeak - vi) / a, + vf: feed, + t: (feed - vi) / accel, pi: pi, pf: pInter }) - if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1]) + if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1]) + // seg2 ramps.push({ - vi: vPeak, + vi: feed, vf: vf, - t: (vPeak - vf) / a, + t: (d - acDist) / feed, pi: pInter, pf: pf }) - if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1]) - } - } // end BIGSWITCH - } -} - -export default function Saturn() { - Hunkify(this) - - let inpts = this.input('array', 'posn') - let outx = this.output('number', 'outx') - let outy = this.output('number', 'outy') - let outz = this.output('number', 'outz') - let outp = this.output('array', 'posn') - let period = 0.050 // his.state('number', 'period', 50) // in ms, - - let allclear = () => { - return (!outx.io() && !outy.io() && !outz.io()) + if (ramps[ramps.length - 1].t < period) console.warn('trouble', 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) + 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: feed, + t: (feed - 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, + pi: pa, + pf: pb + }) + if (ramps[ramps.length - 1].t < period) console.warn('trouble seg2/3', ramps[ramps.length - 1]) + ramps.push({ + vi: feed, + vf: vf, + t: (feed - vf) / accel, + pi: pb, + pf: pf + }) + if (ramps[ramps.length - 1].t < period) console.warn('trouble seg3/3', ramps[ramps.length - 1]) + } else { + if (debug) console.log('/\\') + let vPeak = Math.sqrt(((2 * accel * d + Math.pow(vi, 2) + Math.pow(vf, 2)) / 2)) + let acDist = (Math.pow(vPeak, 2) - Math.pow(vi, 2)) / (2 * accel) + let pInter = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist)) + ramps.push({ + vi: vi, + vf: vPeak, + t: (vPeak - vi) / accel, + pi: pi, + pf: pInter + }) + if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1]) + ramps.push({ + vi: vPeak, + vf: vf, + t: (vPeak - vf) / accel, + pi: pInter, + pf: pf + }) + if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1]) + } + } // end BIGSWITCH + } // end for-over-positions + return ramps } - // 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 // target, (units/s) - let speed = minSpeed // currently - let posUpdated = false /* let calculateNextIncrement = () => { @@ -324,7 +341,7 @@ export default function Saturn() { let np = inpts.get() // reject the baddies try { - if(vLen(math.subtract(np, positions[positions.length - 1])) === 0){ + if (vLen(math.subtract(np, positions[positions.length - 1])) === 0) { // dunk on 'em console.warn('zero length appendage rejected by planner') } else { @@ -344,26 +361,56 @@ 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, feed) + 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, - ReversePass(positions, speeds, accel, minSpeed) - ForwardPass(positions, speeds, accel, minSpeed) + 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. - let ramps = [] // an arr of objs - RampPass(positions, speeds, ramps, accel, feed, true) - console.timeEnd('lookahead') + let ramps = RampPass(speeds) + console.timeLog('lookahead') console.log(`have ${ramps.length} ramps for ${positions.length} positions`) - console.log('times') + 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){ - console.log(r.t) + 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)) + } } // run once, throw new Error('halt')