andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

optimised some math-code

pull/1/head
cocktailyogi 11 years ago
parent
fae7d08698
commit
f0cab611db
2 changed files with 71 additions and 54 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 29
      Marlin/Marlin_main.cpp
  2. 94
      Marlin/example_configurations/SCARA/Configuration.h

29
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_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

Write Preview
Loading…
Cancel
Save