From b3ade97424554b6b9ce27481f107140a6ad4c577 Mon Sep 17 00:00:00 2001
From: Jake Read <jake.read@cba.mit.edu>
Date: Wed, 30 Oct 2019 10:51:26 -0400
Subject: [PATCH] blockpass
---
hunks/adhoc/saturn.js | 89 +++++++++++++++++++++++++++++--------------
scratch/rampcheck.js | 26 -------------
2 files changed, 60 insertions(+), 55 deletions(-)
delete mode 100644 scratch/rampcheck.js
diff --git a/hunks/adhoc/saturn.js b/hunks/adhoc/saturn.js
index 9bcbfc3..9412772 100644
--- a/hunks/adhoc/saturn.js
+++ b/hunks/adhoc/saturn.js
@@ -59,7 +59,7 @@ export default function Saturn() {
let speed = minSpeed // currently
let posUpdated = false
- let JD = (speeds, debug) => {
+ let jd = (speeds, debug) => {
//console.log('positions', positions)
let calcJunctionSpeed = (p0, p1, p2, jd) => {
// junction speed at p1, arrival from p0 exit to p2
@@ -97,7 +97,7 @@ export default function Saturn() {
return speeds
}
- let ReversePass = (speeds, debug) => {
+ 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--) {
@@ -115,7 +115,7 @@ export default function Saturn() {
// v2, where t = period (1.5 is a safety factor for later rounding)
let maxEntranceByPeriod = (period * 1.5) / (2 * d) - speeds[i]
- // set the entrance speed to the min of JD or our Max Entrance, but no lower than the minspeed
+ // 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], maxEntranceByAccel, maxEntranceByPeriod))
// just for logging
let temp = speeds[i]
@@ -129,7 +129,7 @@ export default function Saturn() {
}
}
- let ForwardPass = (speeds, debug) => {
+ 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])
@@ -151,16 +151,15 @@ export default function Saturn() {
// here is assuming positions[0] is current position, for which speed is the current velocity
}
- let RampPass = (speeds, debug) => {
+ 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 > 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 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}`)
@@ -191,11 +190,9 @@ export default function Saturn() {
// since we already swept for these cases in the revpass
if (debug) console.log(`/`)
writeUpTick()
- 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('\\')
writeDownTick()
- if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
} else if (vi === cruise && vf === cruise) {
// similarely, since we're not segmenting cruise any farther, it should also be OK
if (debug) console.log('--')
@@ -206,7 +203,6 @@ export default function Saturn() {
pi: pi,
pf: pf
})
- 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(cruise, 2) - Math.pow(vf, 2)) / (2 * accel)
@@ -229,7 +225,6 @@ export default function Saturn() {
pi: pi,
pf: pInter
})
- if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
// seg 2,
ramps.push({
vi: cruise,
@@ -238,7 +233,6 @@ export default function Saturn() {
pi: pInter,
pf: pf
})
- 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('/--')
@@ -258,7 +252,6 @@ export default function Saturn() {
pi: pi,
pf: pInter
})
- if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
// seg2
ramps.push({
vi: cruise,
@@ -267,7 +260,6 @@ export default function Saturn() {
pi: pInter,
pf: pf
})
- if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
}
} else {
let dcDist = (Math.pow(cruise, 2) - Math.pow(vf, 2)) / (2 * accel)
@@ -295,7 +287,6 @@ export default function Saturn() {
pi: pi,
pf: pInter
})
- if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
ramps.push({
vi: vPeak,
vf: vf,
@@ -303,7 +294,6 @@ export default function Saturn() {
pi: pInter,
pf: pf
})
- if (ramps[ramps.length - 1].t < period) console.warn('RampPass generates too-short ramp:', ramps[ramps.length - 1])
}
}
if (acDist + dcDist < d) {
@@ -386,7 +376,6 @@ export default function Saturn() {
pi: pi,
pf: pa
})
- if (ramps[ramps.length - 1].t < period) console.warn('trouble seg1/3', ramps[ramps.length - 1])
ramps.push({
vi: cruise,
vf: cruise,
@@ -394,7 +383,6 @@ export default function Saturn() {
pi: pa,
pf: pb
})
- if (ramps[ramps.length - 1].t < period) console.warn('trouble seg2/3', ramps[ramps.length - 1])
ramps.push({
vi: cruise,
vf: vf,
@@ -402,7 +390,6 @@ export default function Saturn() {
pi: pb,
pf: pf
})
- if (ramps[ramps.length - 1].t < period) console.warn('trouble seg3/3', ramps[ramps.length - 1])
}
} else {
// the actual triangle case
@@ -413,6 +400,42 @@ export default function Saturn() {
return ramps
}
+ let roundPass = (ramps, debug) => {
+ 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)
+ // does this calc match the ramp time? yes
+ // so ...
+ }
+ }
+
+ let blockPass = (ramps, debug) => {
+ let blocks = []
+ for(let i = 0; i < ramps.length; i++){
+ let r = ramps[i]
+ let d = vDist(r.pi, r.pf)
+ // how many blocks are we going to split it to?
+ let count = r.t / period
+ let integer = Math.round(count)
+ if(integer < 1) {
+ console.warn(`small ramp during blockPass at ${i}`)
+ integer = 1
+ }
+ // the pos'ns to split to:
+ let vu = vUnitBetween(r.pi, r.pf)
+ // now just...
+ for(let b = 0; b < integer; b ++){
+ // percentage through,
+ let start = b / integer
+ let finish = (b + 1) / integer
+ blocks.push([math.add(r.pi, vScalar(vu, start)), math.add(r.pi, vScalar(vu, finish))])
+ }
+ }
+ return blocks
+ }
/*
let calculateNextIncrement = () => {
@@ -468,34 +491,42 @@ export default function Saturn() {
// 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
+ // jd runs an algorithm that calculates maximum allowable
// instantaneous accelerations at corners
- JD(speeds)
+ jd(speeds)
// 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)
+ reversePass(speeds)
+ forwardPass(speeds)
console.timeLog('lookahead')
// 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
- console.timeLog('lookahead')
+ let ramps = rampPass(speeds)
+ // or,
+ //let ramps = timeSegPass(speeds)
// we do one last check:
for (let r of ramps) {
+ //console.log(`${r.t.toFixed(3)}`)
if (r.vi < minSpeed || r.vf < minSpeed || r.vi > (cruise * 1.1) || r.vf > (cruise * 1.1) || r.t < period) {
console.warn(`troublesome ramp found on final check`, r)
}
}
+ // now we're done,
+ console.timeLog('lookahead')
+ console.log(`have ${ramps.length} ramps for ${positions.length} positions`)
+ // should see about writing out motion blocks:
+ let blocks = blockPass(ramps, true)
+ console.log(`have ${blocks.length} blocks for ${ramps.length} ramps`)
+ console.timeEnd('lookahead') // 49ms ... to write ~ 3k blocks from 145 ramps from 64 positions
+ // OK: I think this is it... I'll wrap the loop again to do this once, then write those blocks out at the
+ // flowcontrol'd interval... and see how it looks. this is maybe even enough: if steppers just make these counts
+ // of steps, distance is preserved, but some jerk every 50ms... interpolation next
// run once,
throw new Error('halt')
}
diff --git a/scratch/rampcheck.js b/scratch/rampcheck.js
deleted file mode 100644
index 47e1a9b..0000000
--- a/scratch/rampcheck.js
+++ /dev/null
@@ -1,26 +0,0 @@
-
-// 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
- }
-}
--
GitLab