diff --git a/Grbl_Esp32/Custom/parallel_delta.cpp b/Grbl_Esp32/Custom/parallel_delta.cpp index 52460d14..5d2b1758 100644 --- a/Grbl_Esp32/Custom/parallel_delta.cpp +++ b/Grbl_Esp32/Custom/parallel_delta.cpp @@ -102,9 +102,18 @@ void machine_init() { // 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 calc_forward_kinematics(angles, cartesian); // Sets the cartesian values - // print a startup message to show the kinematics are enabled. Print the offset for reference grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]); + + 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); } bool user_defined_homing() { // true = do not continue with normal Grbl homing @@ -132,7 +141,6 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio float seg_target[3]; // The target of the current segment float feed_rate = pl_data->feed_rate; // save original feed rate - bool start_position_error = false; bool show_error = true; // shows error once KinematicError status; @@ -143,7 +151,7 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio 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; + //start_position_error = true; } // Check the destination to see if it is in work area diff --git a/Grbl_Esp32/src/Machine.h b/Grbl_Esp32/src/Machine.h index d3de7aad..27b5eeb6 100644 --- a/Grbl_Esp32/src/Machine.h +++ b/Grbl_Esp32/src/Machine.h @@ -8,7 +8,7 @@ // !!! For initial testing, start with test_drive.h which disables // all I/O pins // #include "Machines/atari_1020.h" -# include "Machines/tapster_pro_6P_trinamic.h" +# include "Machines/tapster3.h" // !!! For actual use, change the line above to select a board // from Machines/, for example: diff --git a/Grbl_Esp32/src/Machines/tapster3.h b/Grbl_Esp32/src/Machines/tapster3.h index e966367f..64d98898 100644 --- a/Grbl_Esp32/src/Machines/tapster3.h +++ b/Grbl_Esp32/src/Machines/tapster3.h @@ -26,18 +26,24 @@ // ================= Firmware Customization =================== #define USE_KINEMATICS // there are kinematic equations for this machine -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_MACHINE_INIT // There is some custom initialization for this machine -#define USE_CUSTOM_HOMING +#define USE_FWD_KINEMATICS // report in cartesian +#define USE_MACHINE_INIT // There is some custom initialization for this machine // ================== Delta Geometry =========================== -#define RADIUS_FIXED 70.0; // radius of the fixed side (length of motor cranks) -#define RADIUS_EFF 133.5; // radius of end effector side (length of linkages) -#define LENGTH_FIXED_SIDE 173.205f; // sized of fixed side triangel -#define LENGTH_EFF_SIDE 86.6025f; // size of end effector side triangle -#define SEGMENT_LENGTH 0.5 // segment length in mm -#define MAX_NEGATIVE_ANGLE -45 // +#define RADIUS_FIXED 70.0; // radius of the fixed side (length of motor cranks) +#define RADIUS_EFF 133.5; // radius of end effector side (length of linkages) +#define LENGTH_FIXED_SIDE 179.437f; // sized of fixed side triangel +#define LENGTH_EFF_SIDE 86.6025f; // size of end effector side triangle +#define KINEMATIC_SEGMENT_LENGTH 1.0f // segment length in mm +#define MAX_NEGATIVE_ANGLE -(M_PI / 3.0) // 60 degrees up +#define MAX_POSITIVE_ANGLE (M_PI / 2.0) // 90 degrees down +#define ARM_INTERNAL_ANGLE 0.05 // radians 2.866° // due to mounting angle + +#define DEFAULT_X_MAX_TRAVEL MAX_POSITIVE_ANGLE - MAX_NEGATIVE_ANGLE +#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // +#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // + // =================== Machine Hardware Definition ============= @@ -49,9 +55,17 @@ #define Y_DYNAMIXEL_ID 2 // protocol ID #define Z_DYNAMIXEL_ID 3 // protocol ID -// limit servo to 180 degree motion -#define DXL_COUNT_MIN 1024 -#define DXL_COUNT_MAX 3072 +// limit servo to motion range +#define DXL_COUNTS 4096 +#define DXL_COUNT_PER_RADIAN 652 // (DXL_COUNTS / 2.0 * M_PI) + +#define DXL_CENTER 2015 // (DXL_COUNTS / 2) - (ARM_INTERNAL_ANGLE * DXL_COUNT_PER_RADIAN) + +#undef DXL_COUNT_MIN +#define DXL_COUNT_MIN (DXL_CENTER + (MAX_NEGATIVE_ANGLE * DXL_COUNT_PER_RADIAN)) + +#undef DXL_COUNT_MAX +#define DXL_COUNT_MAX (DXL_CENTER + (MAX_POSITIVE_ANGLE * DXL_COUNT_PER_RADIAN)) #define SERVO_TIMER_INTERVAL 50 @@ -85,6 +99,11 @@ #define DEFAULT_HOMING_ENABLE 0 #define DEFAULT_HOMING_DIR_MASK 0 +#define DEFAULT_HOMING_CYCLE_0 (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) +#define DEFAULT_X_HOMING_MPOS MAX_NEGATIVE_ANGLE +#define DEFAULT_Y_HOMING_MPOS DEFAULT_X_HOMING_MPOS +#define DEFAULT_Z_HOMING_MPOS DEFAULT_X_HOMING_MPOS + // the following must have the same values for each axis #define DEFAULT_X_STEPS_PER_MM 800 // value is actually arbitrary, but keep it smallish @@ -99,14 +118,5 @@ #define DEFAULT_Y_ACCELERATION DEFAULT_X_ACCELERATION // 10*60*60 mm/min^2 = 10 mm/sec^2 #define DEFAULT_Z_ACCELERATION DEFAULT_X_ACCELERATION -#define DEFAULT_X_MAX_TRAVEL 3.14159265 // 180° in radians -#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // -#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // - -#define ARM_INTERNAL_ANGLE 2.866 // due to mounting angle - -#define DEFAULT_X_HOMING_MPOS (DEFAULT_X_MAX_TRAVEL / 2.0) -#define DEFAULT_Y_HOMING_MPOS DEFAULT_X_HOMING_MPOS -#define DEFAULT_Z_HOMING_MPOS DEFAULT_X_HOMING_MPOS - +#define SPINDLE_TYPE SpindleType::NONE