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

bugfix not checking for speeds > cruise speed

parent 73cbed4c
Branches
No related tags found
No related merge requests found
......@@ -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')
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment