optimised some math-code
This commit is contained in:
cocktailyogi
parent
fae7d08698
commit
f0cab611db
2 changed files with 71 additions and 54 deletions
|
|
@ -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,24 +3697,21 @@ 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);
|
||||
|
|
@ -3723,7 +3720,6 @@ void calculate_delta(float cartesian[3]){
|
|||
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
Reference in a new issue