@@ -422,33 +422,36 @@ void FxdTiCtrl::reset() {
422422 stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0 ;
423423
424424 for (uint32_t i = 0U ; i < (FTM_BATCH_SIZE); i++) { // Reset trajectory history
425- TERN_ (HAS_X_AXIS, xd[i] = 0 .0f );
426- TERN_ (HAS_Y_AXIS, yd[i] = 0 .0f );
427- TERN_ (HAS_Z_AXIS, zd[i] = 0 .0f );
425+ TERN_ (HAS_X_AXIS, xd[i] = 0 .0f );
426+ TERN_ (HAS_Y_AXIS, yd[i] = 0 .0f );
427+ TERN_ (HAS_Z_AXIS, zd[i] = 0 .0f );
428428 TERN_ (HAS_EXTRUDERS, ed[i] = 0 .0f );
429429 }
430430
431431 blockProcRdy = blockProcRdy_z1 = blockProcDn = false ;
432432 batchRdy = batchRdyForInterp = false ;
433433 runoutEna = false ;
434434
435- TERN_ (HAS_X_AXIS, x_endPosn_prevBlock = 0 .0f );
436- TERN_ (HAS_Y_AXIS, y_endPosn_prevBlock = 0 .0f );
437- TERN_ (HAS_Z_AXIS, z_endPosn_prevBlock = 0 .0f );
435+ TERN_ (HAS_X_AXIS, x_endPosn_prevBlock = 0 .0f );
436+ TERN_ (HAS_Y_AXIS, y_endPosn_prevBlock = 0 .0f );
437+ TERN_ (HAS_Z_AXIS, z_endPosn_prevBlock = 0 .0f );
438438 TERN_ (HAS_EXTRUDERS, e_endPosn_prevBlock = 0 .0f );
439439
440440 makeVector_idx = makeVector_idx_z1 = 0 ;
441441 makeVector_batchIdx = FTM_BATCH_SIZE;
442442
443- TERN_ (HAS_X_AXIS, x_steps = 0 );
444- TERN_ (HAS_Y_AXIS, y_steps = 0 );
445- TERN_ (HAS_Z_AXIS, z_steps = 0 );
443+ TERN_ (HAS_X_AXIS, x_steps = 0 );
444+ TERN_ (HAS_Y_AXIS, y_steps = 0 );
445+ TERN_ (HAS_Z_AXIS, z_steps = 0 );
446446 TERN_ (HAS_EXTRUDERS, e_steps = 0 );
447+
447448 interpIdx = interpIdx_z1 = 0 ;
448- TERN_ (HAS_X_AXIS, x_dirState = stepDirState_NOT_SET);
449- TERN_ (HAS_Y_AXIS, y_dirState = stepDirState_NOT_SET);
450- TERN_ (HAS_Z_AXIS, z_dirState = stepDirState_NOT_SET);
449+
450+ TERN_ (HAS_X_AXIS, x_dirState = stepDirState_NOT_SET);
451+ TERN_ (HAS_Y_AXIS, y_dirState = stepDirState_NOT_SET);
452+ TERN_ (HAS_Z_AXIS, z_dirState = stepDirState_NOT_SET);
451453 TERN_ (HAS_EXTRUDERS, e_dirState = stepDirState_NOT_SET);
454+
452455 nextStepTicks = FTM_MIN_TICKS;
453456
454457 #if HAS_X_AXIS
@@ -486,28 +489,28 @@ void FxdTiCtrl::loadBlockData(block_t * const current_block) {
486489 #if HAS_X_AXIS
487490 x_startPosn = x_endPosn_prevBlock;
488491 float x_moveDist = current_block->steps .a / planner.settings .axis_steps_per_mm [X_AXIS];
489- if (direction.x ) x_moveDist *= -1 .0f ;
492+ if (! direction.x ) x_moveDist *= -1 .0f ;
490493 x_Ratio = x_moveDist * oneOverLength;
491494 #endif
492495
493496 #if HAS_Y_AXIS
494497 y_startPosn = y_endPosn_prevBlock;
495498 float y_moveDist = current_block->steps .b / planner.settings .axis_steps_per_mm [Y_AXIS];
496- if (direction.y ) y_moveDist *= -1 .0f ;
499+ if (! direction.y ) y_moveDist *= -1 .0f ;
497500 y_Ratio = y_moveDist * oneOverLength;
498501 #endif
499502
500503 #if HAS_Z_AXIS
501504 z_startPosn = z_endPosn_prevBlock;
502505 float z_moveDist = current_block->steps .c / planner.settings .axis_steps_per_mm [Z_AXIS];
503- if (direction.z ) z_moveDist *= -1 .0f ;
506+ if (! direction.z ) z_moveDist *= -1 .0f ;
504507 z_Ratio = z_moveDist * oneOverLength;
505508 #endif
506509
507510 #if HAS_EXTRUDERS
508511 e_startPosn = e_endPosn_prevBlock;
509512 float extrusion = current_block->steps .e / planner.settings .axis_steps_per_mm [E_AXIS_N (current_block->extruder )];
510- if (direction.e ) extrusion *= -1 .0f ;
513+ if (! direction.e ) extrusion *= -1 .0f ;
511514 e_Ratio = extrusion * oneOverLength;
512515 #endif
513516
@@ -568,31 +571,31 @@ void FxdTiCtrl::loadBlockData(block_t * const current_block) {
568571 // One less than (Accel + Coasting + Decel) datapoints
569572 max_intervals = N1 + N2 + N3 - 1U ;
570573
571- TERN_ (HAS_X_AXIS, x_endPosn_prevBlock += x_moveDist);
572- TERN_ (HAS_Y_AXIS, y_endPosn_prevBlock += y_moveDist);
573- TERN_ (HAS_Z_AXIS, z_endPosn_prevBlock += z_moveDist);
574+ TERN_ (HAS_X_AXIS, x_endPosn_prevBlock += x_moveDist);
575+ TERN_ (HAS_Y_AXIS, y_endPosn_prevBlock += y_moveDist);
576+ TERN_ (HAS_Z_AXIS, z_endPosn_prevBlock += z_moveDist);
574577 TERN_ (HAS_EXTRUDERS, e_endPosn_prevBlock += extrusion);
575578}
576579
577580// Generate data points of the trajectory.
578581void FxdTiCtrl::makeVector () {
579- float accel_k = 0 .0f ; // (mm/s^2) Acceleration K factor
580- float tau = (makeVector_idx + 1 ) * (FTM_TS); // (s) Time since start of block
581- float dist = 0 .0f ; // (mm) Distance traveled
582+ float accel_k = 0 .0f ; // (mm/s^2) Acceleration K factor
583+ float tau = (makeVector_idx + 1 ) * (FTM_TS); // (s) Time since start of block
584+ float dist = 0 .0f ; // (mm) Distance traveled
582585
583586 if (makeVector_idx < N1) {
584587 // Acceleration phase
585- dist = (f_s * tau) + (0 .5f * accel_P * sq (tau)); // (mm) Distance traveled for acceleration phase
586- accel_k = accel_P; // (mm/s^2) Acceleration K factor from Accel phase
588+ dist = (f_s * tau) + (0 .5f * accel_P * sq (tau)); // (mm) Distance traveled for acceleration phase
589+ accel_k = accel_P; // (mm/s^2) Acceleration K factor from Accel phase
587590 }
588591 else if (makeVector_idx >= N1 && makeVector_idx < (N1 + N2)) {
589592 // Coasting phase
590- dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase
593+ dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase
591594 // accel_k = 0.0f;
592595 }
593596 else {
594597 // Deceleration phase
595- const float tau_ = tau - (N1 + N2) * (FTM_TS); // (s) Time since start of decel phase
598+ const float tau_ = tau - (N1 + N2) * (FTM_TS); // (s) Time since start of decel phase
596599 dist = s_2e + F_P * tau_ + 0 .5f * decel_P * sq (tau_); // (mm) Distance traveled for deceleration phase
597600 accel_k = decel_P; // (mm/s^2) Acceleration K factor from Decel phase
598601 }
@@ -614,7 +617,7 @@ void FxdTiCtrl::makeVector() {
614617 }
615618 else {
616619 ed[makeVector_batchIdx] = new_raw_z1;
617- // Alternatively: coordArray_e [makeVector_batchIdx] = e_startDist + extrusion / (N1 + N2 + N3);
620+ // Alternatively: ed [makeVector_batchIdx] = e_startPosn + (e_Ratio * dist) / (N1 + N2 + N3);
618621 }
619622 #endif
620623
0 commit comments