diff --git a/hunks/adhoc/saturn.js b/hunks/adhoc/saturn.js index 3ee468fb6eaebb7ffff7015e613dcb95b205de1b..4f6615b59b0d62f594c368a3692a8be5ae89772a 100644 --- a/hunks/adhoc/saturn.js +++ b/hunks/adhoc/saturn.js @@ -29,14 +29,28 @@ p[0] and s[0] are always current state ... when we len > 1 we have werk 2 do */ +/* + +speedups: +yr boi saturn, chronos, the ancient titan and god of time is *slow* af: about 50ms +to run ~ 64 segments. much could be done to make this faster, (1) would be to ship it to cpp, +which, tbh, is probably the right answer. also: +Math.pow() where num * num would do: bad +array manipulation +calculate distances, etc, only once + +*/ + export default function Saturn() { Hunkify(this) let inpts = this.input('array', 'posn') + + let outp = this.output('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 allClear = () => { return (!outx.io() && !outy.io() && !outz.io()) @@ -62,6 +76,23 @@ export default function Saturn() { // we nasty, let spmm = 200 + // OK: *i think* the move might be to do this *first* and then never increase moves + let periodPass = (speeds, debug) => { + for (let i = positions.length - 2; i > 0; i--) { + // distance to make, + let d = vDist(positions[i], positions[i + 1]) + // the *fastest* we could go if we go flat out, in one period, is + let v = d / period + // set self, + speeds[i] = v + // traceback: + if (speeds[i + 1] > speeds[i]) { + speeds[i + 1] = v + } + } + } + + // run JD: calculate maximum allowable speeds at corners due to instantaneous turnaround limit let jd = (speeds, debug) => { //console.log('positions', positions) let calcJunctionSpeed = (p0, p1, p2, jd) => { @@ -87,10 +118,10 @@ export default function Saturn() { if (Number.isNaN(jd)) { console.log(`NaN for ${m}`) } - speeds.push(jd) + if (jd < speeds[m]) { + speeds[m] = jd + } } - // finish at zero, - speeds.push(0) // walk for minspeeds for (let s in speeds) { if (speeds[s] < minSpeed) speeds[s] = minSpeed @@ -100,6 +131,7 @@ export default function Saturn() { return speeds } + // link, tail to head let reversePass = (speeds, debug) => { // link, walking back from last // this makes sure we can completely decelerate, through moves, to the last point at zero @@ -123,35 +155,7 @@ export default function Saturn() { } } - // OK: *i think* the move might be to do this *first* and then never increase moves - - let periodPass = (speeds, debug) => { - for (let i = positions.length - 2; i > 0; i--) { - // distance to make, - let d = vDist(positions[i], positions[i+1]) - // 'initial' velocity (final) - let vi = speeds[i] - let vf = speeds[i + 1] - // with maximum acceleration, and this distance, this takes this long: - let tMin = (2 * d) / (vi + vf) - if(tMin < period){ - console.warn('vi, vf, distance, tMin') - console.log(speeds[i].toFixed(3), speeds[i + 1].toFixed(3)) - console.log(d.toFixed(3)) - console.log(tMin.toFixed(3)) - // this is a reverse pass, so we actually want to modify vi - // such that ... - let nvi = period / (2 * d) - vf - console.log('propose', nvi.toFixed(3)) - // ok then: given the d, what v for one period? - let v = d / period - console.log('flat at', v.toFixed(3)) - // however: this might drive vf into the -ves - in that case, we need to walk forwards... - // this might actually be that sorting algo... - } - } - } - + // link, head to tail let forwardPass = (speeds, debug) => { // link, walk forwards: can we accel to these velocities in time? for (let i = 0; i < positions.length - 2; i++) { @@ -174,6 +178,154 @@ export default function Saturn() { // here is assuming positions[0] is current position, for which speed is the current velocity } + // seg filter + let writeSeg = (ramps, vi, vf, pi, pf) => { + let d = vDist(pi, pf) + ramps.push({ + vi: vi, + vf: vf, + t: 2 * d / (vi + vf), + pi: pi, + pf: pf + }) + //if (segWriteLog) console.log(`wrote seg w/ t: ${ramps[ramps.length - 1].t.toFixed(3)}`) + } + + let writeTriangle = (ramps, vi, vf, pi, pf) => { + let d = vDist(pi, pf) + // not sure when I wrote this eqn, seems to work tho + 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)) + // finally, we have to check here if either / or side is too small, then default to smallticks + let tSeg1 = (vPeak - vi) / accel + let tSeg2 = (vPeak - vf) / accel + if (tSeg1 < period || tSeg2 < period) { + // bail hard, write one seg only + writeSeg(ramps, vi, vf, pi, pf) + } else { + // write two segs, + writeSeg(ramps, vi, vPeak, pi, pInter) + writeSeg(ramps, vPeak, vf, pInter, pf) + } + } + + // turn positions, speeds into segments, writing accelerations between + 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] + let vf = speeds[i + 1] + let d = vDist(pi, pf) + 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) { + // the all-up and all-down segments should always be clear: + // since we already swept for these cases in the revpass + if (debug) console.log(`/`) + writeSeg(ramps, vi, vf, pi, pf) + } else if (maxEntry <= vi) { + if (debug) console.log('\\') + writeSeg(ramps, vi, vf, pi, pf) + } else if (vi === cruise && vf === cruise) { + // similarely, since we're not segmenting cruise any farther, it should also be OK + if (debug) console.log('--') + writeSeg(ramps, vi, vf, pi, p) + } else if (vi === cruise) { + if (debug) console.log('--\\') + let dcDist = (Math.pow(vi, 2) - Math.pow(vf, 2)) / (2 * accel) // distance to deccelerate + let pInter = math.add(pf, vScalar(vUnitBetween(pf, pi), dcDist)) + // now, we need to tune accel / cruise phases so that neither t is < 1 period + let tSeg1 = (d - dcDist) / vi + let tSeg2 = (vi - vf) / accel + if (tSeg1 < period || tSeg2 < period) { + // small segs, just write as one downtick, + writeSeg(ramps, vi, vf, pi, pf) + } else { + // if these are both > one period, we can write 'em + writeSeg(ramps, vi, vi, pi, pInter) + writeSeg(ramps, vi, vf, pInter, pi) + } + } else if (vf === cruise) { + if (debug) console.log('/--') + let acDist = (Math.pow(cruise, 2) - Math.pow(vi, 2)) / (2 * accel) + let pInter = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist)) + // I feel the same about this as I did above + let tSeg1 = (cruise - vi) / accel + let tSeg2 = (d - acDist) / cruise + if (tSeg1 < period || tSeg2 < period) { + writeSeg(ramps, vi, vf, pi, pf) + } else { + writeSeg(ramps, vi, vf, pi, pInter) + writeSeg(ramps, vf, vf, pInter, pf) + } + } else { + // here we will handle triangles '/\' and 'full trapezoids' '/--\' + 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 (dcDist + dcDist > d) { + if (debug) console.log('/\\') + writeTriangle(ramps, vi, vf, pi, pf) + } else { // BEGIN TRAP SELECTIONS + if (debug) console.log('/--\\') + let pa = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist)) + let pb = math.add(pf, vScalar(vUnitBetween(pf, pi), dcDist)) + // ok, + let tSeg1 = (cruise - vi) / accel + let tSeg2 = (d - acDist - dcDist) / cruise + let tSeg3 = (cruise - vf) / accel + // here we go + if (tSeg2 < period) { + // for this case, contencating into a triangle is fine... it will be within ~ 50ms of extra accel time: not much + if (debug) console.log('/\\') + writeTriangle() + } else if (tSeg1 < period && tSeg3 < period) { + // contencate into one ramp + writeSeg(ramps, vi, vf, pi, pf) + } else if (tSeg1 < period) { + // first segment smaller: second larger, third larger + // contencate first, second into one, then write last + writeSeg(ramps, vi, cruise, pi, pb) + writeSeg(ramps, cruise, vf, pb, pf) + } else if (tSeg3 < period) { + // last segment smaller: second larger, first larger + // write first, then contencate second, third into one + writeSeg(ramps, vi, cruise, pi, pa) + writeSeg(ramps, cruise, vf, pa, pf) + } + } // end TRAP SELECTIONS + } // end BIGSWITCH + } // end for-over-positions + return ramps + } + + let positionsCheck = (speeds) => { + for (let i = 0; i < positions.length - 1; i++) { + let d = vDist(positions[i], positions[i + 1]) + let vi = speeds[i] + let vf = speeds[i + 1] + let t = 2 * d / (vi + vf) + if (false) console.log(`ap, ${t.toFixed(3)}`) + if (t < (period - 0.001)) console.warning('small link in positions check') + } + } + + let rampCheck = (ramps) => { + for(let r of ramps){ + let d = vDist(r.pi, r.pf) + let t = 2 * d / (r.vi + r.vf) + if(t < (period - 0.001)) console.warning('troublesome ramp, small time', r) + if(r.vi > (cruise + 1) || r.vf > (cruise + 1)) console.warning('troublesome ramp, high speed', r) + } + } + let blockOnce = true let blocks = [] let posNow = [] @@ -199,28 +351,60 @@ export default function Saturn() { // first we get all move final v's by jd: // we jd, if (positions.length > positionsBufferSize - 1 && blockOnce) { - for(let i = 0; i < positions.length - 2; i ++){ - //console.log(vDist(positions[i], positions[i + 1])) - } blockOnce = false // ok, here's the lookahead routine: console.time('lookahead') // positions[] is global, speeds is generated now // speed[0], matching positions[0], are our current situations - let speeds = [speed] + let speeds = new Array(positions.length) + speeds[0] = speed + speeds[speeds.length - 1] = minSpeed + // first, set speeds such that moves can be made within single periods, + periodPass(speeds) + console.timeLog('lookahead') // jd runs an algorithm that calculates maximum allowable // instantaneous accelerations at corners - jd(speeds) + jd(speeds) // at ~ 38ms, this is the beef of it + console.timeLog('lookahead') // revpass to link by max. accel: reversePass(speeds) - // reverse again, checking that times are within periods... - periodPass(speeds) forwardPass(speeds) + // rough check speeds after initial passes, + // TODO: if we find these throwing errs, interrupt control when we + // hit some geometry we can't deal with? + positionsCheck(speeds) + console.timeLog('lookahead') + // ok, ramps: + let ramps = rampPass(speeds) + // and a check, + rampCheck(ramps) console.timeLog('lookahead') + // NEXT + /* + console.timeLog('lookahead') + for (let i = 0; i < positions.length - 1; i++) { + console.log(`${i} of ${positions.length} has speed ${speeds[i].toFixed(3)} dist ${vDist(positions[i], positions[i+1]).toFixed(3)}`) + } + // now ready to segment + //... + let ramps = rampPass(speeds, true) + // hmm... + console.log(`have ${ramps.length} ramps`) + /* + for(let r of ramps){ + console.log(r.t) + } + */ // re-hash from here + // what is a block to a motor? + // if we want to preserve times, perhaps blocks are: + // (uint) timebase (period size) + // (int) rate start + // (int) rate rate + // (uint) rate count } - if(blocks.length > 0 && allClear()){ + if (blocks.length > 0 && allClear()) { let bl = blocks.shift() //console.log('block', bl) let dist = vDist(bl[0], bl[1]) diff --git a/save/contexts/cuttlefish/dbg.json b/save/contexts/cuttlefish/dbg.json new file mode 100644 index 0000000000000000000000000000000000000000..8686d30ff9841bc92dae745cf9a924d7ee97eb27 --- /dev/null +++ b/save/contexts/cuttlefish/dbg.json @@ -0,0 +1,117 @@ +{ + "interpreterName": "cuttlefish", + "interpreterVersion": "v0.1", + "hunks": [ + { + "type": "manager", + "name": "nrol", + "inputs": [ + { + "name": "msgs", + "type": "byteArray" + } + ], + "outputs": [ + { + "name": "msgs", + "type": "byteArray", + "connections": [ + { + "inHunkIndex": "1", + "inHunkInput": "0" + } + ] + } + ] + }, + { + "type": "view", + "name": "tlview", + "inputs": [ + { + "name": "msgs", + "type": "byteArray" + } + ], + "outputs": [ + { + "name": "msgs", + "type": "byteArray", + "connections": [ + { + "inHunkIndex": "0", + "inHunkInput": "0" + } + ] + } + ] + }, + { + "type": "adhoc/tpath", + "name": "adhoc/tpath_2", + "outputs": [ + { + "name": "position", + "type": "array", + "connections": [ + { + "inHunkIndex": "3", + "inHunkInput": "0" + } + ] + } + ], + "states": [ + { + "name": "reset", + "type": "boolean", + "value": "false" + } + ] + }, + { + "type": "adhoc/saturn", + "name": "adhoc/saturn_3", + "inputs": [ + { + "name": "posn", + "type": "array" + } + ], + "outputs": [ + { + "name": "posn", + "type": "array", + "connections": [ + { + "inHunkIndex": "4", + "inHunkInput": "0" + } + ] + }, + { + "name": "outx", + "type": "number" + }, + { + "name": "outy", + "type": "number" + }, + { + "name": "outz", + "type": "number" + } + ] + }, + { + "type": "interface/threejs_ghost", + "name": "interface/threejs_ghost_4", + "inputs": [ + { + "name": "point", + "type": "array" + } + ] + } + ] +} \ No newline at end of file diff --git a/scratch/ramppass.js b/scratch/ramppass.js index a9a205b4999419c66f70b871e3e95b7d189599be..6529f36d27afb1cd5c9889d32e0ccc80a29c7fdb 100644 --- a/scratch/ramppass.js +++ b/scratch/ramppass.js @@ -1,253 +1,34 @@ -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 segPass = (speeds, debug) => { + let bPositions = [positions[0]] + let bSpeeds = [speeds[0]] + // stretchy band ... + for(let i = 0; i < positions.length - 2; i ++){ + let d = vDist(positions[i], positions[i + 1]) let vi = speeds[i] let vf = speeds[i + 1] - let d = vDist(pi, pf) - 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}`) - // these are common and useful: - let writeUpTick = () => { - ramps.push({ - vi: vi, - vf: vf, - t: (vf - vi) / accel, - pi: pi, - pf: pf - }) + // if the plain-move time is close to the period, we ship it + let pt = (2 * d) / (vi + vf) + if(pt < (period - 0.001)){ + // do we have to ship start- and finish- velocities to the motors? + // do if we turn around at junctions... + // goddangit + // ok, now that we can at least be sure we'll have a minimum time, maybe we can + // engineer the stepper side to help us out ... + console.error(`seg in pos ${i} violates plain move time with period ${pt.toFixed(3)}`) + console.error(d.toFixed(3), vi.toFixed(3), vf.toFixed(3)) + } else { + if(debug) console.log(`${i} pt ${pt.toFixed(3)}`) } - let writeDownTick = () => { - ramps.push({ - vi: vi, - vf: vf, - t: (vi - vf) / accel, - pi: pi, - pf: pf - }) + // check size + if(pt - period < 2 * period){ + bPositions.push(positions[i]) + bSpeeds.push(speeds[i]) } - // big switch - if (maxExit <= vf) { - // the all-up and all-down segments should always be clear: - // since we already swept for these cases in the revpass - if (debug) console.log(`/`) - writeUpTick() - } else if (maxEntry <= vi) { - if (debug) console.log('\\') - writeDownTick() - } else if (vi === cruise && vf === cruise) { - // similarely, since we're not segmenting cruise any farther, it should also be OK - 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 * accel) - let pInter = math.add(pf, vScalar(vUnitBetween(pf, pi), dcDist)) - // now, we need to tune accel / cruise phases so that neither t is < 1 period - let tSeg1 = (d - dcDist) / cruise - let tSeg2 = (cruise - vf) / accel - if (tSeg1 < period || tSeg2 < period) { - // hopeless, write downtick - // there are other options here: adjust one to suit other, but we're not here for it - // and in these cases, the small segments, being small, can just get washed into a big ramp, ok - writeDownTick() - } else { - // and if we can't do that, we backtrack to '\\' type - // seg1 - ramps.push({ - vi: vi, - vf: cruise, - t: tSeg1, - pi: pi, - pf: pInter - }) - // seg 2, - ramps.push({ - vi: cruise, - vf: vf, - t: tSeg2, - pi: pInter, - pf: pf - }) - } - } else if (vf === cruise) { - if (debug) console.log('/--') - let acDist = (Math.pow(cruise, 2) - Math.pow(vi, 2)) / (2 * accel) - let pInter = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist)) - // I feel the same about this as I did above - let tSeg1 = (cruise - vi) / accel - let tSeg2 = (d - acDist) / cruise - if (tSeg1 < period || tSeg2 < period) { - writeDownTick() - } else { - // seg1 - ramps.push({ - vi: vi, - vf: cruise, - t: tSeg1, - pi: pi, - pf: pInter - }) - // seg2 - ramps.push({ - vi: cruise, - vf: vf, - t: tSeg2, - pi: pInter, - pf: pf - }) - } - } else { - let dcDist = (Math.pow(cruise, 2) - Math.pow(vf, 2)) / (2 * accel) - let acDist = (Math.pow(cruise, 2) - Math.pow(vi, 2)) / (2 * accel) - let writeTriangle = () => { - 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)) - // finally, we have to check here if either / or side is too small, then default to smallticks - let tSeg1 = (vPeak - vi) / accel - let tSeg2 = (vPeak - vf) / accel - if (tSeg1 < period || tSeg2 < period) { - // bail hard, - if (vf > vi) { - writeUpTick() - } else { - writeDownTick() - } - } else { - ramps.push({ - vi: vi, - vf: vPeak, - t: tSeg1, - pi: pi, - pf: pInter - }) - ramps.push({ - vi: vPeak, - vf: vf, - t: tSeg2, - pi: pInter, - pf: pf - }) - } - } - 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)) - // ok, - let tSeg1 = (cruise - vi) / accel - let tSeg2 = (d - acDist - dcDist) / cruise - let tSeg3 = (cruise - vf) / accel - if (tSeg2 < period) { - // for this case, contencating into a triangle is fine... it will be within ~ 50ms of extra accel time: not much - writeTriangle() - } else if (tSeg1 < period && tSeg3 < period) { - // contencate to slow-up or slow-down - if (vf > vi) { - writeUpTick() - } else { - writeDownTick() - } - } else if (tSeg1 < period) { - // check that enough space to make slow triangle, - if (tSeg1 + tSeg2 < period) { - // sweet lord there must be a better way - if (vf > vi) { - writeUpTick() - } else { - writeDownTick() - } - } else { - // slow-triangle up, - ramps.push({ - vi: vi, - vf: cruise, - t: 2 * vDist(pi, pb) / (vi + cruise), - pi: pi, - pf: pb - }) - // and last seg. as normal - ramps.push({ - vi: cruise, - vf: vf, - t: tSeg3, - pi: pb, - pf: pf - }) - } - } else if (tSeg3 < period) { - if (tSeg2 + tSeg3 < period) { - // ibid - if (vf > vi) { - writeUpTick() - } else { - writeDownTick() - } - } else { - // write first seg as normal, - ramps.push({ - vi: vi, - vf: cruise, - t: tSeg1, - pi: pi, - pf: pa - }) - // second as slow-loss from pa -> vf - ramps.push({ - vi: cruise, - vf: vf, - t: 2 * vDist(pa, pf) / (cruise + vf), - pi: pa, - pf: pf - }) - } - } else { - // 3 segs - ramps.push({ - vi: vi, - vf: cruise, - t: tSeg1, - pi: pi, - pf: pa - }) - ramps.push({ - vi: cruise, - vf: cruise, - t: tSeg2, - pi: pa, - pf: pb - }) - ramps.push({ - vi: cruise, - vf: vf, - t: tSeg3, - pi: pb, - pf: pf - }) - } - } else { - // the actual triangle case - writeTriangle() - } - } // end BIGSWITCH - } // end for-over-positions - return ramps + } } - let blockPass = (ramps, debug) => { let blocks = [] for (let i = 0; i < ramps.length; i++) {