diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 73376b4d1..cc1763372 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -8457,18 +8457,18 @@ void prepare_move_to_destination() { //SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(f_scara[X_AXIS]); //SERIAL_ECHOPGM(" y="); SERIAL_ECHO(f_scara[Y_AXIS]); - x_sin = sin(f_scara[X_AXIS] / SCARA_RAD2DEG) * Linkage_1; - x_cos = cos(f_scara[X_AXIS] / SCARA_RAD2DEG) * Linkage_1; - y_sin = sin(f_scara[Y_AXIS] / SCARA_RAD2DEG) * Linkage_2; - y_cos = cos(f_scara[Y_AXIS] / SCARA_RAD2DEG) * Linkage_2; + x_sin = sin(RADIANS(f_scara[X_AXIS])) * L1; + x_cos = cos(RADIANS(f_scara[X_AXIS])) * L1; + y_sin = sin(RADIANS(f_scara[Y_AXIS])) * L2; + y_cos = cos(RADIANS(f_scara[Y_AXIS])) * L2; //SERIAL_ECHOPGM(" x_sin="); SERIAL_ECHO(x_sin); //SERIAL_ECHOPGM(" x_cos="); SERIAL_ECHO(x_cos); //SERIAL_ECHOPGM(" y_sin="); SERIAL_ECHO(y_sin); //SERIAL_ECHOPGM(" y_cos="); SERIAL_ECHOLN(y_cos); - delta[X_AXIS] = x_cos + y_cos + SCARA_offset_x; //theta - delta[Y_AXIS] = x_sin + y_sin + SCARA_offset_y; //theta+phi + delta[X_AXIS] = x_cos + y_cos + SCARA_OFFSET_X; //theta + delta[Y_AXIS] = x_sin + y_sin + SCARA_OFFSET_Y; //theta+phi //SERIAL_ECHOPGM(" delta[X_AXIS]="); SERIAL_ECHO(delta[X_AXIS]); //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]); @@ -8480,51 +8480,42 @@ void prepare_move_to_destination() { // The maths and first version were done by QHARLEY. // Integrated, tweaked by Joachim Cerny in June 2014. - float SCARA_pos[2]; - static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi; + static float C2, S2, SK1, SK2, THETA, PSI; - SCARA_pos[X_AXIS] = RAW_X_POSITION(cartesian[X_AXIS]) * axis_scaling[X_AXIS] - SCARA_offset_x; //Translate SCARA to standard X Y - SCARA_pos[Y_AXIS] = RAW_Y_POSITION(cartesian[Y_AXIS]) * axis_scaling[Y_AXIS] - SCARA_offset_y; // With scaling factor. + float sx = RAW_X_POSITION(cartesian[X_AXIS]) * axis_scaling[X_AXIS] - SCARA_OFFSET_X, //Translate SCARA to standard X Y + sy = RAW_Y_POSITION(cartesian[Y_AXIS]) * axis_scaling[Y_AXIS] - SCARA_OFFSET_Y; // With scaling factor. - #if (Linkage_1 == Linkage_2) - SCARA_C2 = ((sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS])) / (2 * (float)L1_2)) - 1; + #if (L1 == L2) + C2 = HYPOT2(sx, sy) / (2 * L1_2) - 1; #else - SCARA_C2 = (sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) - (float)L1_2 - (float)L2_2) / 45000; + C2 = (HYPOT2(sx, sy) - L1_2 - L2_2) / 45000; #endif - SCARA_S2 = sqrt(1 - sq(SCARA_C2)); + S2 = sqrt(1 - sq(C2)); - SCARA_K1 = Linkage_1 + Linkage_2 * SCARA_C2; - SCARA_K2 = Linkage_2 * SCARA_S2; + SK1 = L1 + L2 * C2; + SK2 = L2 * S2; - SCARA_theta = (atan2(SCARA_pos[X_AXIS], SCARA_pos[Y_AXIS]) - atan2(SCARA_K1, SCARA_K2)) * -1; - SCARA_psi = atan2(SCARA_S2, SCARA_C2); + THETA = (atan2(sx, sy) - atan2(SK1, SK2)) * -1; + PSI = atan2(S2, C2); - delta[X_AXIS] = SCARA_theta * SCARA_RAD2DEG; // Multiply by 180/Pi - theta is support arm angle - delta[Y_AXIS] = (SCARA_theta + SCARA_psi) * SCARA_RAD2DEG; // - equal to sub arm angle (inverted motor) - delta[Z_AXIS] = RAW_Z_POSITION(cartesian[Z_AXIS]); + delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle + delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor) + delta[Z_AXIS] = cartesian[Z_AXIS]; /** - SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]); - SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]); - SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]); - - SERIAL_ECHOPGM("scara x="); SERIAL_ECHO(SCARA_pos[X_AXIS]); - SERIAL_ECHOPGM(" y="); SERIAL_ECHOLN(SCARA_pos[Y_AXIS]); - - SERIAL_ECHOPGM("delta x="); SERIAL_ECHO(delta[X_AXIS]); - SERIAL_ECHOPGM(" y="); SERIAL_ECHO(delta[Y_AXIS]); - SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(delta[Z_AXIS]); - - SERIAL_ECHOPGM("C2="); SERIAL_ECHO(SCARA_C2); - SERIAL_ECHOPGM(" S2="); SERIAL_ECHO(SCARA_S2); - SERIAL_ECHOPGM(" Theta="); SERIAL_ECHO(SCARA_theta); - SERIAL_ECHOPGM(" Psi="); SERIAL_ECHOLN(SCARA_psi); - SERIAL_EOL; - */ + DEBUG_POS("SCARA IK", cartesian); + DEBUG_POS("SCARA IK", delta); + SERIAL_ECHOPAIR(" SCARA (x,y) ", sx); + SERIAL_ECHOPAIR(",", sy); + SERIAL_ECHOPAIR(" C2=", C2); + SERIAL_ECHOPAIR(" S2=", S2); + SERIAL_ECHOPAIR(" Theta=", THETA); + SERIAL_ECHOLNPAIR(" Phi=", PHI); + //*/ } -#endif // SCARA +#endif // MORGAN_SCARA #if ENABLED(TEMP_STAT_LEDS) diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h index 0234baee1..d04ae6992 100644 --- a/Marlin/SanityCheck.h +++ b/Marlin/SanityCheck.h @@ -137,6 +137,8 @@ #error Please replace "const int dropsegments" with "#define MIN_STEPS_PER_SEGMENT" (and increase by 1) in Configuration_adv.h. #elif defined(PREVENT_DANGEROUS_EXTRUDE) #error "PREVENT_DANGEROUS_EXTRUDE is now PREVENT_COLD_EXTRUSION. Please update your configuration." +#elif defined(SCARA) + #error "SCARA is now MORGAN_SCARA. Please update your configuration." #endif /** @@ -573,11 +575,12 @@ /** * Don't set more than one kinematic type */ -#if (ENABLED(DELTA) && (ENABLED(SCARA) || ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \ +#if (ENABLED(DELTA) && (ENABLED(MORGAN_SCARA) || ENABLED(MAKERARM_SCARA) || ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \ + || (ENABLED(DELTA) && (ENABLED(MAKERARM_SCARA) || ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \ || (ENABLED(SCARA) && (ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \ || (ENABLED(COREXY) && (ENABLED(COREXZ) || ENABLED(COREYZ))) \ || (ENABLED(COREXZ) && ENABLED(COREYZ)) - #error "Please enable only one of DELTA, SCARA, COREXY, COREXZ, or COREYZ." + #error "Please enable only one of DELTA, MORGAN_SCARA, MAKERARM_SCARA, COREXY, COREXZ, or COREYZ." #endif /** @@ -750,7 +753,7 @@ #elif ENABLED(DELTA) #error "Z_DUAL_ENDSTOPS is not compatible with DELTA." #endif -#elif DISABLED(SCARA) +#elif !IS_SCARA #if X_HOME_DIR < 0 && DISABLED(USE_XMIN_PLUG) #error "Enable USE_XMIN_PLUG when homing X to MIN." #elif X_HOME_DIR > 0 && DISABLED(USE_XMAX_PLUG) diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h index 085880e21..3c2e96a69 100644 --- a/Marlin/example_configurations/SCARA/Configuration.h +++ b/Marlin/example_configurations/SCARA/Configuration.h @@ -75,35 +75,37 @@ // //=========================================================================== -//========================= SCARA Settings ================================== +//============================= SCARA Printer =============================== //=========================================================================== -// SCARA-mode for Marlin has been developed by QHARLEY in ZA in 2012/2013. Implemented +// MORGAN_SCARA for Marlin was developed by QHARLEY in ZA in 2012/2013. Implemented // and slightly reworked by JCERNY in 06/2014 with the goal to bring it into Master-Branch // QHARLEYS Autobedlevelling has not been ported, because Marlin has now Bed-levelling // You might need Z-Min endstop on SCARA-Printer to use this feature. Actually untested! -// Uncomment to use Morgan scara mode -#define SCARA -#define SCARA_SEGMENTS_PER_SECOND 200 // If movement is choppy try lowering this value -// Length of inner support arm -#define Linkage_1 150 //mm Preprocessor cannot handle decimal point... -// Length of outer support arm Measure arm lengths precisely and enter -#define Linkage_2 150 //mm -// SCARA tower offset (position of Tower relative to bed zero position) -// This needs to be reasonably accurate as it defines the printbed position in the SCARA space. -#define SCARA_offset_x 100 //mm -#define SCARA_offset_y -56 //mm -#define SCARA_RAD2DEG 57.2957795 // to convert RAD to degrees +// Specify the specific SCARA model +#define MORGAN_SCARA +//#define MAKERARM_SCARA -#define THETA_HOMING_OFFSET 0 //calculatated from Calibration Guide and command M360 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073 -#define PSI_HOMING_OFFSET 0 //calculatated from Calibration Guide and command M364 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073 +#if ENABLED(MORGAN_SCARA) || ENABLED(MAKERARM_SCARA) + //#define DEBUG_SCARA_KINEMATICS -//some helper variables to make kinematics faster -#define L1_2 sq(Linkage_1) // do not change -#define L2_2 sq(Linkage_2) // do not change + #define SCARA_SEGMENTS_PER_SECOND 200 // If movement is choppy try lowering this value + // Length of inner support arm + #define SCARA_LINKAGE_1 150 //mm Preprocessor cannot handle decimal point... + // Length of outer support arm Measure arm lengths precisely and enter + #define SCARA_LINKAGE_2 150 //mm + + // SCARA tower offset (position of Tower relative to bed zero position) + // This needs to be reasonably accurate as it defines the printbed position in the SCARA space. + #define SCARA_OFFSET_X 100 //mm + #define SCARA_OFFSET_Y -56 //mm + + #define THETA_HOMING_OFFSET 0 //calculatated from Calibration Guide and command M360 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073 + #define PSI_HOMING_OFFSET 0 //calculatated from Calibration Guide and command M364 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073 +#endif //=========================================================================== -//========================= SCARA Settings end ============================== +//==================== END ==== SCARA Printer ==== END ====================== //=========================================================================== // @section info