From f0cab611dbb5d9707742e7d46118a99da1b7b6ba Mon Sep 17 00:00:00 2001 From: cocktailyogi Date: Tue, 24 Jun 2014 18:43:36 +0200 Subject: [PATCH] optimised some math-code --- Marlin/Marlin_main.cpp | 31 +++--- .../SCARA/Configuration.h | 94 ++++++++++++------- 2 files changed, 71 insertions(+), 54 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 65842bd87..4e7ed8010 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -3674,10 +3674,10 @@ void calculate_SCARA_forward_Transform(float f_scara[3]) //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/1000; - x_cos = cos(f_scara[X_AXIS]/SCARA_RAD2DEG) * Linkage_1/1000; - y_sin = sin(f_scara[Y_AXIS]/SCARA_RAD2DEG) * Linkage_2/1000; - y_cos = cos(f_scara[Y_AXIS]/SCARA_RAD2DEG) * Linkage_2/1000; + 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; // SERIAL_ECHOPGM(" x_sin="); SERIAL_ECHO(x_sin); // SERIAL_ECHOPGM(" x_cos="); SERIAL_ECHO(x_cos); @@ -3697,33 +3697,29 @@ void calculate_delta(float cartesian[3]){ // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014 float SCARA_pos[2]; - static float L1_2, L2_2, SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi; + static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi; SCARA_pos[X_AXIS] = cartesian[X_AXIS] * axis_scaling[X_AXIS] - SCARA_offset_x; //Translate SCARA to standard X Y SCARA_pos[Y_AXIS] = cartesian[Y_AXIS] * axis_scaling[Y_AXIS] - SCARA_offset_y; // With scaling factor. - L1_2 = pow(Linkage_1/1000,2); - L2_2 = pow(Linkage_2/1000,2); - #if (Linkage_1 == Linkage_2) - SCARA_C2 = ( ( pow(SCARA_pos[X_AXIS],2) + pow(SCARA_pos[Y_AXIS],2) ) / (2 * L1_2) ) - 1; + SCARA_C2 = ( ( sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) ) / (2 * (float)L1_2) ) - 1; #else - SCARA_C2 = ( pow(SCARA_pos[X_AXIS],2) + pow(SCARA_pos[Y_AXIS],2) - L1_2 - L2_2 ) / 45000; + SCARA_C2 = ( sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) - (float)L1_2 - (float)L2_2 ) / 45000; #endif - SCARA_S2 = sqrt( 1 - pow(SCARA_C2,2) ); + SCARA_S2 = sqrt( 1 - sq(SCARA_C2) ); - SCARA_K1 = Linkage_1/1000+Linkage_2/1000*SCARA_C2; - SCARA_K2 = Linkage_2/1000*SCARA_S2; + SCARA_K1 = Linkage_1 + Linkage_2 * SCARA_C2; + SCARA_K2 = Linkage_2 * SCARA_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); + SCARA_theta = ( atan2(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2(SCARA_K1, SCARA_K2) ) * -1; + SCARA_psi = atan2(SCARA_S2,SCARA_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] = cartesian[Z_AXIS]; - /* SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]); SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]); @@ -3740,8 +3736,7 @@ void calculate_delta(float cartesian[3]){ SERIAL_ECHOPGM(" S2="); SERIAL_ECHO(SCARA_S2); SERIAL_ECHOPGM(" Theta="); SERIAL_ECHO(SCARA_theta); SERIAL_ECHOPGM(" Psi="); SERIAL_ECHOLN(SCARA_psi); - SERIAL_ECHOLN(" "); - */ + SERIAL_ECHOLN(" ");*/ } #endif diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h index b3a8cd1d2..7fd7c571f 100644 --- a/Marlin/example_configurations/SCARA/Configuration.h +++ b/Marlin/example_configurations/SCARA/Configuration.h @@ -14,11 +14,11 @@ // 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 +#define scara_segments_per_second 200 //careful, two much will decrease performance... // Length of inner support arm -#define Linkage_1 150000 //um Preprocessor cannot handle decimal point... -// Length of outer support arm Measure arm lengths precisely, and enter -#define Linkage_2 150000 //um define in micrometer +#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. @@ -26,6 +26,13 @@ #define SCARA_offset_y -56 //mm #define SCARA_RAD2DEG 57.2957795 // to convert RAD to degrees +#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 + +//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 + //=========================================================================== //========================= SCARA Settings end ================================== //=========================================================================== @@ -162,8 +169,8 @@ #define MAX_REDUNDANT_TEMP_SENSOR_DIFF 10 // Actual temperature must be close to target for this long before M109 returns success -#define TEMP_RESIDENCY_TIME 10 // (seconds) -#define TEMP_HYSTERESIS 3 // (degC) range of +/- temperatures considered "close" to the target one +#define TEMP_RESIDENCY_TIME 3 // (seconds) +#define TEMP_HYSTERESIS 2 // (degC) range of +/- temperatures considered "close" to the target one #define TEMP_WINDOW 1 // (degC) Window around target to start the residency timer x degC early. // The minimal temperature defines the temperature below which the heater will not be enabled It is used @@ -188,8 +195,8 @@ //#define HEATER_BED_DUTY_CYCLE_DIVIDER 4 // If you want the M105 heater power reported in watts, define the BED_WATTS, and (shared for all extruders) EXTRUDER_WATTS -//#define EXTRUDER_WATTS (12.0*12.0/6.7) // P=I^2/R -//#define BED_WATTS (12.0*12.0/1.1) // P=I^2/R +#define EXTRUDER_WATTS (2*2/5.9) // P=I^2/R +#define BED_WATTS (5.45*5.45/2.2) // P=I^2/R // PID settings: // Comment the following line to disable PID and enable bang-bang. @@ -199,7 +206,7 @@ #ifdef PIDTEMP //#define PID_DEBUG // Sends debug data to the serial port. //#define PID_OPENLOOP 1 // Puts PID in open loop. M104/M140 sets the output power from 0 to PID_MAX - #define PID_FUNCTIONAL_RANGE 10 // If the temperature difference between the target temperature and the actual temperature + #define PID_FUNCTIONAL_RANGE 20 // If the temperature difference between the target temperature and the actual temperature // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max. #define PID_INTEGRAL_DRIVE_MAX 255 //limit for the integral term #define K1 0.95 //smoothing factor within the PID @@ -207,9 +214,19 @@ // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it // Ultimaker - #define DEFAULT_Kp 22.2 - #define DEFAULT_Ki 1.08 - #define DEFAULT_Kd 114 + // #define DEFAULT_Kp 22.2 + // #define DEFAULT_Ki 1.08 + // #define DEFAULT_Kd 114 + + // Jhead MK5: From Autotune + // #define DEFAULT_Kp 20.92 + // #define DEFAULT_Ki 1.51 + // #define DEFAULT_Kd 72.34 + + //Merlin Hotend: From Autotune + #define DEFAULT_Kp 24.5 + #define DEFAULT_Ki 1.72 + #define DEFAULT_Kd 87.73 // MakerGear // #define DEFAULT_Kp 7.0 @@ -217,9 +234,9 @@ // #define DEFAULT_Kd 12 // Mendel Parts V9 on 12V -// #define DEFAULT_Kp 63.0 -// #define DEFAULT_Ki 2.25 -// #define DEFAULT_Kd 440 + // #define DEFAULT_Kp 63.0 + // #define DEFAULT_Ki 2.25 + // #define DEFAULT_Kd 440 #endif // PIDTEMP // Bed Temperature Control @@ -232,9 +249,9 @@ // If your configuration is significantly different than this and you don't understand the issues involved, you probably // shouldn't use bed PID until someone else verifies your hardware works. // If this is enabled, find your own PID constants below. -//#define PIDTEMPBED +#define PIDTEMPBED // -//#define BED_LIMIT_SWITCHING +#define BED_LIMIT_SWITCHING // This sets the max power delivered to the bed, and replaces the HEATER_BED_DUTY_CYCLE_DIVIDER option. // all forms of bed control obey this (PID, bang-bang, bang-bang with hysteresis) @@ -245,9 +262,9 @@ #ifdef PIDTEMPBED //120v 250W silicone heater into 4mm borosilicate (MendelMax 1.5+) //from FOPDT model - kp=.39 Tp=405 Tdead=66, Tc set to 79.2, aggressive factor of .15 (vs .1, 1, 10) - #define DEFAULT_bedKp 10.00 - #define DEFAULT_bedKi .023 - #define DEFAULT_bedKd 305.4 + // #define DEFAULT_bedKp 10.00 + // #define DEFAULT_bedKi .023 + // #define DEFAULT_bedKd 305.4 //120v 250W silicone heater into 4mm borosilicate (MendelMax 1.5+) //from pidautotune @@ -255,6 +272,12 @@ // #define DEFAULT_bedKi 1.41 // #define DEFAULT_bedKd 1675.16 +//12v Heatbed Mk3 12V in parallel +//from pidautotune + #define DEFAULT_bedKp 630.14 + #define DEFAULT_bedKi 121.71 + #define DEFAULT_bedKd 815.64 + // FIND YOUR OWN: "M303 E-1 C8 S90" to run autotune on the bed at 90 degreesC for 8 cycles. #endif // PIDTEMPBED @@ -262,11 +285,11 @@ //this prevents dangerous Extruder moves, i.e. if the temperature is under the limit //can be software-disabled for whatever purposes by -#define PREVENT_DANGEROUS_EXTRUDE +//#define PREVENT_DANGEROUS_EXTRUDE //if PREVENT_DANGEROUS_EXTRUDE is on, you can still disable (uncomment) very long bits of extrusion separately. #define PREVENT_LENGTHY_EXTRUDE -#define EXTRUDE_MINTEMP 170 +#define EXTRUDE_MINTEMP 150 #define EXTRUDE_MAXLENGTH (X_MAX_LENGTH+Y_MAX_LENGTH) //prevent extrusion of very large distances. //=========================================================================== @@ -329,7 +352,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of #define INVERT_X_DIR false // for Mendel set to false, for Orca set to true #define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false #define INVERT_Z_DIR true // for Mendel set to false, for Orca set to true -#define INVERT_E0_DIR true // for direct drive extruder v9 set to true, for geared extruder set to false +#define INVERT_E0_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E1_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false #define INVERT_E2_DIR false // for direct drive extruder v9 set to true, for geared extruder set to false @@ -451,8 +474,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of //Manual homing switch locations: // For deltabots this means top and center of the Cartesian print volume. // For SCARA: Offset between HomingPosition and Bed X=0 / Y=0 -#define MANUAL_X_HOME_POS -20 -#define MANUAL_Y_HOME_POS -48 +#define MANUAL_X_HOME_POS -22. +#define MANUAL_Y_HOME_POS -52. #define MANUAL_Z_HOME_POS 0.1 // Distance between nozzle and print surface after homing. @@ -462,24 +485,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of // default settings -//#define DEFAULT_AXIS_STEPS_PER_UNIT {85.6,85.6,200/1.25,970} // default steps per unit for Ultimaker -#define DEFAULT_AXIS_STEPS_PER_UNIT {109,109,200/1.25,970} // default steps per unit for Ultimaker -#define DEFAULT_MAX_FEEDRATE {200, 200, 30, 45} // (mm/sec) -#define DEFAULT_MAX_ACCELERATION {300,300,30,1500} // X, Y, Z, E maximum start speed for accelerated moves. E default values are good for Skeinforge 40+, for older versions raise them a lot. +#define DEFAULT_AXIS_STEPS_PER_UNIT {103.69,106.65,200/1.25,1000} // default steps per unit for SCARA +#define DEFAULT_MAX_FEEDRATE {300, 300, 30, 25} // (mm/sec) +#define DEFAULT_MAX_ACCELERATION {300,300,20,1000} // X, Y, Z, E maximum start speed for accelerated moves. E default values are good for Skeinforge 40+, for older versions raise them a lot. -#define DEFAULT_ACCELERATION 300 // X, Y, Z and E max acceleration in mm/s^2 for printing moves -#define DEFAULT_RETRACT_ACCELERATION 3000 // X, Y, Z and E max acceleration in mm/s^2 for retracts +#define DEFAULT_ACCELERATION 400 // X, Y, Z and E max acceleration in mm/s^2 for printing moves +#define DEFAULT_RETRACT_ACCELERATION 2000 // X, Y, Z and E max acceleration in mm/s^2 for retracts // Offset of the extruders (uncomment if using more than one and relying on firmware to position when changing). -// The offset has to be X=0, Y=0 for the extruder 0 hotend (default extruder). +// The offset has to be X=0, Y=0 for extruder 0 hotend (default extruder). // For the other hotends it is their distance from the extruder 0 hotend. // #define EXTRUDER_OFFSET_X {0.0, 20.00} // (in mm) for each extruder, offset of the hotend on the X axis // #define EXTRUDER_OFFSET_Y {0.0, 5.00} // (in mm) for each extruder, offset of the hotend on the Y axis // The speed change that does not require acceleration (i.e. the software might assume it can be done instantaneously) -#define DEFAULT_XYJERK 10.0 // (mm/sec) -#define DEFAULT_ZJERK 10.0 // (mm/sec) -#define DEFAULT_EJERK 5.0 // (mm/sec) +#define DEFAULT_XYJERK 5 // (mm/sec) +#define DEFAULT_ZJERK 0.4 // (mm/sec) +#define DEFAULT_EJERK 3 // (mm/sec) //=========================================================================== //=============================Additional Features=========================== @@ -500,7 +522,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of // M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily). // M502 - reverts to the default "factory settings". You still need to store them in EEPROM afterwards if you want to. //define this to enable EEPROM support -#define EEPROM_SETTINGS +//#define EEPROM_SETTINGS //to disable EEPROM Serial responses and decrease program space by ~1700 byte: comment this out: // please keep turned on if you can. #define EEPROM_CHITCHAT