Skip to content

Commit

Permalink
updates for soft limit checking
Browse files Browse the repository at this point in the history
  • Loading branch information
@bdring
bdring committed May 15, 2024
1 parent 66ec90f commit 446f639
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 25 deletions.
59 changes: 38 additions & 21 deletions FluidNC/src/Kinematics/SingleArmScara.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,16 @@ namespace Kinematics {
return true;
}

bool SingleArmScara::invalid_line(float* cartesian) {
float motors[MAX_N_AXIS] = { 0.0, 0.0, 0.0 };

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

// Initialize the machine position
void SingleArmScara::init_position() {
// initialize to the arms fully extended
Expand Down Expand Up @@ -77,36 +87,39 @@ namespace Kinematics {
float shoulder_motor_angle, elbow_motor_angle;
float angles[2];
float motors[n_axis];
xy_to_angles(target, motors);

//return mc_move_motors(motors, pl_data);

// ---------------------------------------- Temporary endpoint -------------------------

float total_cartesian_distance = vector_distance(position, target, n_axis);
float total_cartesian_distance = vector_distance(position, target, 2);

// if move is zero do nothing and pass back the existing values
if (total_cartesian_distance == 0) {
xy_to_angles(target, motors);
if (!xy_to_angles(target, motors)) {
return false;
}
mc_move_motors(motors, pl_data);
return true;
}

//float cartesian_feed_rate = pl_data->feed_rate;

// calculate the total X,Y axis move distance
// higher axes are the same in both coord systems, so it does not undergo conversion
float xydist = vector_distance(target, position, 2);
// Segment the moves. If we choose a small enough _segment_length_mm we can hide the nonlinearity

// this must only be a move in axes above XY
if (xydist == 0) {
mc_move_motors(motor_segment_end, pl_data);
return true;
}

segment_count = xydist / _segment_length_mm;
if (segment_count < 1) { // Make sure there is at least one segment, even if there is no movement
segment_count = 1;
}

//log_info("Segment Count:" << segment_count);

float cartesian_segment_length_mm = total_cartesian_distance / segment_count;

// Calc length of each cartesian segment - The are the same for all segments.
// Calc length of each cartesian segment - They are the same for all segments.
float cartesian_segment_components[n_axis];
for (size_t axis = X_AXIS; axis < n_axis; axis++) {
cartesian_segment_components[axis] = (target[axis] - position[axis]) / segment_count;
Expand All @@ -124,12 +137,14 @@ namespace Kinematics {

// Convert cartesian space coords to motor space
float motor_segment_end[n_axis];
xy_to_angles(cartesian_segment_end, motor_segment_end);
if (!xy_to_angles(cartesian_segment_end, motor_segment_end)) {
return false;
}

// Remember the last motor position so the length can be computed the next time
copyAxes(last_motor_segment_end, motor_segment_end);
copyAxes(_last_motor_segment_end, motor_segment_end);

float motor_dist = vector_distance(last_motor_segment_end, motor_segment_end, 2);
float motor_dist = vector_distance(_last_motor_segment_end, motor_segment_end, 2);

segment_pl_data->feed_rate = pl_data->feed_rate * (motor_dist / cartesian_segment_length_mm);

Expand Down Expand Up @@ -195,10 +210,13 @@ namespace Kinematics {
return false;
}

// if (D < 20.0) {
// log_error("Tip and shoulder too close:" << D << " (" << cartesian[0] << "," << cartesian[1] << ")");
// return false;
// }
// if the forearm is shorter than the upper arm is the target too close to the hub?
if (_upper_arm_mm > _forearm_mm) {
if (D < (_upper_arm_mm - _forearm_mm)) {
log_error("forearm too short to reach location");
return false;
}
}

float L1 = _upper_arm_mm;
float L2 = _forearm_mm;
Expand All @@ -223,15 +241,14 @@ namespace Kinematics {
angles[axis] = cartesian[axis];
}

return true;
}
log_info("Go to angles (" << angles[0] << "," << angles[1] << "," << angles[2] << ")");

bool SingleArmScara::invalid_line(float* cartesian) {
return false;
return true;
}

bool SingleArmScara::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) {
// TO DO not implemented yet
return false;
}

Expand Down
9 changes: 5 additions & 4 deletions FluidNC/src/Kinematics/SingleArmScara.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
*/

#include "Kinematics.h"
#include "../Limits.h"

namespace Kinematics {
class SingleArmScara : public KinematicSystem {
Expand All @@ -29,7 +30,7 @@ namespace Kinematics {
void motors_to_cartesian(float* cartesian, float* motors, int n_axis) override;
bool transform_cartesian_to_motors(float* cartesian, float* motors) override;
bool kinematics_homing(AxisMask& axisMask) override;

virtual bool invalid_line(float* cartesian) override;
virtual bool invalid_arc(float* target,
plan_line_data_t* pl_data,
Expand All @@ -51,13 +52,13 @@ namespace Kinematics {

private:
bool xy_to_angles(float* cartesian, float* angles);
float last_motor_segment_end[MAX_N_AXIS];
float _last_motor_segment_end[MAX_N_AXIS];

// config Parameters
float _upper_arm_mm = 65;
float _forearm_mm = 50;
float _segment_length_mm = 1;
bool _elbow_motor = true; // is the motor at the elbow or belt driven
float _orientation_rad = 0; // which way do the arms extend when fully extended. (bot is to the right)
bool _elbow_motor = true; // is the motor at the elbow or belt driven
float _orientation_rad = 0; // which way do the arms extend when fully extended. (bot is to the right)
};
} // namespace Kinematics

0 comments on commit 446f639

Please sign in to comment.