bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

optimised some math-code

Browse Source
This commit is contained in:
cocktailyogi 2014-06-24 18:43:36 +02:00
parent fae7d08698
commit f0cab611db
2 changed files with 71 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
Marlin/Marlin_main.cpp
View File

@ -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

94
Marlin/example_configurations/SCARA/Configuration.h
View File

@ -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