diff --git a/modules/hardware/motors/stepper b/modules/hardware/motors/stepper index 8ef0a123434e498a077f6cbd83e4e07e9e8344bf..3a912ce71630a02b464f832b19e700984609f913 100644 --- a/modules/hardware/motors/stepper +++ b/modules/hardware/motors/stepper @@ -292,7 +292,7 @@ trap.accelLength = 0; // never accel // we really want speed by multiple times, and total distance we know, for vAvg var tSum = (trap.cruise - trap.exit) / trap.accel + trap.deccelLength / trap.cruise - trap.vAvg = trap.delta / tSum + trap.vAvg = trap.length / tSum } else if (trap.cruise == trap.exit) { // accel, cruise? unless entry also cruise? //console.log('accel then cruise') @@ -301,7 +301,7 @@ trap.deccelLength = trap.length + 1; // and never deccel // wait var tSum = (trap.cruise - trap.entry) / trap.accel + (trap.delta - trap.accellength) / trap.cruise - trap.vAvg = trap.dela / tSum + trap.vAvg = trap.length / tSum } else { // it's either a full trap, or a triangle var accelDistance = (trap.cruise * trap.cruise - trap.entry * trap.entry) / (2 * trap.accel) @@ -313,7 +313,7 @@ trap.deccelLength = trap.length - deccelDistance // wait var tSum = (trap.cruise - trap.entry) / trap.accel + (trap.deccelLength - trap.accellength) / trap.cruise + (trap.cruise - trap.exit) / trap.accel - trap.vAvg = trap.delta / tSum + trap.vAvg = trap.length / tSum } else { //console.log('triangle') trap.ramptype = 'triangle' @@ -323,8 +323,8 @@ var midPoint = (vPeak * vPeak - trap.entry * trap.entry) / (2 * trap.accel) trap.accelLength = midPoint trap.deccelLength = midPoint - var tSum = (trap.vPeak - trap.entry) / trap.accel + (trap.vPeak - trap.exit) / trap.accel - trap.vAvg = trap.delta / tSum + var tSum = (vPeak - trap.entry) / trap.accel + (vPeak - trap.exit) / trap.accel + trap.vAvg = trap.length / tSum } } @@ -362,10 +362,10 @@ packet.push(132) // now we pack these values into C-readable bytes // int32_t steps, uint32_t entryspeed, accel, accellength, decellength - packet = packet.concat(pack32(stepWait.delta)) + packet = packet.concat(pack32(trap.delta)) if(isNaN(stepWait.vAvg)){ - console.log('maths failure on vAvg') + console.log('maths failure on vAvg', trap.ramptype) packet = packet.concat(pack32(1000)) console.log('wait block', stepWait.steps, 1000) } else { @@ -396,7 +396,7 @@ function trapezoidToStepWait(trap) { var stepWait = {} // planner units -> steps - stepWait.steps = Math.round(trap.delta * mod.convert) + stepWait.steps = Math.round(trap.length * mod.convert) // stepWait.vAvg = Math.round(trap.vAvg * mod.convert)