diff --git a/Grbl_Esp32/Custom/parallel_delta.cpp b/Grbl_Esp32/Custom/parallel_delta.cpp index 104e4057..1599c258 100644 --- a/Grbl_Esp32/Custom/parallel_delta.cpp +++ b/Grbl_Esp32/Custom/parallel_delta.cpp @@ -65,6 +65,10 @@ enum class KinematicError : uint8_t { // Create custom run time $ settings FloatSetting* kinematic_segment_len; +FloatSetting* delta_crank_len; +FloatSetting* delta_link_len; +FloatSetting* delta_crank_side_len; +FloatSetting* delta_effector_side_len; // trigonometric constants to speed up calculations const float sqrt3 = 1.732050807; @@ -76,10 +80,10 @@ const float sin30 = 0.5; const float tan30 = 1.0 / sqrt3; // the geometry of the delta -const float rf = RADIUS_FIXED; // radius of the fixed side (length of motor cranks) -const float re = RADIUS_EFF; // radius of end effector side (length of linkages) -const float f = LENGTH_FIXED_SIDE; // sized of fixed side triangel -const float e = LENGTH_EFF_SIDE; // size of end effector side triangle +float rf; // radius of the fixed side (length of motor cranks) +float re; // radius of end effector side (length of linkages) +float f; // sized of fixed side triangel +float e; // size of end effector side triangle static float last_angle[3] = { 0.0, 0.0, 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs static float last_cartesian[N_AXIS] = { @@ -91,13 +95,20 @@ int calc_forward_kinematics(float* angles, float* cartesian); KinematicError delta_calcInverse(float* cartesian, float* angles); KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta); float three_axis_dist(float* point1, float* point2); +void read_settings(); void machine_init() { float angles[N_AXIS] = { 0.0, 0.0, 0.0 }; float cartesian[N_AXIS] = { 0.0, 0.0, 0.0 }; // Custom $ settings - kinematic_segment_len = new FloatSetting(EXTENDED, WG, NULL, "Kinematics/SegmentLength", KINEMATIC_SEGMENT_LENGTH, 0.2, 5.0); + kinematic_segment_len = new FloatSetting(EXTENDED, WG, NULL, "Kinematics/SegmentLength", KINEMATIC_SEGMENT_LENGTH, 0.2, 5.0); + delta_crank_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/CrankLength", RADIUS_FIXED, 50, 150.0); + delta_link_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/LinkLength", RADIUS_EFF, 0.2, 5.0); + delta_crank_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/CrankSideLength", LENGTH_FIXED_SIDE, 0.2, 5.0); + delta_effector_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/EffectorSideLength", LENGTH_EFF_SIDE, 0.2, 5.0); + + read_settings(); // Calculate the Z offset at the arm zero angles ... // Z offset is the z distance from the motor axes to the end effector axes at zero angle @@ -107,14 +118,13 @@ void machine_init() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Angle Range %3.3f, %3.3f", MAX_NEGATIVE_ANGLE, MAX_POSITIVE_ANGLE); -// grbl_msg_sendf(CLIENT_SERIAL, -// MsgLevel::Info, -// "DXL_COUNT_MIN %4.0f CENTER %d MAX %4.0f PER_RAD %d", -// DXL_COUNT_MIN, -// DXL_CENTER, -// DXL_COUNT_MAX, -// DXL_COUNT_PER_RADIAN); - + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "DXL_COUNT_MIN %4.0f CENTER %d MAX %4.0f PER_RAD %d", + // DXL_COUNT_MIN, + // DXL_CENTER, + // DXL_COUNT_MAX, + // DXL_COUNT_PER_RADIAN); } bool user_defined_homing() { // true = do not continue with normal Grbl homing @@ -128,6 +138,8 @@ bool user_defined_homing() { // true = do not continue with normal Grbl homing // This function is used by Grbl void inverse_kinematics(float* position) { float motor_angles[3]; + + read_settings(); delta_calcInverse(position, motor_angles); position[0] = motor_angles[0]; position[1] = motor_angles[1]; @@ -140,25 +152,27 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio float dx, dy, dz; // distances in each cartesian axis float motor_angles[3]; - float seg_target[3]; // The target of the current segment - float feed_rate = pl_data->feed_rate; // save original feed rate - bool show_error = true; // shows error once + float seg_target[3]; // The target of the current segment + float feed_rate = pl_data->feed_rate; // save original feed rate + bool show_error = true; // shows error once KinematicError status; - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start %3.3f %3.3f %3.3f", position[0], position[1], position[2]); - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target %3.3f %3.3f %3.3f", target[0], target[1], target[2]); + read_settings(); + + // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start %3.3f %3.3f %3.3f", position[0], position[1], position[2]); + // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target %3.3f %3.3f %3.3f", target[0], target[1], target[2]); status = delta_calcInverse(position, motor_angles); if (status == KinematicError::OUT_OF_RANGE) { //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error %3.3f %3.3f %3.3f", position[0], position[1], position[2]); //start_position_error = true; - } + } // Check the destination to see if it is in work area status = delta_calcInverse(target, motor_angles); if (status == KinematicError::OUT_OF_RANGE) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable error %3.3f %3.3f %3.3f", target[0], target[1], target[2]); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable error %3.3f %3.3f %3.3f", target[0], target[1], target[2]); } position[X_AXIS] += gc_state.coord_offset[X_AXIS]; @@ -218,11 +232,13 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio uint8_t kinematic_limits_check(float* target) { float motor_angles[3]; + read_settings(); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin Soft Check %3.3f, %3.3f, %3.3f", target[0], target[1], target[2]); KinematicError status = delta_calcInverse(target, motor_angles); - switch(status) { + switch (status) { case KinematicError::OUT_OF_RANGE: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target out of range"); break; @@ -234,7 +250,7 @@ uint8_t kinematic_limits_check(float* target) { break; } - return (status == KinematicError::NONE); + return (status == KinematicError::NONE); } // inverse kinematics: cartesian -> angles @@ -349,6 +365,8 @@ void forward_kinematics(float* position) { float calc_fwd[N_AXIS]; int status; + read_settings(); + // convert the system position in steps to radians float position_radians[N_AXIS]; int32_t position_steps[N_AXIS]; // Copy current state of the system position variable @@ -388,6 +406,8 @@ void kinematics_post_homing() { last_angle[Y_AXIS] = sys_position[Y_AXIS] / axis_settings[Y_AXIS]->steps_per_mm->get(); last_angle[Z_AXIS] = sys_position[Z_AXIS] / axis_settings[Z_AXIS]->steps_per_mm->get(); + read_settings(); + calc_forward_kinematics(last_angle, last_cartesian); // grbl_msg_sendf(CLIENT_SERIAL, @@ -412,3 +432,10 @@ void kinematics_post_homing() { } void user_m30() {} + +void read_settings() { + rf = delta_crank_len->get(); // radius of the fixed side (length of motor cranks) + re = delta_link_len->get(); // radius of end effector side (length of linkages) + f = delta_crank_side_len->get(); // sized of fixed side triangel + e = delta_effector_side_len->get(); // size of end effector side triangle +} diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index c9a7c6a8..818604aa 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -1477,8 +1477,6 @@ Error gc_execute_line(char* line, uint8_t client) { } mc_line_kins(coord_data, pl_data, gc_state.position); memcpy(gc_state.position, coord_data, sizeof(gc_state.position)); - == == == = mc_line_kins(gc_block.values.ijk, pl_data, gc_state.position); - memcpy(gc_state.position, gc_block.values.ijk, MAX_N_AXIS * sizeof(float)); break; case NonModal::SetHome0: coords[CoordIndex::G28]->set(gc_state.position); diff --git a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h index 2f506c21..6f88a816 100644 --- a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h +++ b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h @@ -69,7 +69,7 @@ // === Default settings #define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE -#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set +// #define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set #define I2S_OUT_BCK GPIO_NUM_22 #define I2S_OUT_WS GPIO_NUM_17 @@ -77,12 +77,6 @@ // ================== CPU MAP ====================== -#define X_STEPPER_MS3 I2SO(3) // X_CS -#define Y_STEPPER_MS3 I2SO(6) // Y_CS -#define Z_STEPPER_MS3 I2SO(11) // Z_CS - -#define STEPPER_RESET GPIO_NUM_19 - // Motor Socket #1 #define X_TRINAMIC_DRIVER 2130 #define X_DISABLE_PIN I2SO(0) @@ -168,6 +162,13 @@ #define DEFAULT_Y_ACCELERATION DEFAULT_X_ACCELERATION #define DEFAULT_Z_ACCELERATION DEFAULT_X_ACCELERATION +#define DEFAULT_X_CURRENT 1.0 +#define DEFAULT_X_HOLD_CURRENT 0.5 +#define DEFAULT_Y_CURRENT 1.0 +#define DEFAULT_Y_HOLD_CURRENT 0.5 +#define DEFAULT_Z_CURRENT 1.0 +#define DEFAULT_Z_HOLD_CURRENT 0.5 + // homing #define DEFAULT_HOMING_FEED_RATE 25 #define DEFAULT_HOMING_SEEK_RATE 100 diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index bdd12188..b7f5451d 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -37,7 +37,7 @@ SquaringMode ganged_mode = SquaringMode::Dual; void mc_line_kins(float* target, plan_line_data_t* pl_data, float* position) { #ifndef USE_KINEMATICS mc_line(target, pl_data); -#else // else use kinematics +#else // else use kinematics inverse_kinematics(target, pl_data, position); #endif } diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index 339cc901..d2c6e595 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -204,17 +204,6 @@ namespace Motors { return; } - float max_travel = axis_settings[_axis_index]->max_travel->get(); - float mpos = axis_settings[_axis_index]->home_mpos->get(); - - if (bit_istrue(homing_dir_mask->get(), bit(_axis_index))) { - _position_min = mpos; - _position_max = mpos + max_travel; - } else { - _position_min = mpos - max_travel; - _position_max = mpos; - } - uint16_t run_i_ma = (uint16_t)(axis_settings[_axis_index]->run_current->get() * 1000.0); float hold_i_percent;