*/
using namespace std;
-#include <vector>
#include "mri.h"
#include "nuts_bolts.h"
}
// info needed by laser
- block->s_value= s_value;
- block->is_g123= g123;
+ block->s_value = roundf(s_value*(1<<11)); // 1.11 fixed point
+ block->is_g123 = g123;
// use default JD
float junction_deviation = this->junction_deviation;
if(block->steps[GAMMA_STEPPER] != 0) {
// z only move
if(!isnan(this->z_junction_deviation)) junction_deviation = this->z_junction_deviation;
+
} else {
// is not a primary axis move
- block->primary_axis = false;
+ block->primary_axis= false;
+ #if N_PRIMARY_AXIS > 3
+ for (int i = 3; i < N_PRIMARY_AXIS; ++i) {
+ if(block->steps[i] != 0){
+ block->primary_axis= true;
+ break;
+ }
+ }
+ #endif
+
}
}
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float cos_theta = - this->previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- this->previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- - this->previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
+ - this->previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS];
+ #if N_PRIMARY_AXIS > 3
+ for (int i = 3; i < N_PRIMARY_AXIS; ++i) {
+ cos_theta -= this->previous_unit_vec[i] * unit_vec[i];
+ }
+ #endif
// Skip and use default max junction speed for 0 degree acute junction.
- if (cos_theta < 0.95F) {
+ if (cos_theta <= 0.9999F) {
vmax_junction = std::min(previous_nominal_speed, block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
- if (cos_theta > -0.95F) {
+ if (cos_theta >= -0.9999F) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
float sin_theta_d2 = sqrtf(0.5F * (1.0F - cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = std::min(vmax_junction, sqrtf(acceleration * junction_deviation * sin_theta_d2 / (1.0F - sin_theta_d2)));