From 72017d14c6b01bb772f6020019a498b9fd1ee329 Mon Sep 17 00:00:00 2001
From: Jake Read <jake.read@cba.mit.edu>
Date: Tue, 29 Oct 2019 19:35:22 -0400
Subject: [PATCH] bugfix not checking for speeds > cruise speed
---
hunks/adhoc/saturn.js | 36 ++++++++++++++++++++++++++----------
1 file changed, 26 insertions(+), 10 deletions(-)
diff --git a/hunks/adhoc/saturn.js b/hunks/adhoc/saturn.js
index 7c6bb58..f809d6b 100644
--- a/hunks/adhoc/saturn.js
+++ b/hunks/adhoc/saturn.js
@@ -29,7 +29,7 @@ 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) => {
+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
@@ -61,14 +61,14 @@ let JD = (positions, speeds, deviation, accel, minSpeed) => {
// walk for minspeeds
for (let s in speeds) {
if (speeds[s] < minSpeed) speeds[s] = minSpeed
+ if (speeds[s] > cruise) speeds[s] = cruise
}
// that's it for us
return speeds
}
-let ReversePass = (positions, speeds, accel, minSpeed) => {
+let ReversePass = (positions, speeds, accel, minSpeed, debug) => {
// 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])
@@ -91,9 +91,8 @@ let ReversePass = (positions, speeds, accel, minSpeed) => {
}
}
-let ForwardPass = (positions, speeds, accel, minSpeed) => {
+let ForwardPass = (positions, speeds, accel, minSpeed, debug) => {
// 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])
@@ -114,13 +113,13 @@ let ForwardPass = (positions, speeds, accel, minSpeed) => {
// here is assuming positions[0] is current position, for which speed is the current velocity
}
-let RampPass = (positions, speeds, ramps, a, cruise) => {
- let debug = false
+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)
@@ -138,6 +137,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
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({
@@ -147,6 +147,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
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({
@@ -156,6 +157,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
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)
@@ -168,6 +170,7 @@ let RampPass = (positions, speeds, ramps, a, 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,
@@ -176,6 +179,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
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)
@@ -188,6 +192,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
pi: pi,
pf: pInter
})
+ if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
// seg2
ramps.push({
vi: cruise,
@@ -196,10 +201,11 @@ let RampPass = (positions, speeds, ramps, a, 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(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))
@@ -211,6 +217,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
pi: pi,
pf: pa
})
+ if(ramps[ramps.length - 1].t < 0) console.warn('trouble seg1/3', ramps[ramps.length - 1])
ramps.push({
vi: cruise,
vf: cruise,
@@ -218,6 +225,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
pi: pa,
pf: pb
})
+ if(ramps[ramps.length - 1].t < 0) console.warn('trouble seg2/3', ramps[ramps.length - 1])
ramps.push({
vi: cruise,
vf: vf,
@@ -225,6 +233,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
pi: pb,
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))
@@ -237,6 +246,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
pi: pi,
pf: pInter
})
+ if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
ramps.push({
vi: vPeak,
vf: vf,
@@ -244,6 +254,7 @@ let RampPass = (positions, speeds, ramps, a, cruise) => {
pi: pInter,
pf: pf
})
+ if(ramps[ramps.length - 1].t < 0) console.warn('trouble', ramps[ramps.length - 1])
}
} // end BIGSWITCH
}
@@ -333,12 +344,13 @@ 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)
+ JD(positions, speeds, deviation, accel, minSpeed, feed)
//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)
+ // 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
@@ -346,9 +358,13 @@ export default function Saturn() {
// 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)
+ RampPass(positions, speeds, ramps, accel, feed, true)
console.timeEnd('lookahead')
console.log(`have ${ramps.length} ramps for ${positions.length} positions`)
+ console.log('times')
+ for(let r of ramps){
+ console.log(r.t)
+ }
// run once,
throw new Error('halt')
}
--
GitLab