Patch SCARA example config
This commit is contained in:
Scott Lahteine
parent
6ab54c60b1
commit
74d7f5e57b
@ -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)
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user