diff --git a/FluidNC/src/Kinematics/WallPlotter.cpp b/FluidNC/src/Kinematics/WallPlotter.cpp index 38440ed15..d6d50ab27 100644 --- a/FluidNC/src/Kinematics/WallPlotter.cpp +++ b/FluidNC/src/Kinematics/WallPlotter.cpp @@ -49,7 +49,7 @@ namespace Kinematics { // calculate the total X,Y axis move distance // Z axis is the same in both coord systems, so it does not undergo conversion - float dist = vector_distance(target, position, Y_AXIS); + float dist = vector_distance(target, position, 2); // Only compute distance for both axes. X and Y // Segment our G1 and G0 moves based on yaml file. If we choose a small enough _segment_length we can hide the nonlinearity segment_count = dist / _segment_length; if (segment_count < 1) { // Make sure there is at least one segment, even if there is no movement @@ -57,23 +57,24 @@ namespace Kinematics { // the planner even if there is no movement?? segment_count = 1; } - // Calc distance of individual segments - dx = (target[X_AXIS] - position[X_AXIS]) / segment_count; - dy = (target[Y_AXIS] - position[Y_AXIS]) / segment_count; - dz = (target[Z_AXIS] - position[Z_AXIS]) / segment_count; - // Current cartesian end point of the segment - float seg_x = position[X_AXIS]; + // Calc distance of individual cartesian segments + dx = (target[X_AXIS] - position[X_AXIS])/segment_count; + dy = (target[Y_AXIS] - position[Y_AXIS])/segment_count; + dz = (target[Z_AXIS] - position[Z_AXIS])/segment_count; + // Current cartesian end point of the segment + float seg_x = position[X_AXIS]; float seg_y = position[Y_AXIS]; float seg_z = position[Z_AXIS]; + // Calculate desired cartesian feedrate distance ratio. Same for each seg. + float vdcart_ratio = pl_data->feed_rate/(dist/segment_count); for (uint32_t segment = 1; segment <= segment_count; segment++) { // calc next cartesian end point of the next segment seg_x += dx; seg_y += dy; seg_z += dz; - float seg_left, seg_right; + // Convert cartesian space coords to motor space + float seg_left, seg_right; xy_to_lengths(seg_x, seg_y, seg_left, seg_right); - // TODO: Need to adjust cartesian feedrate to motor/plotter space, just leave them alone for now - #ifdef USE_CHECKED_KINEMATICS // Check the inverse computation. float cx, cy; @@ -84,18 +85,26 @@ namespace Kinematics { // FIX: Produce an alarm state? } #endif // end USE_CHECKED_KINEMATICS - - // mc_move_motors() returns false if a jog is cancelled. - // In that case we stop sending segments to the planner. - + // Set interpolated feedrate in motor space for this segment to + // get desired feedrate in cartesian space + if (!pl_data->motion.rapidMotion) { // Rapid motions ignore feedrate. Don't convert. + // T=D/V, Tcart=Tmotor, Dcart/Vcart=Dmotor/Vmotor + // Vmotor = Dmotor*(Vcart/Dcart) + pl_data->feed_rate = hypot_f(last_left-seg_left,last_right-seg_right)*vdcart_ratio; + } + // TODO: G93 pl_data->motion.inverseTime logic?? Does this even make sense for wallplotter? + // Remember absolute motor lengths for next time last_left = seg_left; last_right = seg_right; last_z = seg_z; - + // Initiate motor movement with converted feedrate and converted position + // mc_move_motors() returns false if a jog is cancelled. + // In that case we stop sending segments to the planner. // Note that the left motor runs backward. // TODO: It might be better to adjust motor direction in .yaml file by inverting direction pin?? - float cables[MAX_N_AXIS] = { 0 - (last_left - zero_left), 0 + (last_right - zero_right), seg_z }; + float cables[MAX_N_AXIS] = { 0 - (seg_left - zero_left), 0 + (seg_right - zero_right), seg_z }; if (!mc_move_motors(cables, pl_data)) { + // TODO fixup last_left last_right?? What is position state when jog is cancelled? return false; } } diff --git a/FluidNC/src/Spindles/BESCSpindle.cpp b/FluidNC/src/Spindles/BESCSpindle.cpp index 9835be241..417ab621b 100644 --- a/FluidNC/src/Spindles/BESCSpindle.cpp +++ b/FluidNC/src/Spindles/BESCSpindle.cpp @@ -52,11 +52,9 @@ namespace Spindles { shelfSpeeds(4000, 20000); } - // We set the dev_speed scale in the speed map to the full PWM period. - // Then, in set_output, we map the dev_speed range of 0..64K to the pulse - // length range of ~1ms .. 2ms - setupSpeeds(_pwm->period()); - + // Use yaml speed_map to setup speed map for "spindle speed" conversion to timer counts used by PWM controller + //setupSpeeds(_pulse_span_counts); // Map the counts for just the part of the pulse that changes to keep math inside 32bits later... + setupSpeeds(_pwm->period()); // Map the entire pulse width period in counts stop(); config_message(); } @@ -73,19 +71,22 @@ namespace Spindles { _current_pwm_duty = duty; - // This maps the dev_speed range of 0.._pwm->period() into the pulse length + // This maps the dev_speed range of 0..(1<<_pwm_precision) into the pulse length // where _min_pulse_counts represents off and (_min_pulse_counts + _pulse_span_counts) // represents full on. Typically the off value is a 1ms pulse length and the // full on value is a 2ms pulse. - uint32_t pulse_counts = _min_pulse_counts + ((duty * _pulse_span_counts) / _pwm->period()); + // uint32_t pulse_counts = _min_pulse_counts + (_pulse_span_counts * (uint64_t) duty)/_pwm->period(); + _pwm->setDuty(_min_pulse_counts + (_pulse_span_counts * (uint64_t) duty)/_pwm->period()); + // _pwm->setDuty(_min_pulse_counts+duty); // More efficient by keeping math within 32bits?? + // log_info(name() << " duty:" << duty << " _min_pulse_counts:" << _min_pulse_counts + // << " _pulse_span_counts:" << _pulse_span_counts << " pulse_counts" << pulse_counts); - _pwm->setDuty(pulse_counts); } // prints the startup message of the spindle config void BESC::config_message() { log_info(name() << " Spindle Out:" << _output_pin.name() << " Min:" << _min_pulse_us << "us Max:" << _max_pulse_us - << "us Freq:" << _pwm->frequency() << "Hz Period:" << _pwm->period()); + << "us Freq:" << _pwm->frequency() << "Hz Full Period count:" << _pwm->period()); } // Configuration registration diff --git a/FluidNC/src/Spindles/BESCSpindle.h b/FluidNC/src/Spindles/BESCSpindle.h index 9bfec6a38..912635738 100644 --- a/FluidNC/src/Spindles/BESCSpindle.h +++ b/FluidNC/src/Spindles/BESCSpindle.h @@ -34,8 +34,8 @@ namespace Spindles { const uint32_t besc_pwm_max_freq = 2000; // 50 Hz // Calculated - uint16_t _pulse_span_counts; // In counts of a 16-bit counter - uint16_t _min_pulse_counts; // In counts of a 16-bit counter + uint32_t _pulse_span_counts; // In counts of a 32-bit counter. ESP32 uses up to 20bits + uint32_t _min_pulse_counts; // In counts of a 32-bit counter ESP32 uses up to 20bits protected: // Configurable diff --git a/FluidNC/src/Spindles/PWMSpindle.h b/FluidNC/src/Spindles/PWMSpindle.h index 273590759..7e0f3385c 100644 --- a/FluidNC/src/Spindles/PWMSpindle.h +++ b/FluidNC/src/Spindles/PWMSpindle.h @@ -59,7 +59,7 @@ namespace Spindles { virtual ~PWM() {} protected: - int32_t _current_pwm_duty = 0; + uint32_t _current_pwm_duty = 0; PwmPin* _pwm = nullptr; // Configurable diff --git a/example_configs/WallPlotter.yaml b/example_configs/WallPlotter.yaml index 777c2f456..82452986c 100644 --- a/example_configs/WallPlotter.yaml +++ b/example_configs/WallPlotter.yaml @@ -104,7 +104,7 @@ besc: spindown_ms: 2000 tool_num: 100 speed_map: 0=0.000% 255=100.000% - min_pulse_us: 600 - max_pulse_us: 2300 + min_pulse_us: 700 + max_pulse_us: 2200