Skip to content
Snippets Groups Projects
Commit 2b79ab62 authored by Jake Read's avatar Jake Read
Browse files

add notes to saturn, some clean, add network check pass to revpass

parent 97ddc443
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
hunks/adhoc/saturn.js
+ 66
− 95
View file @ 2b79ab62
......@@ -52,10 +52,10 @@ export default function Saturn() {
// 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)
let minSpeed = 0.01 // conspicuous, to debug for tails (indexing)
// current states,
let feed = 10 // target, (units/s)
let cruise = 10 // target, (units/s)
let speed = minSpeed // currently
let posUpdated = false
......@@ -91,25 +91,12 @@ export default function Saturn() {
// walk for minspeeds
for (let s in speeds) {
if (speeds[s] < minSpeed) speeds[s] = minSpeed
if (speeds[s] > feed) speeds[s] = feed
if (speeds[s] > cruise) speeds[s] = cruise
}
// that's it for us
return speeds
}
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)
}
}
}
let ReversePass = (speeds, debug) => {
// link, walking back from last
// this makes sure we can completely decelerate, through moves, to the last point at zero
......@@ -117,12 +104,19 @@ export default function Saturn() {
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?
// to calcluate the maximum entrance, given our exit, with pure acceleration:
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)
let maxEntranceByAccel = Math.sqrt(Math.pow(speeds[i + 1], 2) + 2 * accel * d)
// to calculate the maximum entrance, given our exit, to make within one period of time:
// let t = 2 * d / (speeds[i] + speeds[i + 1]) // current t ...
// v2, where t = period
let maxEntranceByPeriod = period / (2 * d) - speeds[i]
// 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))
let max = Math.max(minSpeed, Math.min(speeds[i], maxEntranceByAccel, maxEntranceByPeriod))
// just for logging
let temp = speeds[i]
// stay safe w/ current state at zero
......@@ -164,7 +158,7 @@ export default function Saturn() {
let pi = positions[i]
let pf = positions[i + 1]
let vi = speeds[i]
if (vi > feed) console.warn(`vi at ${i} > feed during RampPass`)
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 * accel * d)
......@@ -182,7 +176,7 @@ export default function Saturn() {
pi: pi,
pf: pf
})
if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
} else if (maxEntry <= vi) {
if (debug) console.log('\\')
ramps.push({
......@@ -192,8 +186,8 @@ export default function Saturn() {
pi: pi,
pf: pf
})
if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
} else if (vi === feed && vf === feed) {
if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
} else if (vi === cruise && vf === cruise) {
if (debug) console.log('--')
ramps.push({
vi: vi,
......@@ -202,54 +196,54 @@ export default function Saturn() {
pi: pi,
pf: pf
})
if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
} else if (vi === feed) {
if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
} else if (vi === cruise) {
if (debug) console.log('--\\')
let dcDist = (Math.pow(feed, 2) - Math.pow(vf, 2)) / (2 * accel)
let dcDist = (Math.pow(cruise, 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,
vf: cruise,
t: (d - dcDist) / cruise,
pi: pi,
pf: pInter
})
if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
// seg 2,
ramps.push({
vi: feed,
vi: cruise,
vf: vf,
t: (feed - vf) / accel,
t: (cruise - 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 (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
} else if (vf === cruise) {
if (debug) console.log('/--')
let acDist = (Math.pow(feed, 2) - Math.pow(vi, 2)) / (2 * accel)
let acDist = (Math.pow(cruise, 2) - Math.pow(vi, 2)) / (2 * accel)
let pInter = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist))
// seg1
ramps.push({
vi: vi,
vf: feed,
t: (feed - vi) / accel,
vf: cruise,
t: (cruise - vi) / accel,
pi: pi,
pf: pInter
})
if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
// seg2
ramps.push({
vi: feed,
vi: cruise,
vf: vf,
t: (d - acDist) / feed,
t: (d - acDist) / cruise,
pi: pInter,
pf: pf
})
if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', 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)
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 (acDist + dcDist < d) {
if (debug) console.log('/--\\')
let pa = math.add(pi, vScalar(vUnitBetween(pi, pf), acDist))
......@@ -257,24 +251,24 @@ export default function Saturn() {
// 3 segs
ramps.push({
vi: vi,
vf: feed,
t: (feed - vi) / accel,
vf: cruise,
t: (cruise - 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,
vi: cruise,
vf: cruise,
t: (d - acDist - dcDist) / cruise,
pi: pa,
pf: pb
})
if (ramps[ramps.length - 1].t < period) console.warn('trouble seg2/3', ramps[ramps.length - 1])
ramps.push({
vi: feed,
vi: cruise,
vf: vf,
t: (feed - vf) / accel,
t: (cruise - vf) / accel,
pi: pb,
pf: pf
})
......@@ -291,7 +285,7 @@ export default function Saturn() {
pi: pi,
pf: pInter
})
if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
ramps.push({
vi: vPeak,
vf: vf,
......@@ -299,7 +293,7 @@ export default function Saturn() {
pi: pInter,
pf: pf
})
if (ramps[ramps.length - 1].t < period) console.warn('trouble', ramps[ramps.length - 1])
if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
}
} // end BIGSWITCH
} // end for-over-positions
......@@ -320,7 +314,7 @@ export default function Saturn() {
// easy increment is just this vector * rate / period
let timeSeg = [0, 0, 0]
timeSeg.forEach((axis, i) => {
timeSeg[i] = vect[i] * (feed / period)
timeSeg[i] = vect[i] * (cruise / period)
})
// now, is this a step-over ? some fast-slow-math for this:
let np = vSum(position, timeSeg)
......@@ -356,60 +350,37 @@ export default function Saturn() {
// first we get all move final v's by jd:
// we jd,
if (positions.length > 63) {
// at the moment, for jd, we'll assume positions[0] is our current position.
// we should time this...
// ok, here's the lookahead routine:
console.time('lookahead')
// we can incorporate this update when we rewrite the loop accordingly
// positions[] is global, speeds is generated now
// speed[0], matching positions[0], are our current situations
let speeds = [speed]
// JD runs an algorithm that calculates maximum allowable
// instantaneous accelerations at corners
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,
// we occasionally (rather, often) need to start decelerating (or accelerating) a few segments before
// the actual constraint: these passes link segments to one another by ensuring that a pass through
// the whole path does not require any accelerations outside of our range.
// the reversepass also sets minimum speeds such that we don't find any moves impossible to execute
// within one network period
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.
// now have allowable maximum speeds at the junctions - the corners - between segments
// but we still need to consider how much acceleration we can do in between these minimum speeds
// i.e. we will now generate the individual accel- deccel- and cruise phases of each segment
// *now* since we are operating on a period basis, we also tune these segments such that
// (1) none are smaller than one period of time and (2) all take some integer division of periods to complete
let ramps = RampPass(speeds)
// now we're done,
console.timeLog('lookahead')
console.log(`have ${ramps.length} ramps for ${positions.length} positions`)
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...
// we do one last check:
for (let r of ramps) {
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))
if (r.vi < minSpeed || r.vf < minSpeed || r.vi > cruise || r.vf > cruise || r.t < period) {
console.warn(`troublesome ramp found on final check`, r)
}
}
// run once,
......
libs/smallvectors.js
+ 1
− 1
View file @ 2b79ab62
scratch/rampcheck.js 0 → 100644
+ 26
− 0
View file @ 2b79ab62
// scratch notes
let fn = () => {
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
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment