diff --git a/hunks/adhoc/saturn.js b/hunks/adhoc/saturn.js index 430233dcba95582f93d8c67243957759ddc7b6b4..6641e9c9ffc24ceb35f91b91468ee1cb2c923b48 100644 --- a/hunks/adhoc/saturn.js +++ b/hunks/adhoc/saturn.js @@ -18,10 +18,17 @@ import { deg } from '../../libs/smallvectors.js' -let JD = (position, speed, accel, moves) => { - //console.log('moves', moves) - let deviation = 0.1 // virtual junction size - let minSpeed = 1 +/* + +indexing: +we should have a positions[n] of positions to get to, +and speeds[n] of speeds to be-at-when-there +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) => { + //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) @@ -38,21 +45,71 @@ let JD = (position, speed, accel, moves) => { return v } // the ops, - let endSpeeds = [speed] // start speed is the current speed - for(let m = 0; m < moves.length; m ++){ - if(m === 0) continue // noop for start: this is our current speed - if(m === moves.length - 1) continue // noop for last move, nothing to junction into - let jd = calcJunctionSpeed(moves[m - 1], moves[m], moves[m+1], deviation, accel) - endSpeeds.push(jd) + 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, - endSpeeds.push(0) + speeds.push(0) // walk for minspeeds - for(let s in endSpeeds){ - if(endSpeeds[s] < minSpeed) endSpeeds[s] = minSpeed + for (let s in speeds) { + if (speeds[s] < minSpeed) speeds[s] = minSpeed } // that's it for us - return endSpeeds + return speeds +} + +let ReversePass = (positions, speeds, accel, minSpeed) => { + // 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]) + 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 + } + if(debug) console.log(`entrance was ${temp}, now ${speeds[i]}`) + } +} + +let ForwardPass = (positions, speeds, accel, minSpeed) => { + // 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]) + 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]}`) + } + // 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 } export default function Saturn() { @@ -69,60 +126,92 @@ export default function Saturn() { return (!outx.io() && !outy.io() && !outz.io()) } - // our path (and num segs to plan over) - let pathBufferSize = 64 - let path = [] + // 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 // (units/s) - let lt = [] // last transform (since we know time, this is our speed data) - let position = [0, 0, 0] // we track position... - let speed = 0 + let feed = 10 // target, (units/s) + let speed = minSpeed // currently let posUpdated = false - let calculateNextIncrement = () => { - // for now, straightforward: current position w/r/t path, - // increment is this posn -> next target, at rate ... - // for simplicity, we can assume that we're always going from where we're at to the [1]th point in the path, - let vect = [0, 0, 0] - let dist = vDist(position, path[0]) - vect.forEach((axis, i) => { - vect[i] = (path[0][i] - position[i]) / dist - }) - // easy increment is just this vector * rate / period - let timeSeg = [0, 0, 0] - timeSeg.forEach((axis, i) => { - timeSeg[i] = vect[i] * (feed / period) - }) - // now, is this a step-over ? some fast-slow-math for this: - let np = vSum(position, timeSeg) - if (vDist(np, position) > vDist(position, path[0])) { - // ts is just to get us exactly to it .. + /* + let calculateNextIncrement = () => { + // for now, straightforward: current position w/r/t positions, + // increment is this posn -> next target, at rate ... + // for simplicity, we can assume that we're always going from where we're at to the [1]th point in the positions, + let vect = [0, 0, 0] + let dist = vDist(position, positions[0]) + vect.forEach((axis, i) => { + vect[i] = (positions[0][i] - position[i]) / dist + }) + // easy increment is just this vector * rate / period + let timeSeg = [0, 0, 0] timeSeg.forEach((axis, i) => { - timeSeg[i] = path[0][i] - position[i] + timeSeg[i] = vect[i] * (feed / period) }) - path.shift() + // now, is this a step-over ? some fast-slow-math for this: + let np = vSum(position, timeSeg) + if (vDist(np, position) > vDist(position, positions[0])) { + // ts is just to get us exactly to it .. + timeSeg.forEach((axis, i) => { + timeSeg[i] = positions[0][i] - position[i] + }) + positions.shift() + } + return timeSeg } - return timeSeg - } + */ this.loop = () => { - if (path.length < pathBufferSize && inpts.io()) { + // loading new points, + if (positions.length < positionsBufferSize && inpts.io()) { let np = inpts.get() - console.log('new pt\t', path.length, np) - path.push(np) - } - if (allclear() && path.length > 0) { - // we jd, - if(path.length > 32){ - console.log('JD', JD(position, speed, 10, path)) - throw new Error('halt') + // reject the baddies + try { + if(vLen(math.subtract(np, positions[positions.length - 1])) === 0){ + // dunk on 'em + console.warn('zero length appendage rejected by planner') + } else { + positions.push(np) // end of queue + //console.log('new pt\t', positions.length, np) + } + } catch (err) { + console.warn('error caught at saturn input', err) } + } + //if (allclear() && positions.length > 32) { + // first we get all move final v's by jd: + // we jd, + if (positions.length > 32) { + // at the moment, for jd, we'll assume positions[0] is our current position. + // we should time this... + console.time('lookahead') + // we can incorporate this update when we rewrite the loop accordingly + let speeds = [speed] + JD(positions, speeds, deviation, accel, minSpeed) + //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) + console.timeEnd('lookahead') + // run once, + throw new Error('halt') + } + /* // this is our major timestep, and should happen only once-every-segment - // path length reads @ 26, but NaN ?? - // do check ... if anything is NaN, print all path, OK. maybe reference problem? - console.log(path.length) - // we have items in the path, + // positions length reads @ 26, but NaN ?? + // do check ... if anything is NaN, print all positions, OK. maybe reference problem? + console.log(positions.length) + // we have items in the positions, // we should calculate an ideal trajectory from our current position and speed // probably recalculating the whole thing, and then send that out... let ts = calculateNextIncrement() @@ -138,5 +227,7 @@ export default function Saturn() { outp.put(position) posUpdated = false } + */ + //} } }