Skip to content

Commit

Permalink
Updates after some testing
Browse files Browse the repository at this point in the history
  • Loading branch information
@bdring
bdring committed Aug 10, 2023
1 parent 6af2505 commit d9ab16b
Show file tree
Hide file tree
Showing 6 changed files with 58 additions and 85 deletions.
123 changes: 48 additions & 75 deletions FluidNC/src/Kinematics/ParallelDelta.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,19 @@ namespace Kinematics {

void ParallelDelta::init() {
// print a startup message to show the kinematics are enabled. Print the offset for reference
log_info("Kinematic system: " << name());
log_info("Kinematic system:" << name() << " soft_limits:" << _softLimits);

auto axes = config->_axes;
auto n_axis = config->_axes->_numberAxis;

// warn about axissofy limits
for (int axis = 0; axis < n_axis; axis++) {
if (axes->_axis[axis]->_softLimits) {
log_warn(" All soft_limits configured in axes should be false");
break;
}
}

init_position();
}

Expand All @@ -94,67 +106,47 @@ namespace Kinematics {
log_info(" Z Offset:" << cartesian[Z_AXIS]);
}


bool ParallelDelta::invalid_line(float* cartesian) {
if (!_softLimits)
return false;

float motors[MAX_N_AXIS] = { 0.0, 0.0, 0.0 };

if (!transform_cartesian_to_motors(motors, cartesian)) {
limit_error();
return true;
}

return false;
}

// TO DO. This is not supported yet. Other levels of protection will prevent "damage"
bool ParallelDelta::invalid_arc(
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) {
return false;
}

// copied from Cartensian. Needs to be optimized for parallel delta.
void ParallelDelta::constrain_jog(float* target, plan_line_data_t* pl_data, float* position) {
auto axes = config->_axes;
auto n_axis = config->_axes->_numberAxis;

float* current_position = get_mpos();
MotorMask lim_pin_state = limits_get_state();
// log_debug("Jog Test: from(" << position[X_AXIS] << ")"
// << " to(" << target[X_AXIS] << ")");
if (!_softLimits)
return;

for (int axis = 0; axis < n_axis; axis++) {
auto axisSetting = axes->_axis[axis];
// If the axis is moving from the current location and soft limits are on.
if (axisSetting->_softLimits && target[axis] != current_position[axis]) {
// When outside the axis range, only small nudges to clear switches are allowed
bool move_positive = target[axis] > current_position[axis];
if ((!move_positive && (current_position[axis] < limitsMinPosition(axis))) ||
(move_positive && (current_position[axis] > limitsMaxPosition(axis)))) {
// only allow a nudge if a switch is active
if (bitnum_is_false(lim_pin_state, Machine::Axes::motor_bit(axis, 0)) &&
bitnum_is_false(lim_pin_state, Machine::Axes::motor_bit(axis, 1))) {
target[axis] = current_position[axis]; // cancel the move on this axis
log_debug("Soft limit violation on " << Machine::Axes::_names[axis]);
continue;
}
float jog_dist = target[axis] - current_position[axis];

MotorMask axisMotors = Machine::Axes::axes_to_motors(1 << axis);
bool posLimited = bits_are_true(Machine::Axes::posLimitMask, axisMotors);
bool negLimited = bits_are_true(Machine::Axes::negLimitMask, axisMotors);

// if jog is positive and only the positive switch is active, then kill the move
// if jog is negative and only the negative switch is active, then kill the move
if (posLimited != negLimited) { // XOR, because ambiguous (both) is OK
if ((negLimited && (jog_dist < 0)) || (posLimited && (jog_dist > 0))) {
target[axis] = current_position[axis]; // cancel the move on this axis
log_debug("Jog into active switch blocked on " << Machine::Axes::_names[axis]);
continue;
}
}

auto nudge_max = axisSetting->_motors[0]->_pulloff;
if (abs(jog_dist) > nudge_max) {
target[axis] = (jog_dist >= 0) ? current_position[axis] + nudge_max : current_position[axis] + nudge_max;
log_debug("Jog amount limited when outside soft limits")
}
continue;
}
float motors[MAX_N_AXIS] = { 0.0, 0.0, 0.0 };

if (target[axis] < limitsMinPosition(axis)) {
target[axis] = limitsMinPosition(axis);
} else if (target[axis] > limitsMaxPosition(axis)) {
target[axis] = limitsMaxPosition(axis);
} else {
continue;
}
log_debug("Jog constrained to axis range");
}
// Temp fix
// If the target is reachable do nothing
if (transform_cartesian_to_motors(motors, target)) {
return;
} else {
log_warn("Kinematics soft limit jog rejection");
copyAxes(target, position);
}
pl_data->limits_checked = true;

// TO DO better idea
// loop back from the target in increments of kinematic_segment_len_mm unitl the position is valid.
// constrain to that target.
}

bool ParallelDelta::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) {
Expand Down Expand Up @@ -288,18 +280,11 @@ namespace Kinematics {
cartesian[X_AXIS] = (a1 * cartesian[Z_AXIS] + b1) / dnm;
cartesian[Y_AXIS] = (a2 * cartesian[Z_AXIS] + b2) / dnm;
}
/*
bool ParallelDelta::canHome(AxisMask& axisMask) {
return true; // signal main code that this handled all homing
}
*/


bool ParallelDelta::kinematics_homing(AxisMask& axisMask) {
auto axes = config->_axes;
auto n_axis = axes->_numberAxis;

log_debug("kinematics_homing");
config->_axes->set_disable(false);

// TODO deal with non kinematic axes above Z
Expand All @@ -322,7 +307,7 @@ namespace Kinematics {
// discriminant
float d = -(a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf);
if (d < 0) {
log_warn("Kinematics: Target unreachable");
//log_warn("Kinematics: Target unreachable");
return false;
} // non-existing point
float yj = (y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point
Expand All @@ -333,18 +318,6 @@ namespace Kinematics {
return true;
}

// checks to see if the target is reachable
// bool ParallelDelta::soft_limit_error_exists(float* cartesian) {
// float motors[MAX_N_AXIS];
// bool calc_ok = true;
// bool limit_error = false;

// if (!_softLimits)
// limit_error = false;

// return !transform_cartesian_to_motors(motors, cartesian);
// }

void ParallelDelta::releaseMotors(AxisMask axisMask, MotorMask motors) {}

bool ParallelDelta::transform_cartesian_to_motors(float* motors, float* cartesian) {
Expand Down
10 changes: 2 additions & 8 deletions FluidNC/src/Kinematics/ParallelDelta.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,6 @@
# define M_PI 3.14159265358979323846
#endif

#define ARM_INTERNAL_ANGLE 0.05 // radians 2.866° // due to mounting angle
#define DXL_CENTER 2015 // (DXL_COUNTS / 2) - (ARM_INTERNAL_ANGLE * DXL_COUNT_PER_RADIAN)

namespace Kinematics {
// enum class KinematicError : uint8_t {
// NONE = 0,
Expand All @@ -46,11 +43,8 @@ namespace Kinematics {
void motors_to_cartesian(float* cartesian, float* motors, int n_axis) override;
bool transform_cartesian_to_motors(float* motors, float* cartesian) override;
//bool soft_limit_error_exists(float* cartesian) override;
bool kinematics_homing(AxisMask& axisMask) override;
bool kinematics_homing(AxisMask& axisMask) override;
virtual void constrain_jog(float* cartesian, plan_line_data_t* pl_data, float* position) override;
void releaseMotors(AxisMask axisMask, MotorMask motors) override;

/*
virtual bool invalid_line(float* cartesian) override;
virtual bool invalid_arc(float* target,
plan_line_data_t* pl_data,
Expand All @@ -60,7 +54,7 @@ namespace Kinematics {
size_t caxes[3],
bool is_clockwise_arc) override;

*/
void releaseMotors(AxisMask axisMask, MotorMask motors) override;

// Configuration handlers:
//void validate() const override {}
Expand Down
6 changes: 6 additions & 0 deletions FluidNC/src/Limits.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,11 @@ bool soft_limit = false;
void limit_error(size_t axis, float coordinate) {
log_info("Soft limit on " << Machine::Axes::_names[axis] << " target:" << coordinate);

limit_error();
}

void limit_error() {

soft_limit = true;
// Force feed hold if cycle is active. All buffered blocks are guaranteed to be within
// workspace volume so just come to a controlled stop so position is not lost. When complete
Expand All @@ -74,6 +79,7 @@ void limit_error(size_t axis, float coordinate) {
}
} while (sys.state != State::Idle);
}

mc_critical(ExecAlarm::SoftLimit);
}

Expand Down
1 change: 1 addition & 0 deletions FluidNC/src/Limits.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ void limits_init();
// Returns limit state
MotorMask limits_get_state();

void limit_error();
void limit_error(size_t axis, float cordinate);

float limitsMaxPosition(size_t axis);
Expand Down
1 change: 0 additions & 1 deletion FluidNC/src/Motors/Dynamixel2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,6 @@ namespace MotorDrivers {

// sets the PWM to zero. This allows most servos to be manually moved
void IRAM_ATTR Dynamixel2::set_disable(bool disable) {
log_info("dyn disable:"<<disable)
if (_disabled == disable) {
return;
}
Expand Down
2 changes: 1 addition & 1 deletion example_configs/TapDelta_XYZ.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ kinematics:
max_z_mm: -90.0

start:
must_home: false
must_home: true
deactivate_parking: false
check_limits: false

Expand Down

0 comments on commit d9ab16b

Please sign in to comment.