From 512f2a313600b3f0e3c48dfcc4ea9972e8b196a9 Mon Sep 17 00:00:00 2001 From: cocktailyogi Date: Mon, 23 Jun 2014 17:09:57 +0200 Subject: [PATCH 1/9] restore Branch from Backup sorry for that --- Marlin/Configuration.h | 8 +- Marlin/ConfigurationStore.cpp | 34 +- Marlin/Marlin.h | 7 + Marlin/Marlin_main.cpp | 368 +- .../SCARA/Configuration.h | 743 ++++ .../SCARA/Configuration_adv.h | 507 +++ Marlin/thermistortables.h | 75 - Marlin/ultralcd.cpp | 3254 +++++++++-------- README.md | 11 +- 9 files changed, 3285 insertions(+), 1722 deletions(-) create mode 100644 Marlin/example_configurations/SCARA/Configuration.h create mode 100644 Marlin/example_configurations/SCARA/Configuration_adv.h diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index 66bf69052d..2d81336cb4 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -12,6 +12,13 @@ // example_configurations/delta directory. // +//=========================================================================== +//============================= SCARA Printer =============================== +//=========================================================================== +// For a Delta printer replace the configuration files with the files in the +// example_configurations/SCARA directory. +// + // User-specified version info of this build to display in [Pronterface, etc] terminal window during // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this // build by the user have been successfully uploaded into firmware. @@ -132,7 +139,6 @@ // 1010 is Pt1000 with 1k pullup (non standard) // 147 is Pt100 with 4k7 pullup // 110 is Pt100 with 1k pullup (non standard) -// 70 is 500C thermistor for Pico hot end #define TEMP_SENSOR_0 -1 #define TEMP_SENSOR_1 -1 diff --git a/Marlin/ConfigurationStore.cpp b/Marlin/ConfigurationStore.cpp index 074ef6ae37..7bbfede752 100644 --- a/Marlin/ConfigurationStore.cpp +++ b/Marlin/ConfigurationStore.cpp @@ -37,10 +37,15 @@ void _EEPROM_readData(int &pos, uint8_t* value, uint8_t size) // the default values are used whenever there is a change to the data, to prevent // wrong data being written to the variables. // ALSO: always make sure the variables in the Store and retrieve sections are in the same order. -#ifdef DELTA -#define EEPROM_VERSION "V11" -#else + #define EEPROM_VERSION "V10" +#ifdef DELTA + #undef EEPROM_VERSION + #define EEPROM_VERSION "V11" +#endif +#ifdef SCARA + #undef EEPROM_VERSION + #define EEPROM_VERSION "V12" #endif #ifdef EEPROM_SETTINGS @@ -49,7 +54,7 @@ void Config_StoreSettings() char ver[4]= "000"; int i=EEPROM_OFFSET; EEPROM_WRITE_VAR(i,ver); // invalidate data first - EEPROM_WRITE_VAR(i,axis_steps_per_unit); + EEPROM_WRITE_VAR(i,axis_steps_per_unit); EEPROM_WRITE_VAR(i,max_feedrate); EEPROM_WRITE_VAR(i,max_acceleration_units_per_sq_second); EEPROM_WRITE_VAR(i,acceleration); @@ -93,6 +98,9 @@ void Config_StoreSettings() int lcd_contrast = 32; #endif EEPROM_WRITE_VAR(i,lcd_contrast); + #ifdef SCARA + EEPROM_WRITE_VAR(i,axis_scaling); // Add scaling for SCARA + #endif char ver2[4]=EEPROM_VERSION; i=EEPROM_OFFSET; EEPROM_WRITE_VAR(i,ver2); // validate data @@ -115,6 +123,16 @@ void Config_PrintSettings() SERIAL_ECHOLN(""); SERIAL_ECHO_START; +#ifdef SCARA +SERIAL_ECHOLNPGM("Scaling factors:"); + SERIAL_ECHO_START; + SERIAL_ECHOPAIR(" M365 X",axis_scaling[0]); + SERIAL_ECHOPAIR(" Y",axis_scaling[1]); + SERIAL_ECHOPAIR(" Z",axis_scaling[2]); + SERIAL_ECHOLN(""); + + SERIAL_ECHO_START; +#endif SERIAL_ECHOLNPGM("Maximum feedrates (mm/s):"); SERIAL_ECHO_START; SERIAL_ECHOPAIR(" M203 X",max_feedrate[0]); @@ -196,7 +214,7 @@ void Config_RetrieveSettings() if (strncmp(ver,stored_ver,3) == 0) { // version number match - EEPROM_READ_VAR(i,axis_steps_per_unit); + EEPROM_READ_VAR(i,axis_steps_per_unit); EEPROM_READ_VAR(i,max_feedrate); EEPROM_READ_VAR(i,max_acceleration_units_per_sq_second); @@ -240,6 +258,9 @@ void Config_RetrieveSettings() int lcd_contrast; #endif EEPROM_READ_VAR(i,lcd_contrast); + #ifdef SCARA + EEPROM_READ_VAR(i,axis_scaling); + #endif // Call updatePID (similar to when we have processed M301) updatePID(); @@ -266,6 +287,9 @@ void Config_ResetDefault() axis_steps_per_unit[i]=tmp1[i]; max_feedrate[i]=tmp2[i]; max_acceleration_units_per_sq_second[i]=tmp3[i]; + #ifdef SCARA + axis_scaling[i]=1; + #endif } // steps per sq second need to be updated to agree with the units per sq second diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h index e7282092e5..989110f5fe 100644 --- a/Marlin/Marlin.h +++ b/Marlin/Marlin.h @@ -178,6 +178,10 @@ void get_coordinates(); void calculate_delta(float cartesian[3]); extern float delta[3]; #endif +#ifdef SCARA +void calculate_delta(float cartesian[3]); +void calculate_SCARA_forward_Transform(float f_scara[3]); +#endif void prepare_move(); void kill(); void Stop(); @@ -215,6 +219,9 @@ extern float delta_diagonal_rod; extern float delta_segments_per_second; void recalc_delta_settings(float radius, float diagonal_rod); #endif +#ifdef SCARA +extern float axis_scaling[3]; // Build size scaling +#endif extern float min_pos[3]; extern float max_pos[3]; extern bool axis_known_position[3]; diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index c4afca7f68..9164808cd1 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -170,6 +170,16 @@ // M908 - Control digital trimpot directly. // M350 - Set microstepping mode. // M351 - Toggle MS1 MS2 pins directly. + +// ************ SCARA Specific - This can change to suit future G-code regulations +// M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration) +// M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree) +// M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration) +// M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree) +// M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position) +// M365 - SCARA calibration: Scaling factor, X, Y, Z axis +//************* SCARA End *************** + // M928 - Start SD logging (M928 filename.g) - ended by M29 // M999 - Restart after being stopped by error @@ -212,6 +222,7 @@ float add_homeing[3]={0,0,0}; #ifdef DELTA float endstop_adj[3]={0,0,0}; #endif + float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS }; float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS }; bool axis_known_position[3] = {false, false, false}; @@ -274,13 +285,18 @@ int EtoPPressure=0; float delta_diagonal_rod= DELTA_DIAGONAL_ROD; float delta_diagonal_rod_2= sq(delta_diagonal_rod); float delta_segments_per_second= DELTA_SEGMENTS_PER_SECOND; -#endif +#endif + +#ifdef SCARA // Build size scaling +float axis_scaling[3]={1,1,1}; // Build size scaling, default to 1 +#endif //=========================================================================== //=============================Private Variables============================= //=========================================================================== const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'}; static float destination[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0}; +static float delta[3] = {0.0, 0.0, 0.0}; static float offset[3] = {0.0, 0.0, 0.0}; static bool home_all_axis = true; static float feedrate = 1500.0, next_feedrate, saved_feedrate; @@ -850,9 +866,59 @@ static void axis_is_at_home(int axis) { } } #endif +#ifdef SCARA + float homeposition[3]; + char i; + + if (axis < 2) + { + + for (i=0; i<3; i++) + { + homeposition[i] = base_home_pos(i); + } + // SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]); + // SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]); + // Works out real Homeposition angles using inverse kinematics, + // and calculates homing offset using forward kinematics + calculate_delta(homeposition); + + // SERIAL_ECHOPGM("base Theta= "); SERIAL_ECHO(delta[X_AXIS]); + // SERIAL_ECHOPGM(" base Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]); + + for (i=0; i<2; i++) + { + delta[i] -= add_homeing[i]; + } + + // SERIAL_ECHOPGM("addhome X="); SERIAL_ECHO(add_homeing[X_AXIS]); + // SERIAL_ECHOPGM(" addhome Y="); SERIAL_ECHO(add_homeing[Y_AXIS]); + // SERIAL_ECHOPGM(" addhome Theta="); SERIAL_ECHO(delta[X_AXIS]); + // SERIAL_ECHOPGM(" addhome Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]); + + calculate_SCARA_forward_Transform(delta); + + // SERIAL_ECHOPGM("Delta X="); SERIAL_ECHO(delta[X_AXIS]); + // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]); + + current_position[axis] = delta[axis]; + + // SCARA home positions are based on configuration since the actual limits are determined by the + // inverse kinematic transform. + min_pos[axis] = base_min_pos(axis); // + (delta[axis] - base_home_pos(axis)); + max_pos[axis] = base_max_pos(axis); // + (delta[axis] - base_home_pos(axis)); + } + else + { + current_position[axis] = base_home_pos(axis) + add_homeing[axis]; + min_pos[axis] = base_min_pos(axis) + add_homeing[axis]; + max_pos[axis] = base_max_pos(axis) + add_homeing[axis]; + } +#else current_position[axis] = base_home_pos(axis) + add_homeing[axis]; min_pos[axis] = base_min_pos(axis) + add_homeing[axis]; max_pos[axis] = base_max_pos(axis) + add_homeing[axis]; +#endif } #ifdef ENABLE_AUTO_BED_LEVELING @@ -1111,6 +1177,7 @@ static void homeaxis(int axis) { } } #define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS) + void refresh_cmd_timeout(void) { previous_millis_cmd = millis(); @@ -1184,6 +1251,7 @@ void process_commands() return; } break; +#ifdef SCARA //disable arc support case 2: // G2 - CW ARC if(Stopped == false) { get_arc_coordinates(); @@ -1198,6 +1266,7 @@ void process_commands() return; } break; +#endif case 4: // G4 dwell LCD_MESSAGEPGM(MSG_DWELL); codenum = 0; @@ -1267,12 +1336,12 @@ void process_commands() HOMEAXIS(Z); calculate_delta(current_position); - plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); - + plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); + #else // NOT DELTA home_all_axis = !((code_seen(axis_codes[X_AXIS])) || (code_seen(axis_codes[Y_AXIS])) || (code_seen(axis_codes[Z_AXIS]))); - + #if Z_HOME_DIR > 0 // If homing away from BED do Z first if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) { HOMEAXIS(Z); @@ -1316,7 +1385,9 @@ void process_commands() current_position[X_AXIS] = destination[X_AXIS]; current_position[Y_AXIS] = destination[Y_AXIS]; + #ifndef SCARA current_position[Z_AXIS] = destination[Z_AXIS]; + #endif } #endif @@ -1346,13 +1417,21 @@ void process_commands() if(code_seen(axis_codes[X_AXIS])) { if(code_value_long() != 0) { - current_position[X_AXIS]=code_value()+add_homeing[0]; + #ifdef SCARA + current_position[X_AXIS]=code_value(); + #else + current_position[X_AXIS]=code_value()+add_homeing[0]; + #endif } } if(code_seen(axis_codes[Y_AXIS])) { if(code_value_long() != 0) { - current_position[Y_AXIS]=code_value()+add_homeing[1]; + #ifdef SCARA + current_position[Y_AXIS]=code_value(); + #else + current_position[Y_AXIS]=code_value()+add_homeing[1]; + #endif } } @@ -1427,6 +1506,11 @@ void process_commands() plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); #endif // else DELTA +#ifdef SCARA + calculate_delta(current_position); + plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); +#endif SCARA + #ifdef ENDSTOPS_ONLY_FOR_HOMING enable_endstops(false); #endif @@ -1623,8 +1707,17 @@ void process_commands() plan_set_e_position(current_position[E_AXIS]); } else { - current_position[i] = code_value()+add_homeing[i]; - plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); +#ifdef SCARA + if (i == X_AXIS || i == Y_AXIS) { + current_position[i] = code_value(); + } + else { + current_position[i] = code_value()+add_homeing[i]; + } +#else + current_position[i] = code_value()+add_homeing[i]; +#endif + plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); } } } @@ -2214,6 +2307,26 @@ void process_commands() SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]); SERIAL_PROTOCOLLN(""); +#ifdef SCARA + SERIAL_PROTOCOLPGM("SCARA Theta:"); + SERIAL_PROTOCOL(delta[X_AXIS]); + SERIAL_PROTOCOLPGM(" Psi+Theta:"); + SERIAL_PROTOCOL(delta[Y_AXIS]); + SERIAL_PROTOCOLLN(""); + + SERIAL_PROTOCOLPGM("SCARA Cal - Theta:"); + SERIAL_PROTOCOL(delta[X_AXIS]+add_homeing[0]); + SERIAL_PROTOCOLPGM(" Psi+Theta (90):"); + SERIAL_PROTOCOL(delta[Y_AXIS]-delta[X_AXIS]-90+add_homeing[1]); + SERIAL_PROTOCOLLN(""); + + SERIAL_PROTOCOLPGM("SCARA step Cal - Theta:"); + SERIAL_PROTOCOL(delta[X_AXIS]/90*axis_steps_per_unit[X_AXIS]); + SERIAL_PROTOCOLPGM(" Psi+Theta:"); + SERIAL_PROTOCOL((delta[Y_AXIS]-delta[X_AXIS])/90*axis_steps_per_unit[Y_AXIS]); + SERIAL_PROTOCOLLN(""); + SERIAL_PROTOCOLLN(""); +#endif break; case 120: // M120 enable_endstops(false) ; @@ -2335,6 +2448,16 @@ void process_commands() { if(code_seen(axis_codes[i])) add_homeing[i] = code_value(); } + #ifdef SCARA + if(code_seen('T')) // Theta + { + add_homeing[0] = code_value() ; + } + if(code_seen('P')) // Psi + { + add_homeing[1] = code_value() ; + } + #endif break; #ifdef DELTA case 665: // M665 set delta configurations L R S @@ -2693,6 +2816,105 @@ void process_commands() PID_autotune(temp, e, c); } break; + #ifdef SCARA + case 360: // M360 SCARA Theta pos1 + SERIAL_ECHOLN(" Cal: Theta 0 "); + //SoftEndsEnabled = false; // Ignore soft endstops during calibration + //SERIAL_ECHOLN(" Soft endstops disabled "); + if(Stopped == false) { + //get_coordinates(); // For X Y Z E F + delta[0] = 0; + delta[1] = 120; + calculate_SCARA_forward_Transform(delta); + destination[0] = delta[0]/axis_scaling[X_AXIS]; + destination[1] = delta[1]/axis_scaling[Y_AXIS]; + + prepare_move(); + //ClearToSend(); + return; + } + break; + + case 361: // SCARA Theta pos2 + SERIAL_ECHOLN(" Cal: Theta 90 "); + //SoftEndsEnabled = false; // Ignore soft endstops during calibration + //SERIAL_ECHOLN(" Soft endstops disabled "); + if(Stopped == false) { + //get_coordinates(); // For X Y Z E F + delta[0] = 90; + delta[1] = 130; + calculate_SCARA_forward_Transform(delta); + destination[0] = delta[0]/axis_scaling[X_AXIS]; + destination[1] = delta[1]/axis_scaling[Y_AXIS]; + + prepare_move(); + //ClearToSend(); + return; + } + break; + case 362: // SCARA Psi pos1 + SERIAL_ECHOLN(" Cal: Psi 0 "); + //SoftEndsEnabled = false; // Ignore soft endstops during calibration + //SERIAL_ECHOLN(" Soft endstops disabled "); + if(Stopped == false) { + //get_coordinates(); // For X Y Z E F + delta[0] = 60; + delta[1] = 180; + calculate_SCARA_forward_Transform(delta); + destination[0] = delta[0]/axis_scaling[X_AXIS]; + destination[1] = delta[1]/axis_scaling[Y_AXIS]; + + prepare_move(); + //ClearToSend(); + return; + } + break; + case 363: // SCARA Psi pos2 + SERIAL_ECHOLN(" Cal: Psi 90 "); + //SoftEndsEnabled = false; // Ignore soft endstops during calibration + //SERIAL_ECHOLN(" Soft endstops disabled "); + if(Stopped == false) { + //get_coordinates(); // For X Y Z E F + delta[0] = 50; + delta[1] = 90; + calculate_SCARA_forward_Transform(delta); + destination[0] = delta[0]/axis_scaling[X_AXIS]; + destination[1] = delta[1]/axis_scaling[Y_AXIS]; + + prepare_move(); + //ClearToSend(); + return; + } + break; + case 364: // SCARA Psi pos3 (90 deg to Theta) + SERIAL_ECHOLN(" Cal: Theta-Psi 90 "); + // SoftEndsEnabled = false; // Ignore soft endstops during calibration + //SERIAL_ECHOLN(" Soft endstops disabled "); + if(Stopped == false) { + //get_coordinates(); // For X Y Z E F + delta[0] = 45; + delta[1] = 135; + calculate_SCARA_forward_Transform(delta); + destination[0] = delta[0]/axis_scaling[X_AXIS]; + destination[1] = delta[1]/axis_scaling[Y_AXIS]; + + prepare_move(); + //ClearToSend(); + return; + } + break; + case 365: // M364 Set SCARA scaling for X Y Z + for(int8_t i=0; i < 3; i++) + { + if(code_seen(axis_codes[i])) + { + + axis_scaling[i] = code_value(); + + } + } + break; + #endif case 400: // M400 finish all moves { st_synchronize(); @@ -3255,8 +3477,46 @@ void calculate_delta(float cartesian[3]) void prepare_move() { clamp_to_software_endstops(destination); - previous_millis_cmd = millis(); + + #ifdef SCARA //for now same as delta-code + +float difference[NUM_AXIS]; +for (int8_t i=0; i < NUM_AXIS; i++) { + difference[i] = destination[i] - current_position[i]; +} + +float cartesian_mm = sqrt( sq(difference[X_AXIS]) + + sq(difference[Y_AXIS]) + + sq(difference[Z_AXIS])); +if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); } +if (cartesian_mm < 0.000001) { return; } +float seconds = 6000 * cartesian_mm / feedrate / feedmultiply; +int steps = max(1, int(scara_segments_per_second * seconds)); + //SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm); + //SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds); + //SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps); +for (int s = 1; s <= steps; s++) { + float fraction = float(s) / float(steps); + for(int8_t i=0; i < NUM_AXIS; i++) { + destination[i] = current_position[i] + difference[i] * fraction; + } + + + calculate_delta(destination); + //SERIAL_ECHOPGM("destination[0]="); SERIAL_ECHOLN(destination[0]); + //SERIAL_ECHOPGM("destination[1]="); SERIAL_ECHOLN(destination[1]); + //SERIAL_ECHOPGM("destination[2]="); SERIAL_ECHOLN(destination[2]); + //SERIAL_ECHOPGM("delta[X_AXIS]="); SERIAL_ECHOLN(delta[X_AXIS]); + //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]); + //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]); + + plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], + destination[E_AXIS], feedrate*feedmultiply/60/100.0, + active_extruder); +} +#endif // SCARA + #ifdef DELTA float difference[NUM_AXIS]; for (int8_t i=0; i < NUM_AXIS; i++) { @@ -3282,7 +3542,8 @@ void prepare_move() destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder); } -#else + +#endif // DELTA #ifdef DUAL_X_CARRIAGE if (active_extruder_parked) @@ -3325,6 +3586,7 @@ void prepare_move() } #endif //DUAL_X_CARRIAGE +#if ! (defined DELTA || defined SCARA) // Do not use feedmultiply for E or Z only moves if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) { plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); @@ -3332,7 +3594,8 @@ void prepare_move() else { plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder); } -#endif //else DELTA +#endif // !(DELTA || SCARA) + for(int8_t i=0; i < NUM_AXIS; i++) { current_position[i] = destination[i]; } @@ -3400,6 +3663,89 @@ void controllerFan() } #endif +#ifdef SCARA +void calculate_SCARA_forward_Transform(float f_scara[3]) +{ + // Perform forward kinematics, and place results in delta[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 x_sin, x_cos, y_sin, y_cos; + + //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; + + // 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 + + //SERIAL_ECHOPGM(" delta[X_AXIS]="); SERIAL_ECHO(delta[X_AXIS]); + //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]); +} + +void calculate_delta(float cartesian[3]){ + //reverse kinematics. + // Perform reversed kinematics, and place results in delta[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; + + 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; + #else + SCARA_C2 = ( pow(SCARA_pos[X_AXIS],2) + pow(SCARA_pos[Y_AXIS],2) - L1_2 - L2_2 ) / 45000; + #endif + + SCARA_S2 = sqrt( 1 - pow(SCARA_C2,2) ); + + SCARA_K1 = Linkage_1/1000+Linkage_2/1000*SCARA_C2; + SCARA_K2 = Linkage_2/1000*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); + + 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]); + 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_ECHOLN(" "); + */ +} + +#endif + #ifdef TEMP_STAT_LEDS static bool blue_led = false; static bool red_led = false; diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h new file mode 100644 index 0000000000..b3a8cd1d2b --- /dev/null +++ b/Marlin/example_configurations/SCARA/Configuration.h @@ -0,0 +1,743 @@ +#ifndef CONFIGURATION_H +#define CONFIGURATION_H + +// This configuration file contains the basic settings. +// Advanced settings can be found in Configuration_adv.h +// BASIC SETTINGS: select your board type, temperature sensor type, axis scaling, and endstop configuration + +//=========================================================================== +//========================= SCARA Settings ================================== +//=========================================================================== +// SCARA-mode for Marlin has been 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 +// 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 + +// 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 + +//=========================================================================== +//========================= SCARA Settings end ================================== +//=========================================================================== + +// User-specified version info of this build to display in [Pronterface, etc] terminal window during +// startup. Implementation of an idea by Prof Braino to inform user that any changes made to this +// build by the user have been successfully uploaded into firmware. +#define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time +#define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes. + +// SERIAL_PORT selects which serial port should be used for communication with the host. +// This allows the connection of wireless adapters (for instance) to non-default port pins. +// Serial port 0 is still used by the Arduino bootloader regardless of this setting. +#define SERIAL_PORT 0 + +// This determines the communication speed of the printer +// This determines the communication speed of the printer +#define BAUDRATE 250000 + +// This enables the serial port associated to the Bluetooth interface +//#define BTENABLED // Enable BT interface on AT90USB devices + + +//// The following define selects which electronics board you have. Please choose the one that matches your setup +// 10 = Gen7 custom (Alfons3 Version) "https://github.com/Alfons3/Generation_7_Electronics" +// 11 = Gen7 v1.1, v1.2 = 11 +// 12 = Gen7 v1.3 +// 13 = Gen7 v1.4 +// 131 = OpenHardware.co.za custom Gen7 electronics +// 2 = Cheaptronic v1.0 +// 20 = Sethi 3D_1 +// 3 = MEGA/RAMPS up to 1.2 = 3 +// 33 = RAMPS 1.3 / 1.4 (Power outputs: Extruder, Fan, Bed) +// 34 = RAMPS 1.3 / 1.4 (Power outputs: Extruder0, Extruder1, Bed) +// 35 = RAMPS 1.3 / 1.4 (Power outputs: Extruder, Fan, Fan) +// 4 = Duemilanove w/ ATMega328P pin assignment +// 5 = Gen6 +// 51 = Gen6 deluxe +// 6 = Sanguinololu < 1.2 +// 62 = Sanguinololu 1.2 and above +// 63 = Melzi +// 64 = STB V1.1 +// 65 = Azteeg X1 +// 66 = Melzi with ATmega1284 (MaKr3d version) +// 67 = Azteeg X3 +// 68 = Azteeg X3 Pro +// 7 = Ultimaker +// 71 = Ultimaker (Older electronics. Pre 1.5.4. This is rare) +// 72 = Ultimainboard 2.x (Uses TEMP_SENSOR 20) +// 77 = 3Drag Controller +// 8 = Teensylu +// 80 = Rumba +// 81 = Printrboard (AT90USB1286) +// 82 = Brainwave (AT90USB646) +// 83 = SAV Mk-I (AT90USB1286) +// 84 = Teensy++2.0 (AT90USB1286) // CLI compile: DEFINES=AT90USBxx_TEENSYPP_ASSIGNMENTS HARDWARE_MOTHERBOARD=84 make +// 9 = Gen3+ +// 70 = Megatronics +// 701= Megatronics v2.0 +// 702= Minitronics v1.0 +// 90 = Alpha OMCA board +// 91 = Final OMCA board +// 301= Rambo +// 21 = Elefu Ra Board (v3) +// 88 = 5DPrint D8 Driver Board + +#ifndef MOTHERBOARD +#define MOTHERBOARD 33 +#endif + +// Define this to set a custom name for your generic Mendel, +// #define CUSTOM_MENDEL_NAME "This Mendel" + +// Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines) +// You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4) +// #define MACHINE_UUID "00000000-0000-0000-0000-000000000000" + +// This defines the number of extruders +#define EXTRUDERS 1 + +//// The following define selects which power supply you have. Please choose the one that matches your setup +// 1 = ATX +// 2 = X-Box 360 203Watts (the blue wire connected to PS_ON and the red wire to VCC) + +#define POWER_SUPPLY 1 + +// Define this to have the electronics keep the power supply off on startup. If you don't know what this is leave it. +// #define PS_DEFAULT_OFF + +//=========================================================================== +//=============================Thermal Settings ============================ +//=========================================================================== +// +//--NORMAL IS 4.7kohm PULLUP!-- 1kohm pullup can be used on hotend sensor, using correct resistor and table +// +//// Temperature sensor settings: +// -2 is thermocouple with MAX6675 (only for sensor 0) +// -1 is thermocouple with AD595 +// 0 is not used +// 1 is 100k thermistor - best choice for EPCOS 100k (4.7k pullup) +// 2 is 200k thermistor - ATC Semitec 204GT-2 (4.7k pullup) +// 3 is Mendel-parts thermistor (4.7k pullup) +// 4 is 10k thermistor !! do not use it for a hotend. It gives bad resolution at high temp. !! +// 5 is 100K thermistor - ATC Semitec 104GT-2 (Used in ParCan & J-Head) (4.7k pullup) +// 6 is 100k EPCOS - Not as accurate as table 1 (created using a fluke thermocouple) (4.7k pullup) +// 7 is 100k Honeywell thermistor 135-104LAG-J01 (4.7k pullup) +// 71 is 100k Honeywell thermistor 135-104LAF-J01 (4.7k pullup) +// 8 is 100k 0603 SMD Vishay NTCS0603E3104FXT (4.7k pullup) +// 9 is 100k GE Sensing AL03006-58.2K-97-G1 (4.7k pullup) +// 10 is 100k RS thermistor 198-961 (4.7k pullup) +// 11 is 100k beta 3950 1% thermistor (4.7k pullup) +// 12 is 100k 0603 SMD Vishay NTCS0603E3104FXT (4.7k pullup) (calibrated for Makibox hot bed) +// 20 is the PT100 circuit found in the Ultimainboard V2.x +// 60 is 100k Maker's Tool Works Kapton Bed Thermistor beta=3950 +// +// 1k ohm pullup tables - This is not normal, you would have to have changed out your 4.7k for 1k +// (but gives greater accuracy and more stable PID) +// 51 is 100k thermistor - EPCOS (1k pullup) +// 52 is 200k thermistor - ATC Semitec 204GT-2 (1k pullup) +// 55 is 100k thermistor - ATC Semitec 104GT-2 (Used in ParCan & J-Head) (1k pullup) +// +// 1047 is Pt1000 with 4k7 pullup +// 1010 is Pt1000 with 1k pullup (non standard) +// 147 is Pt100 with 4k7 pullup +// 110 is Pt100 with 1k pullup (non standard) + +#define TEMP_SENSOR_0 1 +#define TEMP_SENSOR_1 0 +#define TEMP_SENSOR_2 0 +#define TEMP_SENSOR_BED 1 + +// This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted. +//#define TEMP_SENSOR_1_AS_REDUNDANT +#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_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 +// to check that the wiring to the thermistor is not broken. +// Otherwise this would lead to the heater being powered on all the time. +#define HEATER_0_MINTEMP 5 +#define HEATER_1_MINTEMP 5 +#define HEATER_2_MINTEMP 5 +#define BED_MINTEMP 5 + +// When temperature exceeds max temp, your heater will be switched off. +// This feature exists to protect your hotend from overheating accidentally, but *NOT* from thermistor short/failure! +// You should use MINTEMP for thermistor short/failure protection. +#define HEATER_0_MAXTEMP 275 +#define HEATER_1_MAXTEMP 275 +#define HEATER_2_MAXTEMP 275 +#define BED_MAXTEMP 150 + +// If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the +// average current. The value should be an integer and the heat bed will be turned on for 1 interval of +// HEATER_BED_DUTY_CYCLE_DIVIDER intervals. +//#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 + +// PID settings: +// Comment the following line to disable PID and enable bang-bang. +#define PIDTEMP +#define BANG_MAX 255 // limits current to nozzle while in bang-bang mode; 255=full current +#define PID_MAX 255 // limits current to nozzle while PID is active (see PID_FUNCTIONAL_RANGE below); 255=full current +#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 + // 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 + #define PID_dT ((OVERSAMPLENR * 8.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine + +// 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 + +// MakerGear +// #define DEFAULT_Kp 7.0 +// #define DEFAULT_Ki 0.1 +// #define DEFAULT_Kd 12 + +// Mendel Parts V9 on 12V +// #define DEFAULT_Kp 63.0 +// #define DEFAULT_Ki 2.25 +// #define DEFAULT_Kd 440 +#endif // PIDTEMP + +// Bed Temperature Control +// Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis +// +// Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder. +// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz, +// which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating. +// This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater. +// 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 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) +// setting this to anything other than 255 enables a form of PWM to the bed just like HEATER_BED_DUTY_CYCLE_DIVIDER did, +// so you shouldn't use it unless you are OK with PWM on your bed. (see the comment on enabling PIDTEMPBED) +#define MAX_BED_POWER 255 // limits duty cycle to bed; 255=full current + +#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 + +//120v 250W silicone heater into 4mm borosilicate (MendelMax 1.5+) +//from pidautotune +// #define DEFAULT_bedKp 97.1 +// #define DEFAULT_bedKi 1.41 +// #define DEFAULT_bedKd 1675.16 + +// FIND YOUR OWN: "M303 E-1 C8 S90" to run autotune on the bed at 90 degreesC for 8 cycles. +#endif // PIDTEMPBED + + + +//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 +//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_MAXLENGTH (X_MAX_LENGTH+Y_MAX_LENGTH) //prevent extrusion of very large distances. + +//=========================================================================== +//=============================Mechanical Settings=========================== +//=========================================================================== + +// Uncomment the following line to enable CoreXY kinematics +// #define COREXY + +// coarse Endstop Settings +//#define ENDSTOPPULLUPS // Comment this out (using // at the start of the line) to disable the endstop pullup resistors + +#ifndef ENDSTOPPULLUPS + // fine endstop settings: Individual pullups. will be ignored if ENDSTOPPULLUPS is defined + // #define ENDSTOPPULLUP_XMAX + // #define ENDSTOPPULLUP_YMAX + #define ENDSTOPPULLUP_ZMAX // open pin, inverted + #define ENDSTOPPULLUP_XMIN // open pin, inverted + #define ENDSTOPPULLUP_YMIN // open pin, inverted + // #define ENDSTOPPULLUP_ZMIN +#endif + +#ifdef ENDSTOPPULLUPS + #define ENDSTOPPULLUP_XMAX + #define ENDSTOPPULLUP_YMAX + #define ENDSTOPPULLUP_ZMAX + #define ENDSTOPPULLUP_XMIN + #define ENDSTOPPULLUP_YMIN + #define ENDSTOPPULLUP_ZMIN +#endif + +// The pullups are needed if you directly connect a mechanical endswitch between the signal and ground pins. +const bool X_MIN_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop. +const bool Y_MIN_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop. +const bool Z_MIN_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop. +const bool X_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop. +const bool Y_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop. +const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of the endstop. +//#define DISABLE_MAX_ENDSTOPS +//#define DISABLE_MIN_ENDSTOPS + +// Disable max endstops for compatibility with endstop checking routine +#if defined(COREXY) && !defined(DISABLE_MAX_ENDSTOPS) + #define DISABLE_MAX_ENDSTOPS +#endif + +// For Inverting Stepper Enable Pins (Active Low) use 0, Non Inverting (Active High) use 1 +#define X_ENABLE_ON 0 +#define Y_ENABLE_ON 0 +#define Z_ENABLE_ON 0 +#define E_ENABLE_ON 0 // For all extruders + +// Disables axis when it's not being used. +#define DISABLE_X false +#define DISABLE_Y false +#define DISABLE_Z false +#define DISABLE_E false // For all extruders +#define DISABLE_INACTIVE_EXTRUDER true //disable only inactive extruders and keep active extruder enabled + +#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_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 + +// ENDSTOP SETTINGS: +// Sets direction of endstop s when homing; 1=MAX, -1=MIN +#define X_HOME_DIR 1 +#define Y_HOME_DIR 1 +#define Z_HOME_DIR -1 + +#define min_software_endstops true // If true, axis won't move to coordinates less than HOME_POS. +#define max_software_endstops true // If true, axis won't move to coordinates greater than the defined lengths below. + +// Travel limits after homing +#define X_MAX_POS 200 +#define X_MIN_POS 0 +#define Y_MAX_POS 200 +#define Y_MIN_POS 0 +#define Z_MAX_POS 225 +#define Z_MIN_POS MANUAL_Z_HOME_POS + +#define X_MAX_LENGTH (X_MAX_POS - X_MIN_POS) +#define Y_MAX_LENGTH (Y_MAX_POS - Y_MIN_POS) +#define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS) +//============================= Bed Auto Leveling =========================== + +//#define ENABLE_AUTO_BED_LEVELING // Delete the comment to enable (remove // at the start of the line) + +#ifdef ENABLE_AUTO_BED_LEVELING + +// There are 2 different ways to pick the X and Y locations to probe: + +// - "grid" mode +// Probe every point in a rectangular grid +// You must specify the rectangle, and the density of sample points +// This mode is preferred because there are more measurements. +// It used to be called ACCURATE_BED_LEVELING but "grid" is more descriptive + +// - "3-point" mode +// Probe 3 arbitrary points on the bed (that aren't colinear) +// You must specify the X & Y coordinates of all 3 points + + #define AUTO_BED_LEVELING_GRID + // with AUTO_BED_LEVELING_GRID, the bed is sampled in a + // AUTO_BED_LEVELING_GRID_POINTSxAUTO_BED_LEVELING_GRID_POINTS grid + // and least squares solution is calculated + // Note: this feature occupies 10'206 byte + #ifdef AUTO_BED_LEVELING_GRID + + // set the rectangle in which to probe + #define LEFT_PROBE_BED_POSITION 15 + #define RIGHT_PROBE_BED_POSITION 170 + #define BACK_PROBE_BED_POSITION 180 + #define FRONT_PROBE_BED_POSITION 20 + + // set the number of grid points per dimension + // I wouldn't see a reason to go above 3 (=9 probing points on the bed) + #define AUTO_BED_LEVELING_GRID_POINTS 2 + + + #else // not AUTO_BED_LEVELING_GRID + // with no grid, just probe 3 arbitrary points. A simple cross-product + // is used to esimate the plane of the print bed + + #define ABL_PROBE_PT_1_X 15 + #define ABL_PROBE_PT_1_Y 180 + #define ABL_PROBE_PT_2_X 15 + #define ABL_PROBE_PT_2_Y 20 + #define ABL_PROBE_PT_3_X 170 + #define ABL_PROBE_PT_3_Y 20 + + #endif // AUTO_BED_LEVELING_GRID + + + // these are the offsets to the probe relative to the extruder tip (Hotend - Probe) + #define X_PROBE_OFFSET_FROM_EXTRUDER -25 + #define Y_PROBE_OFFSET_FROM_EXTRUDER -29 + #define Z_PROBE_OFFSET_FROM_EXTRUDER -12.35 + + //#define Z_RAISE_BEFORE_HOMING 4 // (in mm) Raise Z before homing (G28) for Probe Clearance. + // Be sure you have this distance over your Z_MAX_POS in case + + #define XY_TRAVEL_SPEED 8000 // X and Y axis travel speed between probes, in mm/min + + #define Z_RAISE_BEFORE_PROBING 15 //How much the extruder will be raised before traveling to the first probing point. + #define Z_RAISE_BETWEEN_PROBINGS 5 //How much the extruder will be raised when traveling from between next probing points + + + //If defined, the Probe servo will be turned on only during movement and then turned off to avoid jerk + //The value is the delay to turn the servo off after powered on - depends on the servo speed; 300ms is good value, but you can try lower it. + // You MUST HAVE the SERVO_ENDSTOPS defined to use here a value higher than zero otherwise your code will not compile. + +// #define PROBE_SERVO_DEACTIVATION_DELAY 300 + + +//If you have enabled the Bed Auto Leveling and are using the same Z Probe for Z Homing, +//it is highly recommended you let this Z_SAFE_HOMING enabled!!! + + // #define Z_SAFE_HOMING // This feature is meant to avoid Z homing with probe outside the bed area. + // When defined, it will: + // - Allow Z homing only after X and Y homing AND stepper drivers still enabled + // - If stepper drivers timeout, it will need X and Y homing again before Z homing + // - Position the probe in a defined XY point before Z Homing when homing all axis (G28) + // - Block Z homing only when the probe is outside bed area. + + #ifdef Z_SAFE_HOMING + + #define Z_SAFE_HOMING_X_POINT (X_MAX_LENGTH/2) // X point for Z homing when homing all axis (G28) + #define Z_SAFE_HOMING_Y_POINT (Y_MAX_LENGTH/2) // Y point for Z homing when homing all axis (G28) + + #endif + +#endif // ENABLE_AUTO_BED_LEVELING + + +// The position of the homing switches +#define MANUAL_HOME_POSITIONS // If defined, MANUAL_*_HOME_POS below will be used +//#define BED_CENTER_AT_0_0 // If defined, the center of the bed is at (X=0, Y=0) + +//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_Z_HOME_POS 0.1 // Distance between nozzle and print surface after homing. + + +//// MOVEMENT SETTINGS +#define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E +#define HOMING_FEEDRATE {40*60, 40*60, 10*60, 0} // set the homing speeds (mm/min) + +// 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_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 + +// 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). +// 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) + +//=========================================================================== +//=============================Additional Features=========================== +//=========================================================================== + +// Custom M code points +//#define CUSTOM_M_CODES +#ifdef CUSTOM_M_CODES + #define CUSTOM_M_CODE_SET_Z_PROBE_OFFSET 851 + #define Z_PROBE_OFFSET_RANGE_MIN -15 + #define Z_PROBE_OFFSET_RANGE_MAX -5 +#endif + + +// EEPROM +// The microcontroller can store settings in the EEPROM, e.g. max velocity... +// M500 - stores parameters in EEPROM +// 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 +//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 + +// Preheat Constants +#define PLA_PREHEAT_HOTEND_TEMP 180 +#define PLA_PREHEAT_HPB_TEMP 70 +#define PLA_PREHEAT_FAN_SPEED 255 // Insert Value between 0 and 255 + +#define ABS_PREHEAT_HOTEND_TEMP 240 +#define ABS_PREHEAT_HPB_TEMP 100 +#define ABS_PREHEAT_FAN_SPEED 255 // Insert Value between 0 and 255 + +//LCD and SD support +//#define ULTRA_LCD //general LCD support, also 16x2 +//#define DOGLCD // Support for SPI LCD 128x64 (Controller ST7565R graphic Display Family) +//#define SDSUPPORT // Enable SD Card Support in Hardware Console +//#define SDSLOW // Use slower SD transfer mode (not normally needed - uncomment if you're getting volume init error) +//#define SD_CHECK_AND_RETRY // Use CRC checks and retries on the SD communication +//#define ENCODER_PULSES_PER_STEP 1 // Increase if you have a high resolution encoder +//#define ENCODER_STEPS_PER_MENU_ITEM 5 // Set according to ENCODER_PULSES_PER_STEP or your liking +//#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store. +//#define ULTIPANEL //the UltiPanel as on Thingiverse +//#define LCD_FEEDBACK_FREQUENCY_HZ 1000 // this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click +//#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 // the duration the buzzer plays the UI feedback sound. ie Screen Click + +// The MaKr3d Makr-Panel with graphic controller and SD support +// http://reprap.org/wiki/MaKr3d_MaKrPanel +//#define MAKRPANEL + +// The RepRapDiscount Smart Controller (white PCB) +// http://reprap.org/wiki/RepRapDiscount_Smart_Controller +//#define REPRAP_DISCOUNT_SMART_CONTROLLER + +// The GADGETS3D G3D LCD/SD Controller (blue PCB) +// http://reprap.org/wiki/RAMPS_1.3/1.4_GADGETS3D_Shield_with_Panel +//#define G3D_PANEL + +// The RepRapDiscount FULL GRAPHIC Smart Controller (quadratic white PCB) +// http://reprap.org/wiki/RepRapDiscount_Full_Graphic_Smart_Controller +// +// ==> REMEMBER TO INSTALL U8glib to your ARDUINO library folder: http://code.google.com/p/u8glib/wiki/u8glib +//#define REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER + +// The RepRapWorld REPRAPWORLD_KEYPAD v1.1 +// http://reprapworld.com/?products_details&products_id=202&cPath=1591_1626 +//#define REPRAPWORLD_KEYPAD +//#define REPRAPWORLD_KEYPAD_MOVE_STEP 10.0 // how much should be moved when a key is pressed, eg 10.0 means 10mm per click + +// The Elefu RA Board Control Panel +// http://www.elefu.com/index.php?route=product/product&product_id=53 +// REMEMBER TO INSTALL LiquidCrystal_I2C.h in your ARUDINO library folder: https://github.com/kiyoshigawa/LiquidCrystal_I2C +//#define RA_CONTROL_PANEL + +//automatic expansion +#if defined (MAKRPANEL) + #define DOGLCD + #define SDSUPPORT + #define ULTIPANEL + #define NEWPANEL + #define DEFAULT_LCD_CONTRAST 17 +#endif + +#if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER) + #define DOGLCD + #define U8GLIB_ST7920 + #define REPRAP_DISCOUNT_SMART_CONTROLLER +#endif + +#if defined(ULTIMAKERCONTROLLER) || defined(REPRAP_DISCOUNT_SMART_CONTROLLER) || defined(G3D_PANEL) + #define ULTIPANEL + #define NEWPANEL +#endif + +#if defined(REPRAPWORLD_KEYPAD) + #define NEWPANEL + #define ULTIPANEL +#endif +#if defined(RA_CONTROL_PANEL) + #define ULTIPANEL + #define NEWPANEL + #define LCD_I2C_TYPE_PCA8574 + #define LCD_I2C_ADDRESS 0x27 // I2C Address of the port expander +#endif + +//I2C PANELS + +//#define LCD_I2C_SAINSMART_YWROBOT +#ifdef LCD_I2C_SAINSMART_YWROBOT + // This uses the LiquidCrystal_I2C library ( https://bitbucket.org/fmalpartida/new-liquidcrystal/wiki/Home ) + // Make sure it is placed in the Arduino libraries directory. + #define LCD_I2C_TYPE_PCF8575 + #define LCD_I2C_ADDRESS 0x27 // I2C Address of the port expander + #define NEWPANEL + #define ULTIPANEL +#endif + +// PANELOLU2 LCD with status LEDs, separate encoder and click inputs +//#define LCD_I2C_PANELOLU2 +#ifdef LCD_I2C_PANELOLU2 + // This uses the LiquidTWI2 library v1.2.3 or later ( https://github.com/lincomatic/LiquidTWI2 ) + // Make sure the LiquidTWI2 directory is placed in the Arduino or Sketchbook libraries subdirectory. + // (v1.2.3 no longer requires you to define PANELOLU in the LiquidTWI2.h library header file) + // Note: The PANELOLU2 encoder click input can either be directly connected to a pin + // (if BTN_ENC defined to != -1) or read through I2C (when BTN_ENC == -1). + #define LCD_I2C_TYPE_MCP23017 + #define LCD_I2C_ADDRESS 0x20 // I2C Address of the port expander + #define LCD_USE_I2C_BUZZER //comment out to disable buzzer on LCD + #define NEWPANEL + #define ULTIPANEL + + #ifndef ENCODER_PULSES_PER_STEP + #define ENCODER_PULSES_PER_STEP 4 + #endif + + #ifndef ENCODER_STEPS_PER_MENU_ITEM + #define ENCODER_STEPS_PER_MENU_ITEM 1 + #endif + + + #ifdef LCD_USE_I2C_BUZZER + #define LCD_FEEDBACK_FREQUENCY_HZ 1000 + #define LCD_FEEDBACK_FREQUENCY_DURATION_MS 100 + #endif + +#endif + +// Panucatt VIKI LCD with status LEDs, integrated click & L/R/U/P buttons, separate encoder inputs +//#define LCD_I2C_VIKI +#ifdef LCD_I2C_VIKI + // This uses the LiquidTWI2 library v1.2.3 or later ( https://github.com/lincomatic/LiquidTWI2 ) + // Make sure the LiquidTWI2 directory is placed in the Arduino or Sketchbook libraries subdirectory. + // Note: The pause/stop/resume LCD button pin should be connected to the Arduino + // BTN_ENC pin (or set BTN_ENC to -1 if not used) + #define LCD_I2C_TYPE_MCP23017 + #define LCD_I2C_ADDRESS 0x20 // I2C Address of the port expander + #define LCD_USE_I2C_BUZZER //comment out to disable buzzer on LCD (requires LiquidTWI2 v1.2.3 or later) + #define NEWPANEL + #define ULTIPANEL +#endif + +// Shift register panels +// --------------------- +// 2 wire Non-latching LCD SR from: +// https://bitbucket.org/fmalpartida/new-liquidcrystal/wiki/schematics#!shiftregister-connection +//#define SR_LCD +#ifdef SR_LCD + #define SR_LCD_2W_NL // Non latching 2 wire shift register + //#define NEWPANEL +#endif + + +#ifdef ULTIPANEL +// #define NEWPANEL //enable this if you have a click-encoder panel + #define SDSUPPORT + #define ULTRA_LCD + #ifdef DOGLCD // Change number of lines to match the DOG graphic display + #define LCD_WIDTH 20 + #define LCD_HEIGHT 5 + #else + #define LCD_WIDTH 20 + #define LCD_HEIGHT 4 + #endif +#else //no panel but just LCD + #ifdef ULTRA_LCD + #ifdef DOGLCD // Change number of lines to match the 128x64 graphics display + #define LCD_WIDTH 20 + #define LCD_HEIGHT 5 + #else + #define LCD_WIDTH 16 + #define LCD_HEIGHT 2 + #endif + #endif +#endif + +// default LCD contrast for dogm-like LCD displays +#ifdef DOGLCD +# ifndef DEFAULT_LCD_CONTRAST +# define DEFAULT_LCD_CONTRAST 32 +# endif +#endif + +// Increase the FAN pwm frequency. Removes the PWM noise but increases heating in the FET/Arduino +//#define FAST_PWM_FAN + +// Temperature status LEDs that display the hotend and bet temperature. +// If all hotends and bed temperature and temperature setpoint are < 54C then the BLUE led is on. +// Otherwise the RED led is on. There is 1C hysteresis. +//#define TEMP_STAT_LEDS + +// Use software PWM to drive the fan, as for the heaters. This uses a very low frequency +// which is not ass annoying as with the hardware PWM. On the other hand, if this frequency +// is too low, you should also increment SOFT_PWM_SCALE. +//#define FAN_SOFT_PWM + +// Incrementing this by 1 will double the software PWM frequency, +// affecting heaters, and the fan if FAN_SOFT_PWM is enabled. +// However, control resolution will be halved for each increment; +// at zero value, there are 128 effective control positions. +#define SOFT_PWM_SCALE 0 + +// M240 Triggers a camera by emulating a Canon RC-1 Remote +// Data from: http://www.doc-diy.net/photo/rc-1_hacked/ +// #define PHOTOGRAPH_PIN 23 + +// SF send wrong arc g-codes when using Arc Point as fillet procedure +//#define SF_ARC_FIX + +// Support for the BariCUDA Paste Extruder. +//#define BARICUDA + +//define BlinkM/CyzRgb Support +//#define BLINKM + +/*********************************************************************\ +* R/C SERVO support +* Sponsored by TrinityLabs, Reworked by codexmas +**********************************************************************/ + +// Number of servos +// +// If you select a configuration below, this will receive a default value and does not need to be set manually +// set it manually if you have more servos than extruders and wish to manually control some +// leaving it undefined or defining as 0 will disable the servo subsystem +// If unsure, leave commented / disabled +// +//#define NUM_SERVOS 3 // Servo index starts with 0 for M280 command + +// Servo Endstops +// +// This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes. +// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500. +// +//#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1 +//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles + +#include "Configuration_adv.h" +#include "thermistortables.h" + +#endif //__CONFIGURATION_H diff --git a/Marlin/example_configurations/SCARA/Configuration_adv.h b/Marlin/example_configurations/SCARA/Configuration_adv.h new file mode 100644 index 0000000000..e500d3d51b --- /dev/null +++ b/Marlin/example_configurations/SCARA/Configuration_adv.h @@ -0,0 +1,507 @@ +#ifndef CONFIGURATION_ADV_H +#define CONFIGURATION_ADV_H + +//=========================================================================== +//=============================Thermal Settings ============================ +//=========================================================================== + +#ifdef BED_LIMIT_SWITCHING + #define BED_HYSTERESIS 2 //only disable heating if T>target+BED_HYSTERESIS and enable heating if T>target-BED_HYSTERESIS +#endif +#define BED_CHECK_INTERVAL 5000 //ms between checks in bang-bang control + +//// Heating sanity check: +// This waits for the watch period in milliseconds whenever an M104 or M109 increases the target temperature +// If the temperature has not increased at the end of that period, the target temperature is set to zero. +// It can be reset with another M104/M109. This check is also only triggered if the target temperature and the current temperature +// differ by at least 2x WATCH_TEMP_INCREASE +//#define WATCH_TEMP_PERIOD 40000 //40 seconds +//#define WATCH_TEMP_INCREASE 10 //Heat up at least 10 degree in 20 seconds + +#ifdef PIDTEMP + // this adds an experimental additional term to the heating power, proportional to the extrusion speed. + // if Kc is chosen well, the additional required power due to increased melting should be compensated. + #define PID_ADD_EXTRUSION_RATE + #ifdef PID_ADD_EXTRUSION_RATE + #define DEFAULT_Kc (1) //heating power=Kc*(e_speed) + #endif +#endif + + +//automatic temperature: The hot end target temperature is calculated by all the buffered lines of gcode. +//The maximum buffered steps/sec of the extruder motor are called "se". +//You enter the autotemp mode by a M109 S B F +// the target temperature is set to mintemp+factor*se[steps/sec] and limited by mintemp and maxtemp +// you exit the value by any M109 without F* +// Also, if the temperature is set to a value +// Mode 0: Full control. The slicer has full control over both x-carriages and can achieve optimal travel results +// as long as it supports dual x-carriages. (M605 S0) +// Mode 1: Auto-park mode. The firmware will automatically park and unpark the x-carriages on tool changes so +// that additional slicer support is not required. (M605 S1) +// Mode 2: Duplication mode. The firmware will transparently make the second x-carriage and extruder copy all +// actions of the first x-carriage. This allows the printer to print 2 arbitrary items at +// once. (2nd extruder x offset and temp offset are set using: M605 S2 [Xnnn] [Rmmm]) + +// This is the default power-up mode which can be later using M605. +#define DEFAULT_DUAL_X_CARRIAGE_MODE 0 + +// As the x-carriages are independent we can now account for any relative Z offset +#define EXTRUDER1_Z_OFFSET 0.0 // z offset relative to extruder 0 + +// Default settings in "Auto-park Mode" +#define TOOLCHANGE_PARK_ZLIFT 0.2 // the distance to raise Z axis when parking an extruder +#define TOOLCHANGE_UNPARK_ZLIFT 1 // the distance to raise Z axis when unparking an extruder + +// Default x offset in duplication mode (typically set to half print bed width) +#define DEFAULT_DUPLICATION_X_OFFSET 100 + +#endif //DUAL_X_CARRIAGE + +//homing hits the endstop, then retracts by this distance, before it tries to slowly bump again: +#define X_HOME_RETRACT_MM 3 +#define Y_HOME_RETRACT_MM 3 +#define Z_HOME_RETRACT_MM 3 +//#define QUICK_HOME //if this is defined, if both x and y are to be homed, a diagonal move will be performed initially. +#ifdef SCARA + #define QUICK_HOME //SCARA needs Quickhome +#endif + +#define AXIS_RELATIVE_MODES {false, false, false, false} + +#define MAX_STEP_FREQUENCY 40000 // Max step frequency for Ultimaker (5000 pps / half step) + +//By default pololu step drivers require an active high signal. However, some high power drivers require an active low signal as step. +#define INVERT_X_STEP_PIN false +#define INVERT_Y_STEP_PIN false +#define INVERT_Z_STEP_PIN false +#define INVERT_E_STEP_PIN false + +//default stepper release if idle +#define DEFAULT_STEPPER_DEACTIVE_TIME 240 + +#define DEFAULT_MINIMUMFEEDRATE 0.0 // minimum feedrate +#define DEFAULT_MINTRAVELFEEDRATE 0.0 + +// Feedrates for manual moves along X, Y, Z, E from panel +#ifdef ULTIPANEL +#define MANUAL_FEEDRATE {50*60, 50*60, 10*60, 60} // set the speeds for manual moves (mm/min) +#endif + +//Comment to disable setting feedrate multiplier via encoder +#ifdef ULTIPANEL + #define ULTIPANEL_FEEDMULTIPLY +#endif + +// minimum time in microseconds that a movement needs to take if the buffer is emptied. +#define DEFAULT_MINSEGMENTTIME 20000 + +// If defined the movements slow down when the look ahead buffer is only half full +//#define SLOWDOWN +#ifdef SCARA + #undef SLOWDOWN +#endif +// Frequency limit +// See nophead's blog for more info +// Not working O +//#define XY_FREQUENCY_LIMIT 15 + +// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end +// of the buffer and all stops. This should not be much greater than zero and should only be changed +// if unwanted behavior is observed on a user's machine when running at very slow speeds. +#define MINIMUM_PLANNER_SPEED 0.05// (mm/sec) + +// MS1 MS2 Stepper Driver Microstepping mode table +#define MICROSTEP1 LOW,LOW +#define MICROSTEP2 HIGH,LOW +#define MICROSTEP4 LOW,HIGH +#define MICROSTEP8 HIGH,HIGH +#define MICROSTEP16 HIGH,HIGH + +// Microstep setting (Only functional when stepper driver microstep pins are connected to MCU. +#define MICROSTEP_MODES {16,16,16,16,16} // [1,2,4,8,16] + +// Motor Current setting (Only functional when motor driver current ref pins are connected to a digital trimpot on supported boards) +#define DIGIPOT_MOTOR_CURRENT {135,135,135,135,135} // Values 0-255 (RAMBO 135 = ~0.75A, 185 = ~1A) + +// uncomment to enable an I2C based DIGIPOT like on the Azteeg X3 Pro +//#define DIGIPOT_I2C +// Number of channels available for I2C digipot, For Azteeg X3 Pro we have 8 +#define DIGIPOT_I2C_NUM_CHANNELS 8 +// actual motor currents in Amps, need as many here as DIGIPOT_I2C_NUM_CHANNELS +#define DIGIPOT_I2C_MOTOR_CURRENTS {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0} + +//=========================================================================== +//=============================Additional Features=========================== +//=========================================================================== + +//#define CHDK 4 //Pin for triggering CHDK to take a picture see how to use it here http://captain-slow.dk/2014/03/09/3d-printing-timelapses/ +#define CHDK_DELAY 50 //How long in ms the pin should stay HIGH before going LOW again + +#define SD_FINISHED_STEPPERRELEASE true //if sd support and the file is finished: disable steppers? +#define SD_FINISHED_RELEASECOMMAND "M84 X Y Z E" // You might want to keep the z enabled so your bed stays in place. + +#define SDCARD_RATHERRECENTFIRST //reverse file order of sd card menu display. Its sorted practically after the file system block order. +// if a file is deleted, it frees a block. hence, the order is not purely chronological. To still have auto0.g accessible, there is again the option to do that. +// using: +//#define MENU_ADDAUTOSTART + +// The hardware watchdog should reset the microcontroller disabling all outputs, in case the firmware gets stuck and doesn't do temperature regulation. +//#define USE_WATCHDOG + +#ifdef USE_WATCHDOG +// If you have a watchdog reboot in an ArduinoMega2560 then the device will hang forever, as a watchdog reset will leave the watchdog on. +// The "WATCHDOG_RESET_MANUAL" goes around this by not using the hardware reset. +// However, THIS FEATURE IS UNSAFE!, as it will only work if interrupts are disabled. And the code could hang in an interrupt routine with interrupts disabled. +//#define WATCHDOG_RESET_MANUAL +#endif + +// Enable the option to stop SD printing when hitting and endstops, needs to be enabled from the LCD menu when this option is enabled. +//#define ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED + +// Babystepping enables the user to control the axis in tiny amounts, independently from the normal printing process +// it can e.g. be used to change z-positions in the print startup phase in real-time +// does not respect endstops! +//#define BABYSTEPPING +#ifdef BABYSTEPPING + #define BABYSTEP_XY //not only z, but also XY in the menu. more clutter, more functions + #define BABYSTEP_INVERT_Z false //true for inverse movements in Z + #define BABYSTEP_Z_MULTIPLICATOR 2 //faster z movements + + #ifdef COREXY + #error BABYSTEPPING not implemented for COREXY yet. + #endif + + #ifdef DELTA + #ifdef BABYSTEP_XY + #error BABYSTEPPING only implemented for Z axis on deltabots. + #endif + #endif + + #ifdef SCARA + #error BABYSTEPPING not implemented for SCARA yet. + #endif + +#endif + +// extruder advance constant (s2/mm3) +// +// advance (steps) = STEPS_PER_CUBIC_MM_E * EXTUDER_ADVANCE_K * cubic mm per second ^ 2 +// +// Hooke's law says: force = k * distance +// Bernoulli's principle says: v ^ 2 / 2 + g . h + pressure / density = constant +// so: v ^ 2 is proportional to number of steps we advance the extruder +//#define ADVANCE + +#ifdef ADVANCE + #define EXTRUDER_ADVANCE_K .0 + + #define D_FILAMENT 1.75 + #define STEPS_MM_E 836 + #define EXTRUTION_AREA (0.25 * D_FILAMENT * D_FILAMENT * 3.14159) + #define STEPS_PER_CUBIC_MM_E (axis_steps_per_unit[E_AXIS]/ EXTRUTION_AREA) + +#endif // ADVANCE + +// Arc interpretation settings: +#define MM_PER_ARC_SEGMENT 1 +#define N_ARC_CORRECTION 25 + +const unsigned int dropsegments=5; //everything with less than this number of steps will be ignored as move and joined with the next movement + +// If you are using a RAMPS board or cheap E-bay purchased boards that do not detect when an SD card is inserted +// You can get round this by connecting a push button or single throw switch to the pin defined as SDCARDCARDDETECT +// in the pins.h file. When using a push button pulling the pin to ground this will need inverted. This setting should +// be commented out otherwise +#define SDCARDDETECTINVERTED + +#ifdef ULTIPANEL + #undef SDCARDDETECTINVERTED +#endif + +// Power Signal Control Definitions +// By default use ATX definition +#ifndef POWER_SUPPLY + #define POWER_SUPPLY 1 +#endif +// 1 = ATX +#if (POWER_SUPPLY == 1) + #define PS_ON_AWAKE LOW + #define PS_ON_ASLEEP HIGH +#endif +// 2 = X-Box 360 203W +#if (POWER_SUPPLY == 2) + #define PS_ON_AWAKE HIGH + #define PS_ON_ASLEEP LOW +#endif + +// Control heater 0 and heater 1 in parallel. +//#define HEATERS_PARALLEL + +//=========================================================================== +//=============================Buffers ============================ +//=========================================================================== + +// The number of linear motions that can be in the plan at any give time. +// THE BLOCK_BUFFER_SIZE NEEDS TO BE A POWER OF 2, i.g. 8,16,32 because shifts and ors are used to do the ring-buffering. +#if defined SDSUPPORT + #define BLOCK_BUFFER_SIZE 16 // SD,LCD,Buttons take more memory, block buffer needs to be smaller +#else + #define BLOCK_BUFFER_SIZE 16 // maximize block buffer +#endif + + +//The ASCII buffer for receiving from the serial: +#define MAX_CMD_SIZE 96 +#define BUFSIZE 4 + + +// Firmware based and LCD controlled retract +// M207 and M208 can be used to define parameters for the retraction. +// The retraction can be called by the slicer using G10 and G11 +// until then, intended retractions can be detected by moves that only extrude and the direction. +// the moves are than replaced by the firmware controlled ones. + +// #define FWRETRACT //ONLY PARTIALLY TESTED +#ifdef FWRETRACT + #define MIN_RETRACT 0.1 //minimum extruded mm to accept a automatic gcode retraction attempt + #define RETRACT_LENGTH 3 //default retract length (positive mm) + #define RETRACT_FEEDRATE 30 //default feedrate for retracting (mm/s) + #define RETRACT_ZLIFT 0 //default retract Z-lift + #define RETRACT_RECOVER_LENGTH 0 //default additional recover length (mm, added to retract length when recovering) + #define RETRACT_RECOVER_FEEDRATE 8 //default feedrate for recovering from retraction (mm/s) +#endif + +//adds support for experimental filament exchange support M600; requires display +#ifdef ULTIPANEL + #define FILAMENTCHANGEENABLE + #ifdef FILAMENTCHANGEENABLE + #define FILAMENTCHANGE_XPOS 3 + #define FILAMENTCHANGE_YPOS 3 + #define FILAMENTCHANGE_ZADD 10 + #define FILAMENTCHANGE_FIRSTRETRACT -2 + #define FILAMENTCHANGE_FINALRETRACT -100 + #endif +#endif + +#ifdef FILAMENTCHANGEENABLE + #ifdef EXTRUDER_RUNOUT_PREVENT + #error EXTRUDER_RUNOUT_PREVENT currently incompatible with FILAMENTCHANGE + #endif +#endif + +//=========================================================================== +//============================= Define Defines ============================ +//=========================================================================== +#if EXTRUDERS > 1 && defined TEMP_SENSOR_1_AS_REDUNDANT + #error "You cannot use TEMP_SENSOR_1_AS_REDUNDANT if EXTRUDERS > 1" +#endif + +#if EXTRUDERS > 1 && defined HEATERS_PARALLEL + #error "You cannot use HEATERS_PARALLEL if EXTRUDERS > 1" +#endif + +#if TEMP_SENSOR_0 > 0 + #define THERMISTORHEATER_0 TEMP_SENSOR_0 + #define HEATER_0_USES_THERMISTOR +#endif +#if TEMP_SENSOR_1 > 0 + #define THERMISTORHEATER_1 TEMP_SENSOR_1 + #define HEATER_1_USES_THERMISTOR +#endif +#if TEMP_SENSOR_2 > 0 + #define THERMISTORHEATER_2 TEMP_SENSOR_2 + #define HEATER_2_USES_THERMISTOR +#endif +#if TEMP_SENSOR_BED > 0 + #define THERMISTORBED TEMP_SENSOR_BED + #define BED_USES_THERMISTOR +#endif +#if TEMP_SENSOR_0 == -1 + #define HEATER_0_USES_AD595 +#endif +#if TEMP_SENSOR_1 == -1 + #define HEATER_1_USES_AD595 +#endif +#if TEMP_SENSOR_2 == -1 + #define HEATER_2_USES_AD595 +#endif +#if TEMP_SENSOR_BED == -1 + #define BED_USES_AD595 +#endif +#if TEMP_SENSOR_0 == -2 + #define HEATER_0_USES_MAX6675 +#endif +#if TEMP_SENSOR_0 == 0 + #undef HEATER_0_MINTEMP + #undef HEATER_0_MAXTEMP +#endif +#if TEMP_SENSOR_1 == 0 + #undef HEATER_1_MINTEMP + #undef HEATER_1_MAXTEMP +#endif +#if TEMP_SENSOR_2 == 0 + #undef HEATER_2_MINTEMP + #undef HEATER_2_MAXTEMP +#endif +#if TEMP_SENSOR_BED == 0 + #undef BED_MINTEMP + #undef BED_MAXTEMP +#endif + + +#endif //__CONFIGURATION_ADV_H diff --git a/Marlin/thermistortables.h b/Marlin/thermistortables.h index 86bf5c2d4e..07b385e119 100644 --- a/Marlin/thermistortables.h +++ b/Marlin/thermistortables.h @@ -1021,81 +1021,6 @@ const short temptable_1047[][2] PROGMEM = { PtLine(300,1000,4700) }; #endif -#if (THERMISTORHEATER_0 == 70) || (THERMISTORHEATER_1 == 70) || (THERMISTORHEATER_2 == 70) || (THERMISTORBED == 70) // 500C thermistor for Pico hot end -const short temptable_70[][2] PROGMEM = { - { 110.774119598719*OVERSAMPLENR , 350 }, - { 118.214386957249*OVERSAMPLENR , 345 }, - { 126.211418543166*OVERSAMPLENR , 340 }, - { 134.789559066223*OVERSAMPLENR , 335 }, - { 144.004513869701*OVERSAMPLENR , 330 }, - { 153.884483790827*OVERSAMPLENR , 325 }, - { 164.484880793637*OVERSAMPLENR , 320 }, - { 175.848885102724*OVERSAMPLENR , 315 }, - { 188.006799079015*OVERSAMPLENR , 310 }, - { 201.008072969044*OVERSAMPLENR , 305 }, - { 214.83716032276*OVERSAMPLENR , 300 }, - { 229.784739779664*OVERSAMPLENR , 295 }, - { 245.499466045473*OVERSAMPLENR , 290 }, - { 262.2766342096*OVERSAMPLENR , 285 }, - { 280.073883176433*OVERSAMPLENR , 280 }, - { 298.952693467726*OVERSAMPLENR , 275 }, - { 318.808251051674*OVERSAMPLENR , 270 }, - { 337.490932563222*OVERSAMPLENR , 265 }, - { 361.683649122745*OVERSAMPLENR , 260 }, - { 384.717024083981*OVERSAMPLENR , 255 }, - { 408.659301759076*OVERSAMPLENR , 250 }, - { 433.471659455884*OVERSAMPLENR , 245 }, - { 459.199039926034*OVERSAMPLENR , 240 }, - { 485.566500982316*OVERSAMPLENR , 235 }, - { 512.538918631075*OVERSAMPLENR , 230 }, - { 539.980999544838*OVERSAMPLENR , 225 }, - { 567.783095549935*OVERSAMPLENR , 220 }, - { 595.698041673552*OVERSAMPLENR , 215 }, - { 623.633922319597*OVERSAMPLENR , 210 }, - { 651.356162750829*OVERSAMPLENR , 205 }, - { 678.700901620956*OVERSAMPLENR , 200 }, - { 705.528145361264*OVERSAMPLENR , 195 }, - { 731.61267976339*OVERSAMPLENR , 190 }, - { 756.786212184365*OVERSAMPLENR , 185 }, - { 780.950223357761*OVERSAMPLENR , 180 }, - { 804.012961595082*OVERSAMPLENR , 175 }, - { 825.904975939166*OVERSAMPLENR , 170 }, - { 846.403941639008*OVERSAMPLENR , 165 }, - { 865.52326974895*OVERSAMPLENR , 160 }, - { 883.246145367727*OVERSAMPLENR , 155 }, - { 899.5821946515*OVERSAMPLENR , 150 }, - { 914.544289228582*OVERSAMPLENR , 145 }, - { 928.145628221761*OVERSAMPLENR , 140 }, - { 940.422208546562*OVERSAMPLENR , 135 }, - { 951.456922916497*OVERSAMPLENR , 130 }, - { 961.303500633788*OVERSAMPLENR , 125 }, - { 970.044756889055*OVERSAMPLENR , 120 }, - { 977.761456230051*OVERSAMPLENR , 115 }, - { 984.540978083453*OVERSAMPLENR , 110 }, - { 990.440780765757*OVERSAMPLENR , 105 }, - { 995.589621465301*OVERSAMPLENR , 100 }, - { 1000.02514280144*OVERSAMPLENR , 95 }, - { 1003.84429789876*OVERSAMPLENR , 90 }, - { 1007.10199009318*OVERSAMPLENR , 85 }, - { 1009.87151698323*OVERSAMPLENR , 80 }, - { 1012.21633594237*OVERSAMPLENR , 75 }, - { 1014.18959892949*OVERSAMPLENR , 70 }, - { 1015.84079162998*OVERSAMPLENR , 65 }, - { 1017.21555915335*OVERSAMPLENR , 60 }, - { 1018.35284662863*OVERSAMPLENR , 55 }, - { 1019.28926921888*OVERSAMPLENR , 50 }, - { 1020.05398015669*OVERSAMPLENR , 45 }, - { 1020.67737496272*OVERSAMPLENR , 40 }, - { 1021.1802909627*OVERSAMPLENR , 35 }, - { 1021.58459281248*OVERSAMPLENR , 30 }, - { 1021.90701441192*OVERSAMPLENR , 25 }, - { 1022.16215103698*OVERSAMPLENR , 20 }, - { 1022.36275529549*OVERSAMPLENR , 15 }, - { 1022.51930392497*OVERSAMPLENR , 10 }, - { 1022.64051573734*OVERSAMPLENR , 5 }, - { 1022.73355805611*OVERSAMPLENR , 0 } -}; -#endif #define _TT_NAME(_N) temptable_ ## _N #define TT_NAME(_N) _TT_NAME(_N) diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index f09dd410d6..0c3ee32a96 100644 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -1,1626 +1,1630 @@ -#include "temperature.h" -#include "ultralcd.h" -#ifdef ULTRA_LCD -#include "Marlin.h" -#include "language.h" -#include "cardreader.h" -#include "temperature.h" -#include "stepper.h" -#include "ConfigurationStore.h" - -int8_t encoderDiff; /* encoderDiff is updated from interrupt context and added to encoderPosition every LCD update */ - -/* Configuration settings */ -int plaPreheatHotendTemp; -int plaPreheatHPBTemp; -int plaPreheatFanSpeed; - -int absPreheatHotendTemp; -int absPreheatHPBTemp; -int absPreheatFanSpeed; - - -#ifdef ULTIPANEL -static float manual_feedrate[] = MANUAL_FEEDRATE; -#endif // ULTIPANEL - -/* !Configuration settings */ - -//Function pointer to menu functions. -typedef void (*menuFunc_t)(); - -uint8_t lcd_status_message_level; -char lcd_status_message[LCD_WIDTH+1] = WELCOME_MSG; - -#ifdef DOGLCD -#include "dogm_lcd_implementation.h" -#else -#include "ultralcd_implementation_hitachi_HD44780.h" -#endif - -/** forward declarations **/ - -void copy_and_scalePID_i(); -void copy_and_scalePID_d(); - -/* Different menus */ -static void lcd_status_screen(); -#ifdef ULTIPANEL -extern bool powersupply; -static void lcd_main_menu(); -static void lcd_tune_menu(); -static void lcd_prepare_menu(); -static void lcd_move_menu(); -static void lcd_control_menu(); -static void lcd_control_temperature_menu(); -static void lcd_control_temperature_preheat_pla_settings_menu(); -static void lcd_control_temperature_preheat_abs_settings_menu(); -static void lcd_control_motion_menu(); -#ifdef DOGLCD -static void lcd_set_contrast(); -#endif -static void lcd_control_retract_menu(); -static void lcd_sdcard_menu(); - -static void lcd_quick_feedback();//Cause an LCD refresh, and give the user visual or audible feedback that something has happened - -/* Different types of actions that can be used in menu items. */ -static void menu_action_back(menuFunc_t data); -static void menu_action_submenu(menuFunc_t data); -static void menu_action_gcode(const char* pgcode); -static void menu_action_function(menuFunc_t data); -static void menu_action_sdfile(const char* filename, char* longFilename); -static void menu_action_sddirectory(const char* filename, char* longFilename); -static void menu_action_setting_edit_bool(const char* pstr, bool* ptr); -static void menu_action_setting_edit_int3(const char* pstr, int* ptr, int minValue, int maxValue); -static void menu_action_setting_edit_float3(const char* pstr, float* ptr, float minValue, float maxValue); -static void menu_action_setting_edit_float32(const char* pstr, float* ptr, float minValue, float maxValue); -static void menu_action_setting_edit_float5(const char* pstr, float* ptr, float minValue, float maxValue); -static void menu_action_setting_edit_float51(const char* pstr, float* ptr, float minValue, float maxValue); -static void menu_action_setting_edit_float52(const char* pstr, float* ptr, float minValue, float maxValue); -static void menu_action_setting_edit_long5(const char* pstr, unsigned long* ptr, unsigned long minValue, unsigned long maxValue); -static void menu_action_setting_edit_callback_bool(const char* pstr, bool* ptr, menuFunc_t callbackFunc); -static void menu_action_setting_edit_callback_int3(const char* pstr, int* ptr, int minValue, int maxValue, menuFunc_t callbackFunc); -static void menu_action_setting_edit_callback_float3(const char* pstr, float* ptr, float minValue, float maxValue, menuFunc_t callbackFunc); -static void menu_action_setting_edit_callback_float32(const char* pstr, float* ptr, float minValue, float maxValue, menuFunc_t callbackFunc); -static void menu_action_setting_edit_callback_float5(const char* pstr, float* ptr, float minValue, float maxValue, menuFunc_t callbackFunc); -static void menu_action_setting_edit_callback_float51(const char* pstr, float* ptr, float minValue, float maxValue, menuFunc_t callbackFunc); -static void menu_action_setting_edit_callback_float52(const char* pstr, float* ptr, float minValue, float maxValue, menuFunc_t callbackFunc); -static void menu_action_setting_edit_callback_long5(const char* pstr, unsigned long* ptr, unsigned long minValue, unsigned long maxValue, menuFunc_t callbackFunc); - -#define ENCODER_FEEDRATE_DEADZONE 10 - -#if !defined(LCD_I2C_VIKI) - #ifndef ENCODER_STEPS_PER_MENU_ITEM - #define ENCODER_STEPS_PER_MENU_ITEM 5 - #endif - #ifndef ENCODER_PULSES_PER_STEP - #define ENCODER_PULSES_PER_STEP 1 - #endif -#else - #ifndef ENCODER_STEPS_PER_MENU_ITEM - #define ENCODER_STEPS_PER_MENU_ITEM 2 // VIKI LCD rotary encoder uses a different number of steps per rotation - #endif - #ifndef ENCODER_PULSES_PER_STEP - #define ENCODER_PULSES_PER_STEP 1 - #endif -#endif - - -/* Helper macros for menus */ -#define START_MENU() do { \ - if (encoderPosition > 0x8000) encoderPosition = 0; \ - if (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM < currentMenuViewOffset) currentMenuViewOffset = encoderPosition / ENCODER_STEPS_PER_MENU_ITEM;\ - uint8_t _lineNr = currentMenuViewOffset, _menuItemNr; \ - bool wasClicked = LCD_CLICKED;\ - for(uint8_t _drawLineNr = 0; _drawLineNr < LCD_HEIGHT; _drawLineNr++, _lineNr++) { \ - _menuItemNr = 0; -#define MENU_ITEM(type, label, args...) do { \ - if (_menuItemNr == _lineNr) { \ - if (lcdDrawUpdate) { \ - const char* _label_pstr = PSTR(label); \ - if ((encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) == _menuItemNr) { \ - lcd_implementation_drawmenu_ ## type ## _selected (_drawLineNr, _label_pstr , ## args ); \ - }else{\ - lcd_implementation_drawmenu_ ## type (_drawLineNr, _label_pstr , ## args ); \ - }\ - }\ - if (wasClicked && (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) == _menuItemNr) {\ - lcd_quick_feedback(); \ - menu_action_ ## type ( args ); \ - return;\ - }\ - }\ - _menuItemNr++;\ -} while(0) -#define MENU_ITEM_DUMMY() do { _menuItemNr++; } while(0) -#define MENU_ITEM_EDIT(type, label, args...) MENU_ITEM(setting_edit_ ## type, label, PSTR(label) , ## args ) -#define MENU_ITEM_EDIT_CALLBACK(type, label, args...) MENU_ITEM(setting_edit_callback_ ## type, label, PSTR(label) , ## args ) -#define END_MENU() \ - if (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM >= _menuItemNr) encoderPosition = _menuItemNr * ENCODER_STEPS_PER_MENU_ITEM - 1; \ - if ((uint8_t)(encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) >= currentMenuViewOffset + LCD_HEIGHT) { currentMenuViewOffset = (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) - LCD_HEIGHT + 1; lcdDrawUpdate = 1; _lineNr = currentMenuViewOffset - 1; _drawLineNr = -1; } \ - } } while(0) - -/** Used variables to keep track of the menu */ -#ifndef REPRAPWORLD_KEYPAD -volatile uint8_t buttons;//Contains the bits of the currently pressed buttons. -#else -volatile uint8_t buttons_reprapworld_keypad; // to store the reprapworld_keypad shift register values -#endif -#ifdef LCD_HAS_SLOW_BUTTONS -volatile uint8_t slow_buttons;//Contains the bits of the currently pressed buttons. -#endif -uint8_t currentMenuViewOffset; /* scroll offset in the current menu */ -uint32_t blocking_enc; -uint8_t lastEncoderBits; -uint32_t encoderPosition; -#if (SDCARDDETECT > 0) -bool lcd_oldcardstatus; -#endif -#endif//ULTIPANEL - -menuFunc_t currentMenu = lcd_status_screen; /* function pointer to the currently active menu */ -uint32_t lcd_next_update_millis; -uint8_t lcd_status_update_delay; -uint8_t lcdDrawUpdate = 2; /* Set to none-zero when the LCD needs to draw, decreased after every draw. Set to 2 in LCD routines so the LCD gets at least 1 full redraw (first redraw is partial) */ - -//prevMenu and prevEncoderPosition are used to store the previous menu location when editing settings. -menuFunc_t prevMenu = NULL; -uint16_t prevEncoderPosition; -//Variables used when editing values. -const char* editLabel; -void* editValue; -int32_t minEditValue, maxEditValue; -menuFunc_t callbackFunc; - -// place-holders for Ki and Kd edits -float raw_Ki, raw_Kd; - -/* Main status screen. It's up to the implementation specific part to show what is needed. As this is very display dependent */ -static void lcd_status_screen() -{ - if (lcd_status_update_delay) - lcd_status_update_delay--; - else - lcdDrawUpdate = 1; - if (lcdDrawUpdate) - { - lcd_implementation_status_screen(); - lcd_status_update_delay = 10; /* redraw the main screen every second. This is easier then trying keep track of all things that change on the screen */ - } -#ifdef ULTIPANEL - if (LCD_CLICKED) - { - currentMenu = lcd_main_menu; - encoderPosition = 0; - lcd_quick_feedback(); +#include "temperature.h" +#include "ultralcd.h" +#ifdef ULTRA_LCD +#include "Marlin.h" +#include "language.h" +#include "cardreader.h" +#include "temperature.h" +#include "stepper.h" +#include "ConfigurationStore.h" + +int8_t encoderDiff; /* encoderDiff is updated from interrupt context and added to encoderPosition every LCD update */ + +/* Configuration settings */ +int plaPreheatHotendTemp; +int plaPreheatHPBTemp; +int plaPreheatFanSpeed; + +int absPreheatHotendTemp; +int absPreheatHPBTemp; +int absPreheatFanSpeed; + + +#ifdef ULTIPANEL +static float manual_feedrate[] = MANUAL_FEEDRATE; +#endif // ULTIPANEL + +/* !Configuration settings */ + +//Function pointer to menu functions. +typedef void (*menuFunc_t)(); + +uint8_t lcd_status_message_level; +char lcd_status_message[LCD_WIDTH+1] = WELCOME_MSG; + +#ifdef DOGLCD +#include "dogm_lcd_implementation.h" +#else +#include "ultralcd_implementation_hitachi_HD44780.h" +#endif + +/** forward declarations **/ + +void copy_and_scalePID_i(); +void copy_and_scalePID_d(); + +/* Different menus */ +static void lcd_status_screen(); +#ifdef ULTIPANEL +extern bool powersupply; +static void lcd_main_menu(); +static void lcd_tune_menu(); +static void lcd_prepare_menu(); +static void lcd_move_menu(); +static void lcd_control_menu(); +static void lcd_control_temperature_menu(); +static void lcd_control_temperature_preheat_pla_settings_menu(); +static void lcd_control_temperature_preheat_abs_settings_menu(); +static void lcd_control_motion_menu(); +#ifdef DOGLCD +static void lcd_set_contrast(); +#endif +static void lcd_control_retract_menu(); +static void lcd_sdcard_menu(); + +static void lcd_quick_feedback();//Cause an LCD refresh, and give the user visual or audible feedback that something has happened + +/* Different types of actions that can be used in menu items. */ +static void menu_action_back(menuFunc_t data); +static void menu_action_submenu(menuFunc_t data); +static void menu_action_gcode(const char* pgcode); +static void menu_action_function(menuFunc_t data); +static void menu_action_sdfile(const char* filename, char* longFilename); +static void menu_action_sddirectory(const char* filename, char* longFilename); +static void menu_action_setting_edit_bool(const char* pstr, bool* ptr); +static void menu_action_setting_edit_int3(const char* pstr, int* ptr, int minValue, int maxValue); +static void menu_action_setting_edit_float3(const char* pstr, float* ptr, float minValue, float maxValue); +static void menu_action_setting_edit_float32(const char* pstr, float* ptr, float minValue, float maxValue); +static void menu_action_setting_edit_float5(const char* pstr, float* ptr, float minValue, float maxValue); +static void menu_action_setting_edit_float51(const char* pstr, float* ptr, float minValue, float maxValue); +static void menu_action_setting_edit_float52(const char* pstr, float* ptr, float minValue, float maxValue); +static void menu_action_setting_edit_long5(const char* pstr, unsigned long* ptr, unsigned long minValue, unsigned long maxValue); +static void menu_action_setting_edit_callback_bool(const char* pstr, bool* ptr, menuFunc_t callbackFunc); +static void menu_action_setting_edit_callback_int3(const char* pstr, int* ptr, int minValue, int maxValue, menuFunc_t callbackFunc); +static void menu_action_setting_edit_callback_float3(const char* pstr, float* ptr, float minValue, float maxValue, menuFunc_t callbackFunc); +static void menu_action_setting_edit_callback_float32(const char* pstr, float* ptr, float minValue, float maxValue, menuFunc_t callbackFunc); +static void menu_action_setting_edit_callback_float5(const char* pstr, float* ptr, float minValue, float maxValue, menuFunc_t callbackFunc); +static void menu_action_setting_edit_callback_float51(const char* pstr, float* ptr, float minValue, float maxValue, menuFunc_t callbackFunc); +static void menu_action_setting_edit_callback_float52(const char* pstr, float* ptr, float minValue, float maxValue, menuFunc_t callbackFunc); +static void menu_action_setting_edit_callback_long5(const char* pstr, unsigned long* ptr, unsigned long minValue, unsigned long maxValue, menuFunc_t callbackFunc); + +#define ENCODER_FEEDRATE_DEADZONE 10 + +#if !defined(LCD_I2C_VIKI) + #ifndef ENCODER_STEPS_PER_MENU_ITEM + #define ENCODER_STEPS_PER_MENU_ITEM 5 + #endif + #ifndef ENCODER_PULSES_PER_STEP + #define ENCODER_PULSES_PER_STEP 1 + #endif +#else + #ifndef ENCODER_STEPS_PER_MENU_ITEM + #define ENCODER_STEPS_PER_MENU_ITEM 2 // VIKI LCD rotary encoder uses a different number of steps per rotation + #endif + #ifndef ENCODER_PULSES_PER_STEP + #define ENCODER_PULSES_PER_STEP 1 + #endif +#endif + + +/* Helper macros for menus */ +#define START_MENU() do { \ + if (encoderPosition > 0x8000) encoderPosition = 0; \ + if (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM < currentMenuViewOffset) currentMenuViewOffset = encoderPosition / ENCODER_STEPS_PER_MENU_ITEM;\ + uint8_t _lineNr = currentMenuViewOffset, _menuItemNr; \ + bool wasClicked = LCD_CLICKED;\ + for(uint8_t _drawLineNr = 0; _drawLineNr < LCD_HEIGHT; _drawLineNr++, _lineNr++) { \ + _menuItemNr = 0; +#define MENU_ITEM(type, label, args...) do { \ + if (_menuItemNr == _lineNr) { \ + if (lcdDrawUpdate) { \ + const char* _label_pstr = PSTR(label); \ + if ((encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) == _menuItemNr) { \ + lcd_implementation_drawmenu_ ## type ## _selected (_drawLineNr, _label_pstr , ## args ); \ + }else{\ + lcd_implementation_drawmenu_ ## type (_drawLineNr, _label_pstr , ## args ); \ + }\ + }\ + if (wasClicked && (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) == _menuItemNr) {\ + lcd_quick_feedback(); \ + menu_action_ ## type ( args ); \ + return;\ + }\ + }\ + _menuItemNr++;\ +} while(0) +#define MENU_ITEM_DUMMY() do { _menuItemNr++; } while(0) +#define MENU_ITEM_EDIT(type, label, args...) MENU_ITEM(setting_edit_ ## type, label, PSTR(label) , ## args ) +#define MENU_ITEM_EDIT_CALLBACK(type, label, args...) MENU_ITEM(setting_edit_callback_ ## type, label, PSTR(label) , ## args ) +#define END_MENU() \ + if (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM >= _menuItemNr) encoderPosition = _menuItemNr * ENCODER_STEPS_PER_MENU_ITEM - 1; \ + if ((uint8_t)(encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) >= currentMenuViewOffset + LCD_HEIGHT) { currentMenuViewOffset = (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) - LCD_HEIGHT + 1; lcdDrawUpdate = 1; _lineNr = currentMenuViewOffset - 1; _drawLineNr = -1; } \ + } } while(0) + +/** Used variables to keep track of the menu */ +#ifndef REPRAPWORLD_KEYPAD +volatile uint8_t buttons;//Contains the bits of the currently pressed buttons. +#else +volatile uint8_t buttons_reprapworld_keypad; // to store the reprapworld_keypad shift register values +#endif +#ifdef LCD_HAS_SLOW_BUTTONS +volatile uint8_t slow_buttons;//Contains the bits of the currently pressed buttons. +#endif +uint8_t currentMenuViewOffset; /* scroll offset in the current menu */ +uint32_t blocking_enc; +uint8_t lastEncoderBits; +uint32_t encoderPosition; +#if (SDCARDDETECT > 0) +bool lcd_oldcardstatus; +#endif +#endif//ULTIPANEL + +menuFunc_t currentMenu = lcd_status_screen; /* function pointer to the currently active menu */ +uint32_t lcd_next_update_millis; +uint8_t lcd_status_update_delay; +uint8_t lcdDrawUpdate = 2; /* Set to none-zero when the LCD needs to draw, decreased after every draw. Set to 2 in LCD routines so the LCD gets at least 1 full redraw (first redraw is partial) */ + +//prevMenu and prevEncoderPosition are used to store the previous menu location when editing settings. +menuFunc_t prevMenu = NULL; +uint16_t prevEncoderPosition; +//Variables used when editing values. +const char* editLabel; +void* editValue; +int32_t minEditValue, maxEditValue; +menuFunc_t callbackFunc; + +// place-holders for Ki and Kd edits +float raw_Ki, raw_Kd; + +/* Main status screen. It's up to the implementation specific part to show what is needed. As this is very display dependent */ +static void lcd_status_screen() +{ + if (lcd_status_update_delay) + lcd_status_update_delay--; + else + lcdDrawUpdate = 1; + if (lcdDrawUpdate) + { + lcd_implementation_status_screen(); + lcd_status_update_delay = 10; /* redraw the main screen every second. This is easier then trying keep track of all things that change on the screen */ + } +#ifdef ULTIPANEL + if (LCD_CLICKED) + { + currentMenu = lcd_main_menu; + encoderPosition = 0; + lcd_quick_feedback(); lcd_implementation_init(); // to maybe revive the LCD if static electricity killed it. - } - -#ifdef ULTIPANEL_FEEDMULTIPLY - // Dead zone at 100% feedrate - if ((feedmultiply < 100 && (feedmultiply + int(encoderPosition)) > 100) || - (feedmultiply > 100 && (feedmultiply + int(encoderPosition)) < 100)) - { - encoderPosition = 0; - feedmultiply = 100; - } - - if (feedmultiply == 100 && int(encoderPosition) > ENCODER_FEEDRATE_DEADZONE) - { - feedmultiply += int(encoderPosition) - ENCODER_FEEDRATE_DEADZONE; - encoderPosition = 0; - } - else if (feedmultiply == 100 && int(encoderPosition) < -ENCODER_FEEDRATE_DEADZONE) - { - feedmultiply += int(encoderPosition) + ENCODER_FEEDRATE_DEADZONE; - encoderPosition = 0; - } - else if (feedmultiply != 100) - { - feedmultiply += int(encoderPosition); - encoderPosition = 0; - } -#endif//ULTIPANEL_FEEDMULTIPLY - - if (feedmultiply < 10) - feedmultiply = 10; - if (feedmultiply > 999) - feedmultiply = 999; -#endif//ULTIPANEL -} - -#ifdef ULTIPANEL -static void lcd_return_to_status() -{ - encoderPosition = 0; - currentMenu = lcd_status_screen; -} - -static void lcd_sdcard_pause() -{ - card.pauseSDPrint(); -} -static void lcd_sdcard_resume() -{ - card.startFileprint(); -} - -static void lcd_sdcard_stop() -{ - card.sdprinting = false; - card.closefile(); - quickStop(); - if(SD_FINISHED_STEPPERRELEASE) - { - enquecommand_P(PSTR(SD_FINISHED_RELEASECOMMAND)); - } - autotempShutdown(); -} - -/* Menu implementation */ -static void lcd_main_menu() -{ - START_MENU(); - MENU_ITEM(back, MSG_WATCH, lcd_status_screen); - if (movesplanned() || IS_SD_PRINTING) - { - MENU_ITEM(submenu, MSG_TUNE, lcd_tune_menu); - }else{ - MENU_ITEM(submenu, MSG_PREPARE, lcd_prepare_menu); - } - MENU_ITEM(submenu, MSG_CONTROL, lcd_control_menu); -#ifdef SDSUPPORT - if (card.cardOK) - { - if (card.isFileOpen()) - { - if (card.sdprinting) - MENU_ITEM(function, MSG_PAUSE_PRINT, lcd_sdcard_pause); - else - MENU_ITEM(function, MSG_RESUME_PRINT, lcd_sdcard_resume); - MENU_ITEM(function, MSG_STOP_PRINT, lcd_sdcard_stop); - }else{ - MENU_ITEM(submenu, MSG_CARD_MENU, lcd_sdcard_menu); -#if SDCARDDETECT < 1 - MENU_ITEM(gcode, MSG_CNG_SDCARD, PSTR("M21")); // SD-card changed by user -#endif - } - }else{ - MENU_ITEM(submenu, MSG_NO_CARD, lcd_sdcard_menu); -#if SDCARDDETECT < 1 - MENU_ITEM(gcode, MSG_INIT_SDCARD, PSTR("M21")); // Manually initialize the SD-card via user interface -#endif - } -#endif - END_MENU(); -} - -#ifdef SDSUPPORT -static void lcd_autostart_sd() -{ - card.lastnr=0; - card.setroot(); - card.checkautostart(true); -} -#endif - -#ifdef BABYSTEPPING -static void lcd_babystep_x() -{ - if (encoderPosition != 0) - { - babystepsTodo[X_AXIS]+=(int)encoderPosition; - encoderPosition=0; - lcdDrawUpdate = 1; - } - if (lcdDrawUpdate) - { - lcd_implementation_drawedit(PSTR(MSG_BABYSTEPPING_X),""); - } - if (LCD_CLICKED) - { - lcd_quick_feedback(); - currentMenu = lcd_tune_menu; - encoderPosition = 0; - } -} - -static void lcd_babystep_y() -{ - if (encoderPosition != 0) - { - babystepsTodo[Y_AXIS]+=(int)encoderPosition; - encoderPosition=0; - lcdDrawUpdate = 1; - } - if (lcdDrawUpdate) - { - lcd_implementation_drawedit(PSTR(MSG_BABYSTEPPING_Y),""); - } - if (LCD_CLICKED) - { - lcd_quick_feedback(); - currentMenu = lcd_tune_menu; - encoderPosition = 0; - } -} - -static void lcd_babystep_z() -{ - if (encoderPosition != 0) - { - babystepsTodo[Z_AXIS]+=BABYSTEP_Z_MULTIPLICATOR*(int)encoderPosition; - encoderPosition=0; - lcdDrawUpdate = 1; - } - if (lcdDrawUpdate) - { - lcd_implementation_drawedit(PSTR(MSG_BABYSTEPPING_Z),""); - } - if (LCD_CLICKED) - { - lcd_quick_feedback(); - currentMenu = lcd_tune_menu; - encoderPosition = 0; - } -} -#endif //BABYSTEPPING - -static void lcd_tune_menu() -{ - START_MENU(); - MENU_ITEM(back, MSG_MAIN, lcd_main_menu); - MENU_ITEM_EDIT(int3, MSG_SPEED, &feedmultiply, 10, 999); - MENU_ITEM_EDIT(int3, MSG_NOZZLE, &target_temperature[0], 0, HEATER_0_MAXTEMP - 15); -#if TEMP_SENSOR_1 != 0 - MENU_ITEM_EDIT(int3, MSG_NOZZLE1, &target_temperature[1], 0, HEATER_1_MAXTEMP - 15); -#endif -#if TEMP_SENSOR_2 != 0 - MENU_ITEM_EDIT(int3, MSG_NOZZLE2, &target_temperature[2], 0, HEATER_2_MAXTEMP - 15); -#endif -#if TEMP_SENSOR_BED != 0 - MENU_ITEM_EDIT(int3, MSG_BED, &target_temperature_bed, 0, BED_MAXTEMP - 15); -#endif - MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &fanSpeed, 0, 255); - MENU_ITEM_EDIT(int3, MSG_FLOW, &extrudemultiply, 10, 999); - MENU_ITEM_EDIT(int3, MSG_FLOW0, &extruder_multiply[0], 10, 999); -#if TEMP_SENSOR_1 != 0 - MENU_ITEM_EDIT(int3, MSG_FLOW1, &extruder_multiply[1], 10, 999); -#endif -#if TEMP_SENSOR_2 != 0 - MENU_ITEM_EDIT(int3, MSG_FLOW2, &extruder_multiply[2], 10, 999); -#endif - -#ifdef BABYSTEPPING - #ifdef BABYSTEP_XY - MENU_ITEM(submenu, MSG_BABYSTEP_X, lcd_babystep_x); - MENU_ITEM(submenu, MSG_BABYSTEP_Y, lcd_babystep_y); - #endif //BABYSTEP_XY - MENU_ITEM(submenu, MSG_BABYSTEP_Z, lcd_babystep_z); -#endif -#ifdef FILAMENTCHANGEENABLE - MENU_ITEM(gcode, MSG_FILAMENTCHANGE, PSTR("M600")); -#endif - END_MENU(); -} - -void lcd_preheat_pla0() -{ - setTargetHotend0(plaPreheatHotendTemp); - setTargetBed(plaPreheatHPBTemp); - fanSpeed = plaPreheatFanSpeed; - lcd_return_to_status(); - setWatch(); // heater sanity check timer -} - -void lcd_preheat_abs0() -{ - setTargetHotend0(absPreheatHotendTemp); - setTargetBed(absPreheatHPBTemp); - fanSpeed = absPreheatFanSpeed; - lcd_return_to_status(); - setWatch(); // heater sanity check timer -} - -#if TEMP_SENSOR_1 != 0 //2nd extruder preheat -void lcd_preheat_pla1() -{ - setTargetHotend1(plaPreheatHotendTemp); - setTargetBed(plaPreheatHPBTemp); - fanSpeed = plaPreheatFanSpeed; - lcd_return_to_status(); - setWatch(); // heater sanity check timer -} - -void lcd_preheat_abs1() -{ - setTargetHotend1(absPreheatHotendTemp); - setTargetBed(absPreheatHPBTemp); - fanSpeed = absPreheatFanSpeed; - lcd_return_to_status(); - setWatch(); // heater sanity check timer -} -#endif //2nd extruder preheat - -#if TEMP_SENSOR_2 != 0 //3 extruder preheat -void lcd_preheat_pla2() -{ - setTargetHotend2(plaPreheatHotendTemp); - setTargetBed(plaPreheatHPBTemp); - fanSpeed = plaPreheatFanSpeed; - lcd_return_to_status(); - setWatch(); // heater sanity check timer -} - -void lcd_preheat_abs2() -{ - setTargetHotend2(absPreheatHotendTemp); - setTargetBed(absPreheatHPBTemp); - fanSpeed = absPreheatFanSpeed; - lcd_return_to_status(); - setWatch(); // heater sanity check timer -} -#endif //3 extruder preheat - -#if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 //more than one extruder present -void lcd_preheat_pla012() -{ - setTargetHotend0(plaPreheatHotendTemp); - setTargetHotend1(plaPreheatHotendTemp); - setTargetHotend2(plaPreheatHotendTemp); - setTargetBed(plaPreheatHPBTemp); - fanSpeed = plaPreheatFanSpeed; - lcd_return_to_status(); - setWatch(); // heater sanity check timer -} - -void lcd_preheat_abs012() -{ - setTargetHotend0(absPreheatHotendTemp); - setTargetHotend1(absPreheatHotendTemp); - setTargetHotend2(absPreheatHotendTemp); - setTargetBed(absPreheatHPBTemp); - fanSpeed = absPreheatFanSpeed; - lcd_return_to_status(); - setWatch(); // heater sanity check timer -} -#endif //more than one extruder present - -void lcd_preheat_pla_bedonly() -{ - setTargetBed(plaPreheatHPBTemp); - fanSpeed = plaPreheatFanSpeed; - lcd_return_to_status(); - setWatch(); // heater sanity check timer -} - -void lcd_preheat_abs_bedonly() -{ - setTargetBed(absPreheatHPBTemp); - fanSpeed = absPreheatFanSpeed; - lcd_return_to_status(); - setWatch(); // heater sanity check timer -} - -static void lcd_preheat_pla_menu() -{ - START_MENU(); - MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu); - MENU_ITEM(function, MSG_PREHEAT_PLA0, lcd_preheat_pla0); -#if TEMP_SENSOR_1 != 0 //2 extruder preheat - MENU_ITEM(function, MSG_PREHEAT_PLA1, lcd_preheat_pla1); -#endif //2 extruder preheat -#if TEMP_SENSOR_2 != 0 //3 extruder preheat - MENU_ITEM(function, MSG_PREHEAT_PLA2, lcd_preheat_pla2); -#endif //3 extruder preheat -#if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 //all extruder preheat - MENU_ITEM(function, MSG_PREHEAT_PLA012, lcd_preheat_pla012); -#endif //2 extruder preheat -#if TEMP_SENSOR_BED != 0 - MENU_ITEM(function, MSG_PREHEAT_PLA_BEDONLY, lcd_preheat_pla_bedonly); -#endif - END_MENU(); -} - -static void lcd_preheat_abs_menu() -{ - START_MENU(); - MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu); - MENU_ITEM(function, MSG_PREHEAT_ABS0, lcd_preheat_abs0); -#if TEMP_SENSOR_1 != 0 //2 extruder preheat - MENU_ITEM(function, MSG_PREHEAT_ABS1, lcd_preheat_abs1); -#endif //2 extruder preheat -#if TEMP_SENSOR_2 != 0 //3 extruder preheat - MENU_ITEM(function, MSG_PREHEAT_ABS2, lcd_preheat_abs2); -#endif //3 extruder preheat -#if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 //all extruder preheat - MENU_ITEM(function, MSG_PREHEAT_ABS012, lcd_preheat_abs012); -#endif //2 extruder preheat -#if TEMP_SENSOR_BED != 0 - MENU_ITEM(function, MSG_PREHEAT_ABS_BEDONLY, lcd_preheat_abs_bedonly); -#endif - END_MENU(); -} - -void lcd_cooldown() -{ - setTargetHotend0(0); - setTargetHotend1(0); - setTargetHotend2(0); - setTargetBed(0); - fanSpeed = 0; - lcd_return_to_status(); -} - -static void lcd_prepare_menu() -{ - START_MENU(); - MENU_ITEM(back, MSG_MAIN, lcd_main_menu); -#ifdef SDSUPPORT - #ifdef MENU_ADDAUTOSTART - MENU_ITEM(function, MSG_AUTOSTART, lcd_autostart_sd); - #endif -#endif - MENU_ITEM(gcode, MSG_DISABLE_STEPPERS, PSTR("M84")); - MENU_ITEM(gcode, MSG_AUTO_HOME, PSTR("G28")); - //MENU_ITEM(gcode, MSG_SET_ORIGIN, PSTR("G92 X0 Y0 Z0")); -#if TEMP_SENSOR_0 != 0 - #if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 || TEMP_SENSOR_BED != 0 - MENU_ITEM(submenu, MSG_PREHEAT_PLA, lcd_preheat_pla_menu); - MENU_ITEM(submenu, MSG_PREHEAT_ABS, lcd_preheat_abs_menu); - #else - MENU_ITEM(function, MSG_PREHEAT_PLA, lcd_preheat_pla0); - MENU_ITEM(function, MSG_PREHEAT_ABS, lcd_preheat_abs0); - #endif -#endif - MENU_ITEM(function, MSG_COOLDOWN, lcd_cooldown); -#if PS_ON_PIN > -1 - if (powersupply) - { - MENU_ITEM(gcode, MSG_SWITCH_PS_OFF, PSTR("M81")); - }else{ - MENU_ITEM(gcode, MSG_SWITCH_PS_ON, PSTR("M80")); - } -#endif - MENU_ITEM(submenu, MSG_MOVE_AXIS, lcd_move_menu); - END_MENU(); -} - -float move_menu_scale; -static void lcd_move_menu_axis(); - -static void lcd_move_x() -{ - if (encoderPosition != 0) - { - refresh_cmd_timeout(); - current_position[X_AXIS] += float((int)encoderPosition) * move_menu_scale; - if (min_software_endstops && current_position[X_AXIS] < X_MIN_POS) - current_position[X_AXIS] = X_MIN_POS; - if (max_software_endstops && current_position[X_AXIS] > X_MAX_POS) - current_position[X_AXIS] = X_MAX_POS; - encoderPosition = 0; - #ifdef DELTA - calculate_delta(current_position); - plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[X_AXIS]/60, active_extruder); - #else - plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[X_AXIS]/60, active_extruder); - #endif - lcdDrawUpdate = 1; - } - if (lcdDrawUpdate) - { - lcd_implementation_drawedit(PSTR("X"), ftostr31(current_position[X_AXIS])); - } - if (LCD_CLICKED) - { - lcd_quick_feedback(); - currentMenu = lcd_move_menu_axis; - encoderPosition = 0; - } -} -static void lcd_move_y() -{ - if (encoderPosition != 0) - { - refresh_cmd_timeout(); - current_position[Y_AXIS] += float((int)encoderPosition) * move_menu_scale; - if (min_software_endstops && current_position[Y_AXIS] < Y_MIN_POS) - current_position[Y_AXIS] = Y_MIN_POS; - if (max_software_endstops && current_position[Y_AXIS] > Y_MAX_POS) - current_position[Y_AXIS] = Y_MAX_POS; - encoderPosition = 0; - #ifdef DELTA - calculate_delta(current_position); - plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[Y_AXIS]/60, active_extruder); - #else - plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[Y_AXIS]/60, active_extruder); - #endif - lcdDrawUpdate = 1; - } - if (lcdDrawUpdate) - { - lcd_implementation_drawedit(PSTR("Y"), ftostr31(current_position[Y_AXIS])); - } - if (LCD_CLICKED) - { - lcd_quick_feedback(); - currentMenu = lcd_move_menu_axis; - encoderPosition = 0; - } -} -static void lcd_move_z() -{ - if (encoderPosition != 0) - { - refresh_cmd_timeout(); - current_position[Z_AXIS] += float((int)encoderPosition) * move_menu_scale; - if (min_software_endstops && current_position[Z_AXIS] < Z_MIN_POS) - current_position[Z_AXIS] = Z_MIN_POS; - if (max_software_endstops && current_position[Z_AXIS] > Z_MAX_POS) - current_position[Z_AXIS] = Z_MAX_POS; - encoderPosition = 0; - #ifdef DELTA - calculate_delta(current_position); - plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[Z_AXIS]/60, active_extruder); - #else - plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[Z_AXIS]/60, active_extruder); - #endif - lcdDrawUpdate = 1; - } - if (lcdDrawUpdate) - { - lcd_implementation_drawedit(PSTR("Z"), ftostr31(current_position[Z_AXIS])); - } - if (LCD_CLICKED) - { - lcd_quick_feedback(); - currentMenu = lcd_move_menu_axis; - encoderPosition = 0; - } -} -static void lcd_move_e() -{ - if (encoderPosition != 0) - { - current_position[E_AXIS] += float((int)encoderPosition) * move_menu_scale; - encoderPosition = 0; - #ifdef DELTA - calculate_delta(current_position); - plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder); - #else - plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder); - #endif - lcdDrawUpdate = 1; - } - if (lcdDrawUpdate) - { - lcd_implementation_drawedit(PSTR("Extruder"), ftostr31(current_position[E_AXIS])); - } - if (LCD_CLICKED) - { - lcd_quick_feedback(); - currentMenu = lcd_move_menu_axis; - encoderPosition = 0; - } -} - -static void lcd_move_menu_axis() -{ - START_MENU(); - MENU_ITEM(back, MSG_MOVE_AXIS, lcd_move_menu); - MENU_ITEM(submenu, MSG_MOVE_X, lcd_move_x); - MENU_ITEM(submenu, MSG_MOVE_Y, lcd_move_y); - if (move_menu_scale < 10.0) - { - MENU_ITEM(submenu, MSG_MOVE_Z, lcd_move_z); - MENU_ITEM(submenu, MSG_MOVE_E, lcd_move_e); - } - END_MENU(); -} - -static void lcd_move_menu_10mm() -{ - move_menu_scale = 10.0; - lcd_move_menu_axis(); -} -static void lcd_move_menu_1mm() -{ - move_menu_scale = 1.0; - lcd_move_menu_axis(); -} -static void lcd_move_menu_01mm() -{ - move_menu_scale = 0.1; - lcd_move_menu_axis(); -} - -static void lcd_move_menu() -{ - START_MENU(); - MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu); - MENU_ITEM(submenu, MSG_MOVE_10MM, lcd_move_menu_10mm); - MENU_ITEM(submenu, MSG_MOVE_1MM, lcd_move_menu_1mm); - MENU_ITEM(submenu, MSG_MOVE_01MM, lcd_move_menu_01mm); - //TODO:X,Y,Z,E - END_MENU(); -} - -static void lcd_control_menu() -{ - START_MENU(); - MENU_ITEM(back, MSG_MAIN, lcd_main_menu); - MENU_ITEM(submenu, MSG_TEMPERATURE, lcd_control_temperature_menu); - MENU_ITEM(submenu, MSG_MOTION, lcd_control_motion_menu); -#ifdef DOGLCD -// MENU_ITEM_EDIT(int3, MSG_CONTRAST, &lcd_contrast, 0, 63); - MENU_ITEM(submenu, MSG_CONTRAST, lcd_set_contrast); -#endif -#ifdef FWRETRACT - MENU_ITEM(submenu, MSG_RETRACT, lcd_control_retract_menu); -#endif -#ifdef EEPROM_SETTINGS - MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings); - MENU_ITEM(function, MSG_LOAD_EPROM, Config_RetrieveSettings); -#endif - MENU_ITEM(function, MSG_RESTORE_FAILSAFE, Config_ResetDefault); - END_MENU(); -} - -static void lcd_control_temperature_menu() -{ -#ifdef PIDTEMP - // set up temp variables - undo the default scaling - raw_Ki = unscalePID_i(Ki); - raw_Kd = unscalePID_d(Kd); -#endif - - START_MENU(); - MENU_ITEM(back, MSG_CONTROL, lcd_control_menu); - MENU_ITEM_EDIT(int3, MSG_NOZZLE, &target_temperature[0], 0, HEATER_0_MAXTEMP - 15); -#if TEMP_SENSOR_1 != 0 - MENU_ITEM_EDIT(int3, MSG_NOZZLE1, &target_temperature[1], 0, HEATER_1_MAXTEMP - 15); -#endif -#if TEMP_SENSOR_2 != 0 - MENU_ITEM_EDIT(int3, MSG_NOZZLE2, &target_temperature[2], 0, HEATER_2_MAXTEMP - 15); -#endif -#if TEMP_SENSOR_BED != 0 - MENU_ITEM_EDIT(int3, MSG_BED, &target_temperature_bed, 0, BED_MAXTEMP - 15); -#endif - MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &fanSpeed, 0, 255); -#ifdef AUTOTEMP - MENU_ITEM_EDIT(bool, MSG_AUTOTEMP, &autotemp_enabled); - MENU_ITEM_EDIT(float3, MSG_MIN, &autotemp_min, 0, HEATER_0_MAXTEMP - 15); - MENU_ITEM_EDIT(float3, MSG_MAX, &autotemp_max, 0, HEATER_0_MAXTEMP - 15); - MENU_ITEM_EDIT(float32, MSG_FACTOR, &autotemp_factor, 0.0, 1.0); -#endif -#ifdef PIDTEMP - MENU_ITEM_EDIT(float52, MSG_PID_P, &Kp, 1, 9990); - // i is typically a small value so allows values below 1 - MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_I, &raw_Ki, 0.01, 9990, copy_and_scalePID_i); - MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_D, &raw_Kd, 1, 9990, copy_and_scalePID_d); -# ifdef PID_ADD_EXTRUSION_RATE - MENU_ITEM_EDIT(float3, MSG_PID_C, &Kc, 1, 9990); -# endif//PID_ADD_EXTRUSION_RATE -#endif//PIDTEMP - MENU_ITEM(submenu, MSG_PREHEAT_PLA_SETTINGS, lcd_control_temperature_preheat_pla_settings_menu); - MENU_ITEM(submenu, MSG_PREHEAT_ABS_SETTINGS, lcd_control_temperature_preheat_abs_settings_menu); - END_MENU(); -} - -static void lcd_control_temperature_preheat_pla_settings_menu() -{ - START_MENU(); - MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu); - MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &plaPreheatFanSpeed, 0, 255); - MENU_ITEM_EDIT(int3, MSG_NOZZLE, &plaPreheatHotendTemp, 0, HEATER_0_MAXTEMP - 15); -#if TEMP_SENSOR_BED != 0 - MENU_ITEM_EDIT(int3, MSG_BED, &plaPreheatHPBTemp, 0, BED_MAXTEMP - 15); -#endif -#ifdef EEPROM_SETTINGS - MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings); -#endif - END_MENU(); -} - -static void lcd_control_temperature_preheat_abs_settings_menu() -{ - START_MENU(); - MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu); - MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &absPreheatFanSpeed, 0, 255); - MENU_ITEM_EDIT(int3, MSG_NOZZLE, &absPreheatHotendTemp, 0, HEATER_0_MAXTEMP - 15); -#if TEMP_SENSOR_BED != 0 - MENU_ITEM_EDIT(int3, MSG_BED, &absPreheatHPBTemp, 0, BED_MAXTEMP - 15); -#endif -#ifdef EEPROM_SETTINGS - MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings); -#endif - END_MENU(); -} - -static void lcd_control_motion_menu() -{ - START_MENU(); - MENU_ITEM(back, MSG_CONTROL, lcd_control_menu); -#ifdef ENABLE_AUTO_BED_LEVELING - MENU_ITEM_EDIT(float32, MSG_ZPROBE_ZOFFSET, &zprobe_zoffset, 0.5, 50); -#endif - MENU_ITEM_EDIT(float5, MSG_ACC, &acceleration, 500, 99000); - MENU_ITEM_EDIT(float3, MSG_VXY_JERK, &max_xy_jerk, 1, 990); - MENU_ITEM_EDIT(float52, MSG_VZ_JERK, &max_z_jerk, 0.1, 990); - MENU_ITEM_EDIT(float3, MSG_VE_JERK, &max_e_jerk, 1, 990); - MENU_ITEM_EDIT(float3, MSG_VMAX MSG_X, &max_feedrate[X_AXIS], 1, 999); - MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Y, &max_feedrate[Y_AXIS], 1, 999); - MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Z, &max_feedrate[Z_AXIS], 1, 999); - MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E, &max_feedrate[E_AXIS], 1, 999); - MENU_ITEM_EDIT(float3, MSG_VMIN, &minimumfeedrate, 0, 999); - MENU_ITEM_EDIT(float3, MSG_VTRAV_MIN, &mintravelfeedrate, 0, 999); - MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_X, &max_acceleration_units_per_sq_second[X_AXIS], 100, 99000, reset_acceleration_rates); - MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Y, &max_acceleration_units_per_sq_second[Y_AXIS], 100, 99000, reset_acceleration_rates); - MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Z, &max_acceleration_units_per_sq_second[Z_AXIS], 100, 99000, reset_acceleration_rates); - MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_E, &max_acceleration_units_per_sq_second[E_AXIS], 100, 99000, reset_acceleration_rates); - MENU_ITEM_EDIT(float5, MSG_A_RETRACT, &retract_acceleration, 100, 99000); - MENU_ITEM_EDIT(float52, MSG_XSTEPS, &axis_steps_per_unit[X_AXIS], 5, 9999); - MENU_ITEM_EDIT(float52, MSG_YSTEPS, &axis_steps_per_unit[Y_AXIS], 5, 9999); - MENU_ITEM_EDIT(float51, MSG_ZSTEPS, &axis_steps_per_unit[Z_AXIS], 5, 9999); - MENU_ITEM_EDIT(float51, MSG_ESTEPS, &axis_steps_per_unit[E_AXIS], 5, 9999); -#ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED - MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &abort_on_endstop_hit); -#endif - END_MENU(); -} - -#ifdef DOGLCD -static void lcd_set_contrast() -{ - if (encoderPosition != 0) - { - lcd_contrast -= encoderPosition; - if (lcd_contrast < 0) lcd_contrast = 0; - else if (lcd_contrast > 63) lcd_contrast = 63; - encoderPosition = 0; - lcdDrawUpdate = 1; - u8g.setContrast(lcd_contrast); - } - if (lcdDrawUpdate) - { - lcd_implementation_drawedit(PSTR(MSG_CONTRAST), itostr2(lcd_contrast)); - } - if (LCD_CLICKED) - { - lcd_quick_feedback(); - currentMenu = lcd_control_menu; - encoderPosition = 0; - } -} -#endif - -#ifdef FWRETRACT -static void lcd_control_retract_menu() -{ - START_MENU(); - MENU_ITEM(back, MSG_CONTROL, lcd_control_menu); - MENU_ITEM_EDIT(bool, MSG_AUTORETRACT, &autoretract_enabled); - MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT, &retract_length, 0, 100); - MENU_ITEM_EDIT(float3, MSG_CONTROL_RETRACTF, &retract_feedrate, 1, 999); - MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_ZLIFT, &retract_zlift, 0, 999); - MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_RECOVER, &retract_recover_length, 0, 100); - MENU_ITEM_EDIT(float3, MSG_CONTROL_RETRACT_RECOVERF, &retract_recover_feedrate, 1, 999); - END_MENU(); -} -#endif - -#if SDCARDDETECT == -1 -static void lcd_sd_refresh() -{ - card.initsd(); - currentMenuViewOffset = 0; -} -#endif -static void lcd_sd_updir() -{ - card.updir(); - currentMenuViewOffset = 0; -} - -void lcd_sdcard_menu() -{ - if (lcdDrawUpdate == 0 && LCD_CLICKED == 0) - return; // nothing to do (so don't thrash the SD card) - uint16_t fileCnt = card.getnrfilenames(); - START_MENU(); - MENU_ITEM(back, MSG_MAIN, lcd_main_menu); - card.getWorkDirName(); - if(card.filename[0]=='/') - { -#if SDCARDDETECT == -1 - MENU_ITEM(function, LCD_STR_REFRESH MSG_REFRESH, lcd_sd_refresh); -#endif - }else{ - MENU_ITEM(function, LCD_STR_FOLDER "..", lcd_sd_updir); - } - - for(uint16_t i=0;i maxEditValue) \ - encoderPosition = maxEditValue; \ - if (lcdDrawUpdate) \ - lcd_implementation_drawedit(editLabel, _strFunc(((_type)encoderPosition) / scale)); \ - if (LCD_CLICKED) \ - { \ - *((_type*)editValue) = ((_type)encoderPosition) / scale; \ - lcd_quick_feedback(); \ - currentMenu = prevMenu; \ - encoderPosition = prevEncoderPosition; \ - } \ - } \ - void menu_edit_callback_ ## _name () \ - { \ - if ((int32_t)encoderPosition < minEditValue) \ - encoderPosition = minEditValue; \ - if ((int32_t)encoderPosition > maxEditValue) \ - encoderPosition = maxEditValue; \ - if (lcdDrawUpdate) \ - lcd_implementation_drawedit(editLabel, _strFunc(((_type)encoderPosition) / scale)); \ - if (LCD_CLICKED) \ - { \ - *((_type*)editValue) = ((_type)encoderPosition) / scale; \ - lcd_quick_feedback(); \ - currentMenu = prevMenu; \ - encoderPosition = prevEncoderPosition; \ - (*callbackFunc)();\ - } \ - } \ - static void menu_action_setting_edit_ ## _name (const char* pstr, _type* ptr, _type minValue, _type maxValue) \ - { \ - prevMenu = currentMenu; \ - prevEncoderPosition = encoderPosition; \ - \ - lcdDrawUpdate = 2; \ - currentMenu = menu_edit_ ## _name; \ - \ - editLabel = pstr; \ - editValue = ptr; \ - minEditValue = minValue * scale; \ - maxEditValue = maxValue * scale; \ - encoderPosition = (*ptr) * scale; \ - }\ - static void menu_action_setting_edit_callback_ ## _name (const char* pstr, _type* ptr, _type minValue, _type maxValue, menuFunc_t callback) \ - { \ - prevMenu = currentMenu; \ - prevEncoderPosition = encoderPosition; \ - \ - lcdDrawUpdate = 2; \ - currentMenu = menu_edit_callback_ ## _name; \ - \ - editLabel = pstr; \ - editValue = ptr; \ - minEditValue = minValue * scale; \ - maxEditValue = maxValue * scale; \ - encoderPosition = (*ptr) * scale; \ - callbackFunc = callback;\ - } -menu_edit_type(int, int3, itostr3, 1) -menu_edit_type(float, float3, ftostr3, 1) -menu_edit_type(float, float32, ftostr32, 100) -menu_edit_type(float, float5, ftostr5, 0.01) -menu_edit_type(float, float51, ftostr51, 10) -menu_edit_type(float, float52, ftostr52, 100) -menu_edit_type(unsigned long, long5, ftostr5, 0.01) - -#ifdef REPRAPWORLD_KEYPAD - static void reprapworld_keypad_move_z_up() { - encoderPosition = 1; - move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; - lcd_move_z(); - } - static void reprapworld_keypad_move_z_down() { - encoderPosition = -1; - move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; - lcd_move_z(); - } - static void reprapworld_keypad_move_x_left() { - encoderPosition = -1; - move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; - lcd_move_x(); - } - static void reprapworld_keypad_move_x_right() { - encoderPosition = 1; - move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; - lcd_move_x(); - } - static void reprapworld_keypad_move_y_down() { - encoderPosition = 1; - move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; - lcd_move_y(); - } - static void reprapworld_keypad_move_y_up() { - encoderPosition = -1; - move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; - lcd_move_y(); - } - static void reprapworld_keypad_move_home() { - enquecommand_P((PSTR("G28"))); // move all axis home - } -#endif - -/** End of menus **/ - -static void lcd_quick_feedback() -{ - lcdDrawUpdate = 2; - blocking_enc = millis() + 500; - lcd_implementation_quick_feedback(); -} - -/** Menu action functions **/ -static void menu_action_back(menuFunc_t data) -{ - currentMenu = data; - encoderPosition = 0; -} -static void menu_action_submenu(menuFunc_t data) -{ - currentMenu = data; - encoderPosition = 0; -} -static void menu_action_gcode(const char* pgcode) -{ - enquecommand_P(pgcode); -} -static void menu_action_function(menuFunc_t data) -{ - (*data)(); -} -static void menu_action_sdfile(const char* filename, char* longFilename) -{ - char cmd[30]; - char* c; - sprintf_P(cmd, PSTR("M23 %s"), filename); - for(c = &cmd[4]; *c; c++) - *c = tolower(*c); - enquecommand(cmd); - enquecommand_P(PSTR("M24")); - lcd_return_to_status(); -} -static void menu_action_sddirectory(const char* filename, char* longFilename) -{ - card.chdir(filename); - encoderPosition = 0; -} -static void menu_action_setting_edit_bool(const char* pstr, bool* ptr) -{ - *ptr = !(*ptr); -} -#endif//ULTIPANEL - -/** LCD API **/ -void lcd_init() -{ - lcd_implementation_init(); - -#ifdef NEWPANEL - pinMode(BTN_EN1,INPUT); - pinMode(BTN_EN2,INPUT); - WRITE(BTN_EN1,HIGH); - WRITE(BTN_EN2,HIGH); - #if BTN_ENC > 0 - pinMode(BTN_ENC,INPUT); - WRITE(BTN_ENC,HIGH); - #endif - #ifdef REPRAPWORLD_KEYPAD - pinMode(SHIFT_CLK,OUTPUT); - pinMode(SHIFT_LD,OUTPUT); - pinMode(SHIFT_OUT,INPUT); - WRITE(SHIFT_OUT,HIGH); - WRITE(SHIFT_LD,HIGH); - #endif -#else // Not NEWPANEL - #ifdef SR_LCD_2W_NL // Non latching 2 wire shift register - pinMode (SR_DATA_PIN, OUTPUT); - pinMode (SR_CLK_PIN, OUTPUT); - #elif defined(SHIFT_CLK) - pinMode(SHIFT_CLK,OUTPUT); - pinMode(SHIFT_LD,OUTPUT); - pinMode(SHIFT_EN,OUTPUT); - pinMode(SHIFT_OUT,INPUT); - WRITE(SHIFT_OUT,HIGH); - WRITE(SHIFT_LD,HIGH); - WRITE(SHIFT_EN,LOW); - #else - #ifdef ULTIPANEL - #error ULTIPANEL requires an encoder - #endif - #endif // SR_LCD_2W_NL -#endif//!NEWPANEL - -#if defined (SDSUPPORT) && defined(SDCARDDETECT) && (SDCARDDETECT > 0) - pinMode(SDCARDDETECT,INPUT); - WRITE(SDCARDDETECT, HIGH); - lcd_oldcardstatus = IS_SD_INSERTED; -#endif//(SDCARDDETECT > 0) -#ifdef LCD_HAS_SLOW_BUTTONS - slow_buttons = 0; -#endif - lcd_buttons_update(); -#ifdef ULTIPANEL - encoderDiff = 0; -#endif -} - -void lcd_update() -{ - static unsigned long timeoutToStatus = 0; - - #ifdef LCD_HAS_SLOW_BUTTONS - slow_buttons = lcd_implementation_read_slow_buttons(); // buttons which take too long to read in interrupt context - #endif - - lcd_buttons_update(); - - #if (SDCARDDETECT > 0) - if((IS_SD_INSERTED != lcd_oldcardstatus)) - { - lcdDrawUpdate = 2; - lcd_oldcardstatus = IS_SD_INSERTED; - lcd_implementation_init(); // to maybe revive the LCD if static electricity killed it. - - if(lcd_oldcardstatus) - { - card.initsd(); - LCD_MESSAGEPGM(MSG_SD_INSERTED); - } - else - { - card.release(); - LCD_MESSAGEPGM(MSG_SD_REMOVED); - } - } - #endif//CARDINSERTED - - if (lcd_next_update_millis < millis()) - { -#ifdef ULTIPANEL - #ifdef REPRAPWORLD_KEYPAD - if (REPRAPWORLD_KEYPAD_MOVE_Z_UP) { - reprapworld_keypad_move_z_up(); - } - if (REPRAPWORLD_KEYPAD_MOVE_Z_DOWN) { - reprapworld_keypad_move_z_down(); - } - if (REPRAPWORLD_KEYPAD_MOVE_X_LEFT) { - reprapworld_keypad_move_x_left(); - } - if (REPRAPWORLD_KEYPAD_MOVE_X_RIGHT) { - reprapworld_keypad_move_x_right(); - } - if (REPRAPWORLD_KEYPAD_MOVE_Y_DOWN) { - reprapworld_keypad_move_y_down(); - } - if (REPRAPWORLD_KEYPAD_MOVE_Y_UP) { - reprapworld_keypad_move_y_up(); - } - if (REPRAPWORLD_KEYPAD_MOVE_HOME) { - reprapworld_keypad_move_home(); - } - #endif - if (abs(encoderDiff) >= ENCODER_PULSES_PER_STEP) - { - lcdDrawUpdate = 1; - encoderPosition += encoderDiff / ENCODER_PULSES_PER_STEP; - encoderDiff = 0; - timeoutToStatus = millis() + LCD_TIMEOUT_TO_STATUS; - } - if (LCD_CLICKED) - timeoutToStatus = millis() + LCD_TIMEOUT_TO_STATUS; -#endif//ULTIPANEL - -#ifdef DOGLCD // Changes due to different driver architecture of the DOGM display - blink++; // Variable for fan animation and alive dot - u8g.firstPage(); - do - { - u8g.setFont(u8g_font_6x10_marlin); - u8g.setPrintPos(125,0); - if (blink % 2) u8g.setColorIndex(1); else u8g.setColorIndex(0); // Set color for the alive dot - u8g.drawPixel(127,63); // draw alive dot - u8g.setColorIndex(1); // black on white - (*currentMenu)(); - if (!lcdDrawUpdate) break; // Terminate display update, when nothing new to draw. This must be done before the last dogm.next() - } while( u8g.nextPage() ); -#else - (*currentMenu)(); -#endif - -#ifdef LCD_HAS_STATUS_INDICATORS - lcd_implementation_update_indicators(); -#endif - -#ifdef ULTIPANEL - if(timeoutToStatus < millis() && currentMenu != lcd_status_screen) - { - lcd_return_to_status(); - lcdDrawUpdate = 2; - } -#endif//ULTIPANEL - if (lcdDrawUpdate == 2) - lcd_implementation_clear(); - if (lcdDrawUpdate) - lcdDrawUpdate--; - lcd_next_update_millis = millis() + 100; - } -} - -void lcd_setstatus(const char* message) -{ - if (lcd_status_message_level > 0) - return; - strncpy(lcd_status_message, message, LCD_WIDTH); - lcdDrawUpdate = 2; -} -void lcd_setstatuspgm(const char* message) -{ - if (lcd_status_message_level > 0) - return; - strncpy_P(lcd_status_message, message, LCD_WIDTH); - lcdDrawUpdate = 2; -} -void lcd_setalertstatuspgm(const char* message) -{ - lcd_setstatuspgm(message); - lcd_status_message_level = 1; -#ifdef ULTIPANEL - lcd_return_to_status(); -#endif//ULTIPANEL -} -void lcd_reset_alert_level() -{ - lcd_status_message_level = 0; -} - -#ifdef DOGLCD -void lcd_setcontrast(uint8_t value) -{ - lcd_contrast = value & 63; - u8g.setContrast(lcd_contrast); -} -#endif - -#ifdef ULTIPANEL -/* Warning: This function is called from interrupt context */ -void lcd_buttons_update() -{ -#ifdef NEWPANEL - uint8_t newbutton=0; - if(READ(BTN_EN1)==0) newbutton|=EN_A; - if(READ(BTN_EN2)==0) newbutton|=EN_B; - #if BTN_ENC > 0 - if((blocking_enc>1; - if(READ(SHIFT_OUT)) - newbutton_reprapworld_keypad|=(1<<7); - WRITE(SHIFT_CLK,HIGH); - WRITE(SHIFT_CLK,LOW); - } - buttons_reprapworld_keypad=~newbutton_reprapworld_keypad; //invert it, because a pressed switch produces a logical 0 - #endif -#else //read it from the shift register - uint8_t newbutton=0; - WRITE(SHIFT_LD,LOW); - WRITE(SHIFT_LD,HIGH); - unsigned char tmp_buttons=0; - for(int8_t i=0;i<8;i++) - { - newbutton = newbutton>>1; - if(READ(SHIFT_OUT)) - newbutton|=(1<<7); - WRITE(SHIFT_CLK,HIGH); - WRITE(SHIFT_CLK,LOW); - } - buttons=~newbutton; //invert it, because a pressed switch produces a logical 0 -#endif//!NEWPANEL - - //manage encoder rotation - uint8_t enc=0; - if(buttons&EN_A) - enc|=(1<<0); - if(buttons&EN_B) - enc|=(1<<1); - if(enc != lastEncoderBits) - { - switch(enc) - { - case encrot0: - if(lastEncoderBits==encrot3) - encoderDiff++; - else if(lastEncoderBits==encrot1) - encoderDiff--; - break; - case encrot1: - if(lastEncoderBits==encrot0) - encoderDiff++; - else if(lastEncoderBits==encrot2) - encoderDiff--; - break; - case encrot2: - if(lastEncoderBits==encrot1) - encoderDiff++; - else if(lastEncoderBits==encrot3) - encoderDiff--; - break; - case encrot3: - if(lastEncoderBits==encrot2) - encoderDiff++; - else if(lastEncoderBits==encrot0) - encoderDiff--; - break; - } - } - lastEncoderBits = enc; -} - -void lcd_buzz(long duration, uint16_t freq) -{ -#ifdef LCD_USE_I2C_BUZZER - lcd.buzz(duration,freq); -#endif -} - -bool lcd_clicked() -{ - return LCD_CLICKED; -} -#endif//ULTIPANEL - -/********************************/ -/** Float conversion utilities **/ -/********************************/ -// convert float to string with +123.4 format -char conv[8]; -char *ftostr3(const float &x) -{ - return itostr3((int)x); -} - -char *itostr2(const uint8_t &x) -{ - //sprintf(conv,"%5.1f",x); - int xx=x; - conv[0]=(xx/10)%10+'0'; - conv[1]=(xx)%10+'0'; - conv[2]=0; - return conv; -} - -// convert float to string with +123.4 format -char *ftostr31(const float &x) -{ - int xx=x*10; - conv[0]=(xx>=0)?'+':'-'; - xx=abs(xx); - conv[1]=(xx/1000)%10+'0'; - conv[2]=(xx/100)%10+'0'; - conv[3]=(xx/10)%10+'0'; - conv[4]='.'; - conv[5]=(xx)%10+'0'; - conv[6]=0; - return conv; -} - -// convert float to string with 123.4 format -char *ftostr31ns(const float &x) -{ - int xx=x*10; - //conv[0]=(xx>=0)?'+':'-'; - xx=abs(xx); - conv[0]=(xx/1000)%10+'0'; - conv[1]=(xx/100)%10+'0'; - conv[2]=(xx/10)%10+'0'; - conv[3]='.'; - conv[4]=(xx)%10+'0'; - conv[5]=0; - return conv; -} - -char *ftostr32(const float &x) -{ - long xx=x*100; - if (xx >= 0) - conv[0]=(xx/10000)%10+'0'; - else - conv[0]='-'; - xx=abs(xx); - conv[1]=(xx/1000)%10+'0'; - conv[2]=(xx/100)%10+'0'; - conv[3]='.'; - conv[4]=(xx/10)%10+'0'; - conv[5]=(xx)%10+'0'; - conv[6]=0; - return conv; -} - -char *itostr31(const int &xx) -{ - conv[0]=(xx>=0)?'+':'-'; - conv[1]=(xx/1000)%10+'0'; - conv[2]=(xx/100)%10+'0'; - conv[3]=(xx/10)%10+'0'; - conv[4]='.'; - conv[5]=(xx)%10+'0'; - conv[6]=0; - return conv; -} - -char *itostr3(const int &xx) -{ - if (xx >= 100) - conv[0]=(xx/100)%10+'0'; - else - conv[0]=' '; - if (xx >= 10) - conv[1]=(xx/10)%10+'0'; - else - conv[1]=' '; - conv[2]=(xx)%10+'0'; - conv[3]=0; - return conv; -} - -char *itostr3left(const int &xx) -{ - if (xx >= 100) - { - conv[0]=(xx/100)%10+'0'; - conv[1]=(xx/10)%10+'0'; - conv[2]=(xx)%10+'0'; - conv[3]=0; - } - else if (xx >= 10) - { - conv[0]=(xx/10)%10+'0'; - conv[1]=(xx)%10+'0'; - conv[2]=0; - } - else - { - conv[0]=(xx)%10+'0'; - conv[1]=0; - } - return conv; -} - -char *itostr4(const int &xx) -{ - if (xx >= 1000) - conv[0]=(xx/1000)%10+'0'; - else - conv[0]=' '; - if (xx >= 100) - conv[1]=(xx/100)%10+'0'; - else - conv[1]=' '; - if (xx >= 10) - conv[2]=(xx/10)%10+'0'; - else - conv[2]=' '; - conv[3]=(xx)%10+'0'; - conv[4]=0; - return conv; -} - -// convert float to string with 12345 format -char *ftostr5(const float &x) -{ - long xx=abs(x); - if (xx >= 10000) - conv[0]=(xx/10000)%10+'0'; - else - conv[0]=' '; - if (xx >= 1000) - conv[1]=(xx/1000)%10+'0'; - else - conv[1]=' '; - if (xx >= 100) - conv[2]=(xx/100)%10+'0'; - else - conv[2]=' '; - if (xx >= 10) - conv[3]=(xx/10)%10+'0'; - else - conv[3]=' '; - conv[4]=(xx)%10+'0'; - conv[5]=0; - return conv; -} - -// convert float to string with +1234.5 format -char *ftostr51(const float &x) -{ - long xx=x*10; - conv[0]=(xx>=0)?'+':'-'; - xx=abs(xx); - conv[1]=(xx/10000)%10+'0'; - conv[2]=(xx/1000)%10+'0'; - conv[3]=(xx/100)%10+'0'; - conv[4]=(xx/10)%10+'0'; - conv[5]='.'; - conv[6]=(xx)%10+'0'; - conv[7]=0; - return conv; -} - -// convert float to string with +123.45 format -char *ftostr52(const float &x) -{ - long xx=x*100; - conv[0]=(xx>=0)?'+':'-'; - xx=abs(xx); - conv[1]=(xx/10000)%10+'0'; - conv[2]=(xx/1000)%10+'0'; - conv[3]=(xx/100)%10+'0'; - conv[4]='.'; - conv[5]=(xx/10)%10+'0'; - conv[6]=(xx)%10+'0'; - conv[7]=0; - return conv; -} - -// Callback for after editing PID i value -// grab the PID i value out of the temp variable; scale it; then update the PID driver -void copy_and_scalePID_i() -{ -#ifdef PIDTEMP - Ki = scalePID_i(raw_Ki); - updatePID(); -#endif -} - -// Callback for after editing PID d value -// grab the PID d value out of the temp variable; scale it; then update the PID driver -void copy_and_scalePID_d() -{ -#ifdef PIDTEMP - Kd = scalePID_d(raw_Kd); - updatePID(); -#endif -} - -#endif //ULTRA_LCD + } + +#ifdef ULTIPANEL_FEEDMULTIPLY + // Dead zone at 100% feedrate + if ((feedmultiply < 100 && (feedmultiply + int(encoderPosition)) > 100) || + (feedmultiply > 100 && (feedmultiply + int(encoderPosition)) < 100)) + { + encoderPosition = 0; + feedmultiply = 100; + } + + if (feedmultiply == 100 && int(encoderPosition) > ENCODER_FEEDRATE_DEADZONE) + { + feedmultiply += int(encoderPosition) - ENCODER_FEEDRATE_DEADZONE; + encoderPosition = 0; + } + else if (feedmultiply == 100 && int(encoderPosition) < -ENCODER_FEEDRATE_DEADZONE) + { + feedmultiply += int(encoderPosition) + ENCODER_FEEDRATE_DEADZONE; + encoderPosition = 0; + } + else if (feedmultiply != 100) + { + feedmultiply += int(encoderPosition); + encoderPosition = 0; + } +#endif//ULTIPANEL_FEEDMULTIPLY + + if (feedmultiply < 10) + feedmultiply = 10; + if (feedmultiply > 999) + feedmultiply = 999; +#endif//ULTIPANEL +} + +#ifdef ULTIPANEL +static void lcd_return_to_status() +{ + encoderPosition = 0; + currentMenu = lcd_status_screen; +} + +static void lcd_sdcard_pause() +{ + card.pauseSDPrint(); +} +static void lcd_sdcard_resume() +{ + card.startFileprint(); +} + +static void lcd_sdcard_stop() +{ + card.sdprinting = false; + card.closefile(); + quickStop(); + if(SD_FINISHED_STEPPERRELEASE) + { + enquecommand_P(PSTR(SD_FINISHED_RELEASECOMMAND)); + } + autotempShutdown(); +} + +/* Menu implementation */ +static void lcd_main_menu() +{ + START_MENU(); + MENU_ITEM(back, MSG_WATCH, lcd_status_screen); + if (movesplanned() || IS_SD_PRINTING) + { + MENU_ITEM(submenu, MSG_TUNE, lcd_tune_menu); + }else{ + MENU_ITEM(submenu, MSG_PREPARE, lcd_prepare_menu); + } + MENU_ITEM(submenu, MSG_CONTROL, lcd_control_menu); +#ifdef SDSUPPORT + if (card.cardOK) + { + if (card.isFileOpen()) + { + if (card.sdprinting) + MENU_ITEM(function, MSG_PAUSE_PRINT, lcd_sdcard_pause); + else + MENU_ITEM(function, MSG_RESUME_PRINT, lcd_sdcard_resume); + MENU_ITEM(function, MSG_STOP_PRINT, lcd_sdcard_stop); + }else{ + MENU_ITEM(submenu, MSG_CARD_MENU, lcd_sdcard_menu); +#if SDCARDDETECT < 1 + MENU_ITEM(gcode, MSG_CNG_SDCARD, PSTR("M21")); // SD-card changed by user +#endif + } + }else{ + MENU_ITEM(submenu, MSG_NO_CARD, lcd_sdcard_menu); +#if SDCARDDETECT < 1 + MENU_ITEM(gcode, MSG_INIT_SDCARD, PSTR("M21")); // Manually initialize the SD-card via user interface +#endif + } +#endif + END_MENU(); +} + +#ifdef SDSUPPORT +static void lcd_autostart_sd() +{ + card.lastnr=0; + card.setroot(); + card.checkautostart(true); +} +#endif + +#ifdef BABYSTEPPING +static void lcd_babystep_x() +{ + if (encoderPosition != 0) + { + babystepsTodo[X_AXIS]+=(int)encoderPosition; + encoderPosition=0; + lcdDrawUpdate = 1; + } + if (lcdDrawUpdate) + { + lcd_implementation_drawedit(PSTR(MSG_BABYSTEPPING_X),""); + } + if (LCD_CLICKED) + { + lcd_quick_feedback(); + currentMenu = lcd_tune_menu; + encoderPosition = 0; + } +} + +static void lcd_babystep_y() +{ + if (encoderPosition != 0) + { + babystepsTodo[Y_AXIS]+=(int)encoderPosition; + encoderPosition=0; + lcdDrawUpdate = 1; + } + if (lcdDrawUpdate) + { + lcd_implementation_drawedit(PSTR(MSG_BABYSTEPPING_Y),""); + } + if (LCD_CLICKED) + { + lcd_quick_feedback(); + currentMenu = lcd_tune_menu; + encoderPosition = 0; + } +} + +static void lcd_babystep_z() +{ + if (encoderPosition != 0) + { + babystepsTodo[Z_AXIS]+=BABYSTEP_Z_MULTIPLICATOR*(int)encoderPosition; + encoderPosition=0; + lcdDrawUpdate = 1; + } + if (lcdDrawUpdate) + { + lcd_implementation_drawedit(PSTR(MSG_BABYSTEPPING_Z),""); + } + if (LCD_CLICKED) + { + lcd_quick_feedback(); + currentMenu = lcd_tune_menu; + encoderPosition = 0; + } +} +#endif //BABYSTEPPING + +static void lcd_tune_menu() +{ + START_MENU(); + MENU_ITEM(back, MSG_MAIN, lcd_main_menu); + MENU_ITEM_EDIT(int3, MSG_SPEED, &feedmultiply, 10, 999); + MENU_ITEM_EDIT(int3, MSG_NOZZLE, &target_temperature[0], 0, HEATER_0_MAXTEMP - 15); +#if TEMP_SENSOR_1 != 0 + MENU_ITEM_EDIT(int3, MSG_NOZZLE1, &target_temperature[1], 0, HEATER_1_MAXTEMP - 15); +#endif +#if TEMP_SENSOR_2 != 0 + MENU_ITEM_EDIT(int3, MSG_NOZZLE2, &target_temperature[2], 0, HEATER_2_MAXTEMP - 15); +#endif +#if TEMP_SENSOR_BED != 0 + MENU_ITEM_EDIT(int3, MSG_BED, &target_temperature_bed, 0, BED_MAXTEMP - 15); +#endif + MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &fanSpeed, 0, 255); + MENU_ITEM_EDIT(int3, MSG_FLOW, &extrudemultiply, 10, 999); + MENU_ITEM_EDIT(int3, MSG_FLOW0, &extruder_multiply[0], 10, 999); +#if TEMP_SENSOR_1 != 0 + MENU_ITEM_EDIT(int3, MSG_FLOW1, &extruder_multiply[1], 10, 999); +#endif +#if TEMP_SENSOR_2 != 0 + MENU_ITEM_EDIT(int3, MSG_FLOW2, &extruder_multiply[2], 10, 999); +#endif + +#ifdef BABYSTEPPING + #ifdef BABYSTEP_XY + MENU_ITEM(submenu, MSG_BABYSTEP_X, lcd_babystep_x); + MENU_ITEM(submenu, MSG_BABYSTEP_Y, lcd_babystep_y); + #endif //BABYSTEP_XY + MENU_ITEM(submenu, MSG_BABYSTEP_Z, lcd_babystep_z); +#endif +#ifdef FILAMENTCHANGEENABLE + MENU_ITEM(gcode, MSG_FILAMENTCHANGE, PSTR("M600")); +#endif + END_MENU(); +} + +void lcd_preheat_pla0() +{ + setTargetHotend0(plaPreheatHotendTemp); + setTargetBed(plaPreheatHPBTemp); + fanSpeed = plaPreheatFanSpeed; + lcd_return_to_status(); + setWatch(); // heater sanity check timer +} + +void lcd_preheat_abs0() +{ + setTargetHotend0(absPreheatHotendTemp); + setTargetBed(absPreheatHPBTemp); + fanSpeed = absPreheatFanSpeed; + lcd_return_to_status(); + setWatch(); // heater sanity check timer +} + +#if TEMP_SENSOR_1 != 0 //2nd extruder preheat +void lcd_preheat_pla1() +{ + setTargetHotend1(plaPreheatHotendTemp); + setTargetBed(plaPreheatHPBTemp); + fanSpeed = plaPreheatFanSpeed; + lcd_return_to_status(); + setWatch(); // heater sanity check timer +} + +void lcd_preheat_abs1() +{ + setTargetHotend1(absPreheatHotendTemp); + setTargetBed(absPreheatHPBTemp); + fanSpeed = absPreheatFanSpeed; + lcd_return_to_status(); + setWatch(); // heater sanity check timer +} +#endif //2nd extruder preheat + +#if TEMP_SENSOR_2 != 0 //3 extruder preheat +void lcd_preheat_pla2() +{ + setTargetHotend2(plaPreheatHotendTemp); + setTargetBed(plaPreheatHPBTemp); + fanSpeed = plaPreheatFanSpeed; + lcd_return_to_status(); + setWatch(); // heater sanity check timer +} + +void lcd_preheat_abs2() +{ + setTargetHotend2(absPreheatHotendTemp); + setTargetBed(absPreheatHPBTemp); + fanSpeed = absPreheatFanSpeed; + lcd_return_to_status(); + setWatch(); // heater sanity check timer +} +#endif //3 extruder preheat + +#if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 //more than one extruder present +void lcd_preheat_pla012() +{ + setTargetHotend0(plaPreheatHotendTemp); + setTargetHotend1(plaPreheatHotendTemp); + setTargetHotend2(plaPreheatHotendTemp); + setTargetBed(plaPreheatHPBTemp); + fanSpeed = plaPreheatFanSpeed; + lcd_return_to_status(); + setWatch(); // heater sanity check timer +} + +void lcd_preheat_abs012() +{ + setTargetHotend0(absPreheatHotendTemp); + setTargetHotend1(absPreheatHotendTemp); + setTargetHotend2(absPreheatHotendTemp); + setTargetBed(absPreheatHPBTemp); + fanSpeed = absPreheatFanSpeed; + lcd_return_to_status(); + setWatch(); // heater sanity check timer +} +#endif //more than one extruder present + +void lcd_preheat_pla_bedonly() +{ + setTargetBed(plaPreheatHPBTemp); + fanSpeed = plaPreheatFanSpeed; + lcd_return_to_status(); + setWatch(); // heater sanity check timer +} + +void lcd_preheat_abs_bedonly() +{ + setTargetBed(absPreheatHPBTemp); + fanSpeed = absPreheatFanSpeed; + lcd_return_to_status(); + setWatch(); // heater sanity check timer +} + +static void lcd_preheat_pla_menu() +{ + START_MENU(); + MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu); + MENU_ITEM(function, MSG_PREHEAT_PLA0, lcd_preheat_pla0); +#if TEMP_SENSOR_1 != 0 //2 extruder preheat + MENU_ITEM(function, MSG_PREHEAT_PLA1, lcd_preheat_pla1); +#endif //2 extruder preheat +#if TEMP_SENSOR_2 != 0 //3 extruder preheat + MENU_ITEM(function, MSG_PREHEAT_PLA2, lcd_preheat_pla2); +#endif //3 extruder preheat +#if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 //all extruder preheat + MENU_ITEM(function, MSG_PREHEAT_PLA012, lcd_preheat_pla012); +#endif //2 extruder preheat +#if TEMP_SENSOR_BED != 0 + MENU_ITEM(function, MSG_PREHEAT_PLA_BEDONLY, lcd_preheat_pla_bedonly); +#endif + END_MENU(); +} + +static void lcd_preheat_abs_menu() +{ + START_MENU(); + MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu); + MENU_ITEM(function, MSG_PREHEAT_ABS0, lcd_preheat_abs0); +#if TEMP_SENSOR_1 != 0 //2 extruder preheat + MENU_ITEM(function, MSG_PREHEAT_ABS1, lcd_preheat_abs1); +#endif //2 extruder preheat +#if TEMP_SENSOR_2 != 0 //3 extruder preheat + MENU_ITEM(function, MSG_PREHEAT_ABS2, lcd_preheat_abs2); +#endif //3 extruder preheat +#if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 //all extruder preheat + MENU_ITEM(function, MSG_PREHEAT_ABS012, lcd_preheat_abs012); +#endif //2 extruder preheat +#if TEMP_SENSOR_BED != 0 + MENU_ITEM(function, MSG_PREHEAT_ABS_BEDONLY, lcd_preheat_abs_bedonly); +#endif + END_MENU(); +} + +void lcd_cooldown() +{ + setTargetHotend0(0); + setTargetHotend1(0); + setTargetHotend2(0); + setTargetBed(0); + fanSpeed = 0; + lcd_return_to_status(); +} + +static void lcd_prepare_menu() +{ + START_MENU(); + MENU_ITEM(back, MSG_MAIN, lcd_main_menu); +#ifdef SDSUPPORT + #ifdef MENU_ADDAUTOSTART + MENU_ITEM(function, MSG_AUTOSTART, lcd_autostart_sd); + #endif +#endif + MENU_ITEM(gcode, MSG_DISABLE_STEPPERS, PSTR("M84")); + MENU_ITEM(gcode, MSG_AUTO_HOME, PSTR("G28")); + //MENU_ITEM(gcode, MSG_SET_ORIGIN, PSTR("G92 X0 Y0 Z0")); +#if TEMP_SENSOR_0 != 0 + #if TEMP_SENSOR_1 != 0 || TEMP_SENSOR_2 != 0 || TEMP_SENSOR_BED != 0 + MENU_ITEM(submenu, MSG_PREHEAT_PLA, lcd_preheat_pla_menu); + MENU_ITEM(submenu, MSG_PREHEAT_ABS, lcd_preheat_abs_menu); + #else + MENU_ITEM(function, MSG_PREHEAT_PLA, lcd_preheat_pla0); + MENU_ITEM(function, MSG_PREHEAT_ABS, lcd_preheat_abs0); + #endif +#endif + MENU_ITEM(function, MSG_COOLDOWN, lcd_cooldown); +#if PS_ON_PIN > -1 + if (powersupply) + { + MENU_ITEM(gcode, MSG_SWITCH_PS_OFF, PSTR("M81")); + }else{ + MENU_ITEM(gcode, MSG_SWITCH_PS_ON, PSTR("M80")); + } +#endif + MENU_ITEM(submenu, MSG_MOVE_AXIS, lcd_move_menu); + END_MENU(); +} + +float move_menu_scale; +static void lcd_move_menu_axis(); + +static void lcd_move_x() +{ + if (encoderPosition != 0) + { + refresh_cmd_timeout(); + current_position[X_AXIS] += float((int)encoderPosition) * move_menu_scale; + if (min_software_endstops && current_position[X_AXIS] < X_MIN_POS) + current_position[X_AXIS] = X_MIN_POS; + if (max_software_endstops && current_position[X_AXIS] > X_MAX_POS) + current_position[X_AXIS] = X_MAX_POS; + encoderPosition = 0; + #ifdef DELTA + calculate_delta(current_position); + plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[X_AXIS]/60, active_extruder); + #else + plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[X_AXIS]/60, active_extruder); + #endif + lcdDrawUpdate = 1; + } + if (lcdDrawUpdate) + { + lcd_implementation_drawedit(PSTR("X"), ftostr31(current_position[X_AXIS])); + } + if (LCD_CLICKED) + { + lcd_quick_feedback(); + currentMenu = lcd_move_menu_axis; + encoderPosition = 0; + } +} +static void lcd_move_y() +{ + if (encoderPosition != 0) + { + refresh_cmd_timeout(); + current_position[Y_AXIS] += float((int)encoderPosition) * move_menu_scale; + if (min_software_endstops && current_position[Y_AXIS] < Y_MIN_POS) + current_position[Y_AXIS] = Y_MIN_POS; + if (max_software_endstops && current_position[Y_AXIS] > Y_MAX_POS) + current_position[Y_AXIS] = Y_MAX_POS; + encoderPosition = 0; + #ifdef DELTA + calculate_delta(current_position); + plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[Y_AXIS]/60, active_extruder); + #else + plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[Y_AXIS]/60, active_extruder); + #endif + lcdDrawUpdate = 1; + } + if (lcdDrawUpdate) + { + lcd_implementation_drawedit(PSTR("Y"), ftostr31(current_position[Y_AXIS])); + } + if (LCD_CLICKED) + { + lcd_quick_feedback(); + currentMenu = lcd_move_menu_axis; + encoderPosition = 0; + } +} +static void lcd_move_z() +{ + if (encoderPosition != 0) + { + refresh_cmd_timeout(); + current_position[Z_AXIS] += float((int)encoderPosition) * move_menu_scale; + if (min_software_endstops && current_position[Z_AXIS] < Z_MIN_POS) + current_position[Z_AXIS] = Z_MIN_POS; + if (max_software_endstops && current_position[Z_AXIS] > Z_MAX_POS) + current_position[Z_AXIS] = Z_MAX_POS; + encoderPosition = 0; + #ifdef DELTA + calculate_delta(current_position); + plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[Z_AXIS]/60, active_extruder); + #else + plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[Z_AXIS]/60, active_extruder); + #endif + lcdDrawUpdate = 1; + } + if (lcdDrawUpdate) + { + lcd_implementation_drawedit(PSTR("Z"), ftostr31(current_position[Z_AXIS])); + } + if (LCD_CLICKED) + { + lcd_quick_feedback(); + currentMenu = lcd_move_menu_axis; + encoderPosition = 0; + } +} +static void lcd_move_e() +{ + if (encoderPosition != 0) + { + current_position[E_AXIS] += float((int)encoderPosition) * move_menu_scale; + encoderPosition = 0; + #ifdef DELTA + calculate_delta(current_position); + plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder); + #else + plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder); + #endif + lcdDrawUpdate = 1; + } + if (lcdDrawUpdate) + { + lcd_implementation_drawedit(PSTR("Extruder"), ftostr31(current_position[E_AXIS])); + } + if (LCD_CLICKED) + { + lcd_quick_feedback(); + currentMenu = lcd_move_menu_axis; + encoderPosition = 0; + } +} + +static void lcd_move_menu_axis() +{ + START_MENU(); + MENU_ITEM(back, MSG_MOVE_AXIS, lcd_move_menu); + MENU_ITEM(submenu, MSG_MOVE_X, lcd_move_x); + MENU_ITEM(submenu, MSG_MOVE_Y, lcd_move_y); + if (move_menu_scale < 10.0) + { + MENU_ITEM(submenu, MSG_MOVE_Z, lcd_move_z); + MENU_ITEM(submenu, MSG_MOVE_E, lcd_move_e); + } + END_MENU(); +} + +static void lcd_move_menu_10mm() +{ + move_menu_scale = 10.0; + lcd_move_menu_axis(); +} +static void lcd_move_menu_1mm() +{ + move_menu_scale = 1.0; + lcd_move_menu_axis(); +} +static void lcd_move_menu_01mm() +{ + move_menu_scale = 0.1; + lcd_move_menu_axis(); +} + +static void lcd_move_menu() +{ + START_MENU(); + MENU_ITEM(back, MSG_PREPARE, lcd_prepare_menu); + MENU_ITEM(submenu, MSG_MOVE_10MM, lcd_move_menu_10mm); + MENU_ITEM(submenu, MSG_MOVE_1MM, lcd_move_menu_1mm); + MENU_ITEM(submenu, MSG_MOVE_01MM, lcd_move_menu_01mm); + //TODO:X,Y,Z,E + END_MENU(); +} + +static void lcd_control_menu() +{ + START_MENU(); + MENU_ITEM(back, MSG_MAIN, lcd_main_menu); + MENU_ITEM(submenu, MSG_TEMPERATURE, lcd_control_temperature_menu); + MENU_ITEM(submenu, MSG_MOTION, lcd_control_motion_menu); +#ifdef DOGLCD +// MENU_ITEM_EDIT(int3, MSG_CONTRAST, &lcd_contrast, 0, 63); + MENU_ITEM(submenu, MSG_CONTRAST, lcd_set_contrast); +#endif +#ifdef FWRETRACT + MENU_ITEM(submenu, MSG_RETRACT, lcd_control_retract_menu); +#endif +#ifdef EEPROM_SETTINGS + MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings); + MENU_ITEM(function, MSG_LOAD_EPROM, Config_RetrieveSettings); +#endif + MENU_ITEM(function, MSG_RESTORE_FAILSAFE, Config_ResetDefault); + END_MENU(); +} + +static void lcd_control_temperature_menu() +{ +#ifdef PIDTEMP + // set up temp variables - undo the default scaling + raw_Ki = unscalePID_i(Ki); + raw_Kd = unscalePID_d(Kd); +#endif + + START_MENU(); + MENU_ITEM(back, MSG_CONTROL, lcd_control_menu); + MENU_ITEM_EDIT(int3, MSG_NOZZLE, &target_temperature[0], 0, HEATER_0_MAXTEMP - 15); +#if TEMP_SENSOR_1 != 0 + MENU_ITEM_EDIT(int3, MSG_NOZZLE1, &target_temperature[1], 0, HEATER_1_MAXTEMP - 15); +#endif +#if TEMP_SENSOR_2 != 0 + MENU_ITEM_EDIT(int3, MSG_NOZZLE2, &target_temperature[2], 0, HEATER_2_MAXTEMP - 15); +#endif +#if TEMP_SENSOR_BED != 0 + MENU_ITEM_EDIT(int3, MSG_BED, &target_temperature_bed, 0, BED_MAXTEMP - 15); +#endif + MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &fanSpeed, 0, 255); +#ifdef AUTOTEMP + MENU_ITEM_EDIT(bool, MSG_AUTOTEMP, &autotemp_enabled); + MENU_ITEM_EDIT(float3, MSG_MIN, &autotemp_min, 0, HEATER_0_MAXTEMP - 15); + MENU_ITEM_EDIT(float3, MSG_MAX, &autotemp_max, 0, HEATER_0_MAXTEMP - 15); + MENU_ITEM_EDIT(float32, MSG_FACTOR, &autotemp_factor, 0.0, 1.0); +#endif +#ifdef PIDTEMP + MENU_ITEM_EDIT(float52, MSG_PID_P, &Kp, 1, 9990); + // i is typically a small value so allows values below 1 + MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_I, &raw_Ki, 0.01, 9990, copy_and_scalePID_i); + MENU_ITEM_EDIT_CALLBACK(float52, MSG_PID_D, &raw_Kd, 1, 9990, copy_and_scalePID_d); +# ifdef PID_ADD_EXTRUSION_RATE + MENU_ITEM_EDIT(float3, MSG_PID_C, &Kc, 1, 9990); +# endif//PID_ADD_EXTRUSION_RATE +#endif//PIDTEMP + MENU_ITEM(submenu, MSG_PREHEAT_PLA_SETTINGS, lcd_control_temperature_preheat_pla_settings_menu); + MENU_ITEM(submenu, MSG_PREHEAT_ABS_SETTINGS, lcd_control_temperature_preheat_abs_settings_menu); + END_MENU(); +} + +static void lcd_control_temperature_preheat_pla_settings_menu() +{ + START_MENU(); + MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu); + MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &plaPreheatFanSpeed, 0, 255); + MENU_ITEM_EDIT(int3, MSG_NOZZLE, &plaPreheatHotendTemp, 0, HEATER_0_MAXTEMP - 15); +#if TEMP_SENSOR_BED != 0 + MENU_ITEM_EDIT(int3, MSG_BED, &plaPreheatHPBTemp, 0, BED_MAXTEMP - 15); +#endif +#ifdef EEPROM_SETTINGS + MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings); +#endif + END_MENU(); +} + +static void lcd_control_temperature_preheat_abs_settings_menu() +{ + START_MENU(); + MENU_ITEM(back, MSG_TEMPERATURE, lcd_control_temperature_menu); + MENU_ITEM_EDIT(int3, MSG_FAN_SPEED, &absPreheatFanSpeed, 0, 255); + MENU_ITEM_EDIT(int3, MSG_NOZZLE, &absPreheatHotendTemp, 0, HEATER_0_MAXTEMP - 15); +#if TEMP_SENSOR_BED != 0 + MENU_ITEM_EDIT(int3, MSG_BED, &absPreheatHPBTemp, 0, BED_MAXTEMP - 15); +#endif +#ifdef EEPROM_SETTINGS + MENU_ITEM(function, MSG_STORE_EPROM, Config_StoreSettings); +#endif + END_MENU(); +} + +static void lcd_control_motion_menu() +{ + START_MENU(); + MENU_ITEM(back, MSG_CONTROL, lcd_control_menu); +#ifdef ENABLE_AUTO_BED_LEVELING + MENU_ITEM_EDIT(float32, MSG_ZPROBE_ZOFFSET, &zprobe_zoffset, 0.5, 50); +#endif + MENU_ITEM_EDIT(float5, MSG_ACC, &acceleration, 500, 99000); + MENU_ITEM_EDIT(float3, MSG_VXY_JERK, &max_xy_jerk, 1, 990); + MENU_ITEM_EDIT(float52, MSG_VZ_JERK, &max_z_jerk, 0.1, 990); + MENU_ITEM_EDIT(float3, MSG_VE_JERK, &max_e_jerk, 1, 990); + MENU_ITEM_EDIT(float3, MSG_VMAX MSG_X, &max_feedrate[X_AXIS], 1, 999); + MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Y, &max_feedrate[Y_AXIS], 1, 999); + MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Z, &max_feedrate[Z_AXIS], 1, 999); + MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E, &max_feedrate[E_AXIS], 1, 999); + MENU_ITEM_EDIT(float3, MSG_VMIN, &minimumfeedrate, 0, 999); + MENU_ITEM_EDIT(float3, MSG_VTRAV_MIN, &mintravelfeedrate, 0, 999); + MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_X, &max_acceleration_units_per_sq_second[X_AXIS], 100, 99000, reset_acceleration_rates); + MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Y, &max_acceleration_units_per_sq_second[Y_AXIS], 100, 99000, reset_acceleration_rates); + MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Z, &max_acceleration_units_per_sq_second[Z_AXIS], 100, 99000, reset_acceleration_rates); + MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_E, &max_acceleration_units_per_sq_second[E_AXIS], 100, 99000, reset_acceleration_rates); + MENU_ITEM_EDIT(float5, MSG_A_RETRACT, &retract_acceleration, 100, 99000); + MENU_ITEM_EDIT(float52, MSG_XSTEPS, &axis_steps_per_unit[X_AXIS], 5, 9999); + MENU_ITEM_EDIT(float52, MSG_YSTEPS, &axis_steps_per_unit[Y_AXIS], 5, 9999); + MENU_ITEM_EDIT(float51, MSG_ZSTEPS, &axis_steps_per_unit[Z_AXIS], 5, 9999); + MENU_ITEM_EDIT(float51, MSG_ESTEPS, &axis_steps_per_unit[E_AXIS], 5, 9999); +#ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED + MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &abort_on_endstop_hit); +#endif +#ifdef SCARA + MENU_ITEM_EDIT(float74, MSG_XSCALE, &axis_scaling[X_AXIS],0.5,2); + MENU_ITEM_EDIT(float74, MSG_YSCALE, &axis_scaling[Y_AXIS],0.5,2); +#endif + END_MENU(); +} + +#ifdef DOGLCD +static void lcd_set_contrast() +{ + if (encoderPosition != 0) + { + lcd_contrast -= encoderPosition; + if (lcd_contrast < 0) lcd_contrast = 0; + else if (lcd_contrast > 63) lcd_contrast = 63; + encoderPosition = 0; + lcdDrawUpdate = 1; + u8g.setContrast(lcd_contrast); + } + if (lcdDrawUpdate) + { + lcd_implementation_drawedit(PSTR(MSG_CONTRAST), itostr2(lcd_contrast)); + } + if (LCD_CLICKED) + { + lcd_quick_feedback(); + currentMenu = lcd_control_menu; + encoderPosition = 0; + } +} +#endif + +#ifdef FWRETRACT +static void lcd_control_retract_menu() +{ + START_MENU(); + MENU_ITEM(back, MSG_CONTROL, lcd_control_menu); + MENU_ITEM_EDIT(bool, MSG_AUTORETRACT, &autoretract_enabled); + MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT, &retract_length, 0, 100); + MENU_ITEM_EDIT(float3, MSG_CONTROL_RETRACTF, &retract_feedrate, 1, 999); + MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_ZLIFT, &retract_zlift, 0, 999); + MENU_ITEM_EDIT(float52, MSG_CONTROL_RETRACT_RECOVER, &retract_recover_length, 0, 100); + MENU_ITEM_EDIT(float3, MSG_CONTROL_RETRACT_RECOVERF, &retract_recover_feedrate, 1, 999); + END_MENU(); +} +#endif + +#if SDCARDDETECT == -1 +static void lcd_sd_refresh() +{ + card.initsd(); + currentMenuViewOffset = 0; +} +#endif +static void lcd_sd_updir() +{ + card.updir(); + currentMenuViewOffset = 0; +} + +void lcd_sdcard_menu() +{ + if (lcdDrawUpdate == 0 && LCD_CLICKED == 0) + return; // nothing to do (so don't thrash the SD card) + uint16_t fileCnt = card.getnrfilenames(); + START_MENU(); + MENU_ITEM(back, MSG_MAIN, lcd_main_menu); + card.getWorkDirName(); + if(card.filename[0]=='/') + { +#if SDCARDDETECT == -1 + MENU_ITEM(function, LCD_STR_REFRESH MSG_REFRESH, lcd_sd_refresh); +#endif + }else{ + MENU_ITEM(function, LCD_STR_FOLDER "..", lcd_sd_updir); + } + + for(uint16_t i=0;i maxEditValue) \ + encoderPosition = maxEditValue; \ + if (lcdDrawUpdate) \ + lcd_implementation_drawedit(editLabel, _strFunc(((_type)encoderPosition) / scale)); \ + if (LCD_CLICKED) \ + { \ + *((_type*)editValue) = ((_type)encoderPosition) / scale; \ + lcd_quick_feedback(); \ + currentMenu = prevMenu; \ + encoderPosition = prevEncoderPosition; \ + } \ + } \ + void menu_edit_callback_ ## _name () \ + { \ + if ((int32_t)encoderPosition < minEditValue) \ + encoderPosition = minEditValue; \ + if ((int32_t)encoderPosition > maxEditValue) \ + encoderPosition = maxEditValue; \ + if (lcdDrawUpdate) \ + lcd_implementation_drawedit(editLabel, _strFunc(((_type)encoderPosition) / scale)); \ + if (LCD_CLICKED) \ + { \ + *((_type*)editValue) = ((_type)encoderPosition) / scale; \ + lcd_quick_feedback(); \ + currentMenu = prevMenu; \ + encoderPosition = prevEncoderPosition; \ + (*callbackFunc)();\ + } \ + } \ + static void menu_action_setting_edit_ ## _name (const char* pstr, _type* ptr, _type minValue, _type maxValue) \ + { \ + prevMenu = currentMenu; \ + prevEncoderPosition = encoderPosition; \ + \ + lcdDrawUpdate = 2; \ + currentMenu = menu_edit_ ## _name; \ + \ + editLabel = pstr; \ + editValue = ptr; \ + minEditValue = minValue * scale; \ + maxEditValue = maxValue * scale; \ + encoderPosition = (*ptr) * scale; \ + }\ + static void menu_action_setting_edit_callback_ ## _name (const char* pstr, _type* ptr, _type minValue, _type maxValue, menuFunc_t callback) \ + { \ + prevMenu = currentMenu; \ + prevEncoderPosition = encoderPosition; \ + \ + lcdDrawUpdate = 2; \ + currentMenu = menu_edit_callback_ ## _name; \ + \ + editLabel = pstr; \ + editValue = ptr; \ + minEditValue = minValue * scale; \ + maxEditValue = maxValue * scale; \ + encoderPosition = (*ptr) * scale; \ + callbackFunc = callback;\ + } +menu_edit_type(int, int3, itostr3, 1) +menu_edit_type(float, float3, ftostr3, 1) +menu_edit_type(float, float32, ftostr32, 100) +menu_edit_type(float, float5, ftostr5, 0.01) +menu_edit_type(float, float51, ftostr51, 10) +menu_edit_type(float, float52, ftostr52, 100) +menu_edit_type(unsigned long, long5, ftostr5, 0.01) + +#ifdef REPRAPWORLD_KEYPAD + static void reprapworld_keypad_move_z_up() { + encoderPosition = 1; + move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; + lcd_move_z(); + } + static void reprapworld_keypad_move_z_down() { + encoderPosition = -1; + move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; + lcd_move_z(); + } + static void reprapworld_keypad_move_x_left() { + encoderPosition = -1; + move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; + lcd_move_x(); + } + static void reprapworld_keypad_move_x_right() { + encoderPosition = 1; + move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; + lcd_move_x(); + } + static void reprapworld_keypad_move_y_down() { + encoderPosition = 1; + move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; + lcd_move_y(); + } + static void reprapworld_keypad_move_y_up() { + encoderPosition = -1; + move_menu_scale = REPRAPWORLD_KEYPAD_MOVE_STEP; + lcd_move_y(); + } + static void reprapworld_keypad_move_home() { + enquecommand_P((PSTR("G28"))); // move all axis home + } +#endif + +/** End of menus **/ + +static void lcd_quick_feedback() +{ + lcdDrawUpdate = 2; + blocking_enc = millis() + 500; + lcd_implementation_quick_feedback(); +} + +/** Menu action functions **/ +static void menu_action_back(menuFunc_t data) +{ + currentMenu = data; + encoderPosition = 0; +} +static void menu_action_submenu(menuFunc_t data) +{ + currentMenu = data; + encoderPosition = 0; +} +static void menu_action_gcode(const char* pgcode) +{ + enquecommand_P(pgcode); +} +static void menu_action_function(menuFunc_t data) +{ + (*data)(); +} +static void menu_action_sdfile(const char* filename, char* longFilename) +{ + char cmd[30]; + char* c; + sprintf_P(cmd, PSTR("M23 %s"), filename); + for(c = &cmd[4]; *c; c++) + *c = tolower(*c); + enquecommand(cmd); + enquecommand_P(PSTR("M24")); + lcd_return_to_status(); +} +static void menu_action_sddirectory(const char* filename, char* longFilename) +{ + card.chdir(filename); + encoderPosition = 0; +} +static void menu_action_setting_edit_bool(const char* pstr, bool* ptr) +{ + *ptr = !(*ptr); +} +#endif//ULTIPANEL + +/** LCD API **/ +void lcd_init() +{ + lcd_implementation_init(); + +#ifdef NEWPANEL + pinMode(BTN_EN1,INPUT); + pinMode(BTN_EN2,INPUT); + WRITE(BTN_EN1,HIGH); + WRITE(BTN_EN2,HIGH); + #if BTN_ENC > 0 + pinMode(BTN_ENC,INPUT); + WRITE(BTN_ENC,HIGH); + #endif + #ifdef REPRAPWORLD_KEYPAD + pinMode(SHIFT_CLK,OUTPUT); + pinMode(SHIFT_LD,OUTPUT); + pinMode(SHIFT_OUT,INPUT); + WRITE(SHIFT_OUT,HIGH); + WRITE(SHIFT_LD,HIGH); + #endif +#else // Not NEWPANEL + #ifdef SR_LCD_2W_NL // Non latching 2 wire shift register + pinMode (SR_DATA_PIN, OUTPUT); + pinMode (SR_CLK_PIN, OUTPUT); + #elif defined(SHIFT_CLK) + pinMode(SHIFT_CLK,OUTPUT); + pinMode(SHIFT_LD,OUTPUT); + pinMode(SHIFT_EN,OUTPUT); + pinMode(SHIFT_OUT,INPUT); + WRITE(SHIFT_OUT,HIGH); + WRITE(SHIFT_LD,HIGH); + WRITE(SHIFT_EN,LOW); + #else + #ifdef ULTIPANEL + #error ULTIPANEL requires an encoder + #endif + #endif // SR_LCD_2W_NL +#endif//!NEWPANEL + +#if defined (SDSUPPORT) && defined(SDCARDDETECT) && (SDCARDDETECT > 0) + pinMode(SDCARDDETECT,INPUT); + WRITE(SDCARDDETECT, HIGH); + lcd_oldcardstatus = IS_SD_INSERTED; +#endif//(SDCARDDETECT > 0) +#ifdef LCD_HAS_SLOW_BUTTONS + slow_buttons = 0; +#endif + lcd_buttons_update(); +#ifdef ULTIPANEL + encoderDiff = 0; +#endif +} + +void lcd_update() +{ + static unsigned long timeoutToStatus = 0; + + #ifdef LCD_HAS_SLOW_BUTTONS + slow_buttons = lcd_implementation_read_slow_buttons(); // buttons which take too long to read in interrupt context + #endif + + lcd_buttons_update(); + + #if (SDCARDDETECT > 0) + if((IS_SD_INSERTED != lcd_oldcardstatus)) + { + lcdDrawUpdate = 2; + lcd_oldcardstatus = IS_SD_INSERTED; + lcd_implementation_init(); // to maybe revive the LCD if static electricity killed it. + + if(lcd_oldcardstatus) + { + card.initsd(); + LCD_MESSAGEPGM(MSG_SD_INSERTED); + } + else + { + card.release(); + LCD_MESSAGEPGM(MSG_SD_REMOVED); + } + } + #endif//CARDINSERTED + + if (lcd_next_update_millis < millis()) + { +#ifdef ULTIPANEL + #ifdef REPRAPWORLD_KEYPAD + if (REPRAPWORLD_KEYPAD_MOVE_Z_UP) { + reprapworld_keypad_move_z_up(); + } + if (REPRAPWORLD_KEYPAD_MOVE_Z_DOWN) { + reprapworld_keypad_move_z_down(); + } + if (REPRAPWORLD_KEYPAD_MOVE_X_LEFT) { + reprapworld_keypad_move_x_left(); + } + if (REPRAPWORLD_KEYPAD_MOVE_X_RIGHT) { + reprapworld_keypad_move_x_right(); + } + if (REPRAPWORLD_KEYPAD_MOVE_Y_DOWN) { + reprapworld_keypad_move_y_down(); + } + if (REPRAPWORLD_KEYPAD_MOVE_Y_UP) { + reprapworld_keypad_move_y_up(); + } + if (REPRAPWORLD_KEYPAD_MOVE_HOME) { + reprapworld_keypad_move_home(); + } + #endif + if (abs(encoderDiff) >= ENCODER_PULSES_PER_STEP) + { + lcdDrawUpdate = 1; + encoderPosition += encoderDiff / ENCODER_PULSES_PER_STEP; + encoderDiff = 0; + timeoutToStatus = millis() + LCD_TIMEOUT_TO_STATUS; + } + if (LCD_CLICKED) + timeoutToStatus = millis() + LCD_TIMEOUT_TO_STATUS; +#endif//ULTIPANEL + +#ifdef DOGLCD // Changes due to different driver architecture of the DOGM display + blink++; // Variable for fan animation and alive dot + u8g.firstPage(); + do + { + u8g.setFont(u8g_font_6x10_marlin); + u8g.setPrintPos(125,0); + if (blink % 2) u8g.setColorIndex(1); else u8g.setColorIndex(0); // Set color for the alive dot + u8g.drawPixel(127,63); // draw alive dot + u8g.setColorIndex(1); // black on white + (*currentMenu)(); + if (!lcdDrawUpdate) break; // Terminate display update, when nothing new to draw. This must be done before the last dogm.next() + } while( u8g.nextPage() ); +#else + (*currentMenu)(); +#endif + +#ifdef LCD_HAS_STATUS_INDICATORS + lcd_implementation_update_indicators(); +#endif + +#ifdef ULTIPANEL + if(timeoutToStatus < millis() && currentMenu != lcd_status_screen) + { + lcd_return_to_status(); + lcdDrawUpdate = 2; + } +#endif//ULTIPANEL + if (lcdDrawUpdate == 2) + lcd_implementation_clear(); + if (lcdDrawUpdate) + lcdDrawUpdate--; + lcd_next_update_millis = millis() + 100; + } +} + +void lcd_setstatus(const char* message) +{ + if (lcd_status_message_level > 0) + return; + strncpy(lcd_status_message, message, LCD_WIDTH); + lcdDrawUpdate = 2; +} +void lcd_setstatuspgm(const char* message) +{ + if (lcd_status_message_level > 0) + return; + strncpy_P(lcd_status_message, message, LCD_WIDTH); + lcdDrawUpdate = 2; +} +void lcd_setalertstatuspgm(const char* message) +{ + lcd_setstatuspgm(message); + lcd_status_message_level = 1; +#ifdef ULTIPANEL + lcd_return_to_status(); +#endif//ULTIPANEL +} +void lcd_reset_alert_level() +{ + lcd_status_message_level = 0; +} + +#ifdef DOGLCD +void lcd_setcontrast(uint8_t value) +{ + lcd_contrast = value & 63; + u8g.setContrast(lcd_contrast); +} +#endif + +#ifdef ULTIPANEL +/* Warning: This function is called from interrupt context */ +void lcd_buttons_update() +{ +#ifdef NEWPANEL + uint8_t newbutton=0; + if(READ(BTN_EN1)==0) newbutton|=EN_A; + if(READ(BTN_EN2)==0) newbutton|=EN_B; + #if BTN_ENC > 0 + if((blocking_enc>1; + if(READ(SHIFT_OUT)) + newbutton_reprapworld_keypad|=(1<<7); + WRITE(SHIFT_CLK,HIGH); + WRITE(SHIFT_CLK,LOW); + } + buttons_reprapworld_keypad=~newbutton_reprapworld_keypad; //invert it, because a pressed switch produces a logical 0 + #endif +#else //read it from the shift register + uint8_t newbutton=0; + WRITE(SHIFT_LD,LOW); + WRITE(SHIFT_LD,HIGH); + unsigned char tmp_buttons=0; + for(int8_t i=0;i<8;i++) + { + newbutton = newbutton>>1; + if(READ(SHIFT_OUT)) + newbutton|=(1<<7); + WRITE(SHIFT_CLK,HIGH); + WRITE(SHIFT_CLK,LOW); + } + buttons=~newbutton; //invert it, because a pressed switch produces a logical 0 +#endif//!NEWPANEL + + //manage encoder rotation + uint8_t enc=0; + if(buttons&EN_A) + enc|=(1<<0); + if(buttons&EN_B) + enc|=(1<<1); + if(enc != lastEncoderBits) + { + switch(enc) + { + case encrot0: + if(lastEncoderBits==encrot3) + encoderDiff++; + else if(lastEncoderBits==encrot1) + encoderDiff--; + break; + case encrot1: + if(lastEncoderBits==encrot0) + encoderDiff++; + else if(lastEncoderBits==encrot2) + encoderDiff--; + break; + case encrot2: + if(lastEncoderBits==encrot1) + encoderDiff++; + else if(lastEncoderBits==encrot3) + encoderDiff--; + break; + case encrot3: + if(lastEncoderBits==encrot2) + encoderDiff++; + else if(lastEncoderBits==encrot0) + encoderDiff--; + break; + } + } + lastEncoderBits = enc; +} + +void lcd_buzz(long duration, uint16_t freq) +{ +#ifdef LCD_USE_I2C_BUZZER + lcd.buzz(duration,freq); +#endif +} + +bool lcd_clicked() +{ + return LCD_CLICKED; +} +#endif//ULTIPANEL + +/********************************/ +/** Float conversion utilities **/ +/********************************/ +// convert float to string with +123.4 format +char conv[8]; +char *ftostr3(const float &x) +{ + return itostr3((int)x); +} + +char *itostr2(const uint8_t &x) +{ + //sprintf(conv,"%5.1f",x); + int xx=x; + conv[0]=(xx/10)%10+'0'; + conv[1]=(xx)%10+'0'; + conv[2]=0; + return conv; +} + +// convert float to string with +123.4 format +char *ftostr31(const float &x) +{ + int xx=x*10; + conv[0]=(xx>=0)?'+':'-'; + xx=abs(xx); + conv[1]=(xx/1000)%10+'0'; + conv[2]=(xx/100)%10+'0'; + conv[3]=(xx/10)%10+'0'; + conv[4]='.'; + conv[5]=(xx)%10+'0'; + conv[6]=0; + return conv; +} + +// convert float to string with 123.4 format +char *ftostr31ns(const float &x) +{ + int xx=x*10; + //conv[0]=(xx>=0)?'+':'-'; + xx=abs(xx); + conv[0]=(xx/1000)%10+'0'; + conv[1]=(xx/100)%10+'0'; + conv[2]=(xx/10)%10+'0'; + conv[3]='.'; + conv[4]=(xx)%10+'0'; + conv[5]=0; + return conv; +} + +char *ftostr32(const float &x) +{ + long xx=x*100; + if (xx >= 0) + conv[0]=(xx/10000)%10+'0'; + else + conv[0]='-'; + xx=abs(xx); + conv[1]=(xx/1000)%10+'0'; + conv[2]=(xx/100)%10+'0'; + conv[3]='.'; + conv[4]=(xx/10)%10+'0'; + conv[5]=(xx)%10+'0'; + conv[6]=0; + return conv; +} + +char *itostr31(const int &xx) +{ + conv[0]=(xx>=0)?'+':'-'; + conv[1]=(xx/1000)%10+'0'; + conv[2]=(xx/100)%10+'0'; + conv[3]=(xx/10)%10+'0'; + conv[4]='.'; + conv[5]=(xx)%10+'0'; + conv[6]=0; + return conv; +} + +char *itostr3(const int &xx) +{ + if (xx >= 100) + conv[0]=(xx/100)%10+'0'; + else + conv[0]=' '; + if (xx >= 10) + conv[1]=(xx/10)%10+'0'; + else + conv[1]=' '; + conv[2]=(xx)%10+'0'; + conv[3]=0; + return conv; +} + +char *itostr3left(const int &xx) +{ + if (xx >= 100) + { + conv[0]=(xx/100)%10+'0'; + conv[1]=(xx/10)%10+'0'; + conv[2]=(xx)%10+'0'; + conv[3]=0; + } + else if (xx >= 10) + { + conv[0]=(xx/10)%10+'0'; + conv[1]=(xx)%10+'0'; + conv[2]=0; + } + else + { + conv[0]=(xx)%10+'0'; + conv[1]=0; + } + return conv; +} + +char *itostr4(const int &xx) +{ + if (xx >= 1000) + conv[0]=(xx/1000)%10+'0'; + else + conv[0]=' '; + if (xx >= 100) + conv[1]=(xx/100)%10+'0'; + else + conv[1]=' '; + if (xx >= 10) + conv[2]=(xx/10)%10+'0'; + else + conv[2]=' '; + conv[3]=(xx)%10+'0'; + conv[4]=0; + return conv; +} + +// convert float to string with 12345 format +char *ftostr5(const float &x) +{ + long xx=abs(x); + if (xx >= 10000) + conv[0]=(xx/10000)%10+'0'; + else + conv[0]=' '; + if (xx >= 1000) + conv[1]=(xx/1000)%10+'0'; + else + conv[1]=' '; + if (xx >= 100) + conv[2]=(xx/100)%10+'0'; + else + conv[2]=' '; + if (xx >= 10) + conv[3]=(xx/10)%10+'0'; + else + conv[3]=' '; + conv[4]=(xx)%10+'0'; + conv[5]=0; + return conv; +} + +// convert float to string with +1234.5 format +char *ftostr51(const float &x) +{ + long xx=x*10; + conv[0]=(xx>=0)?'+':'-'; + xx=abs(xx); + conv[1]=(xx/10000)%10+'0'; + conv[2]=(xx/1000)%10+'0'; + conv[3]=(xx/100)%10+'0'; + conv[4]=(xx/10)%10+'0'; + conv[5]='.'; + conv[6]=(xx)%10+'0'; + conv[7]=0; + return conv; +} + +// convert float to string with +123.45 format +char *ftostr52(const float &x) +{ + long xx=x*100; + conv[0]=(xx>=0)?'+':'-'; + xx=abs(xx); + conv[1]=(xx/10000)%10+'0'; + conv[2]=(xx/1000)%10+'0'; + conv[3]=(xx/100)%10+'0'; + conv[4]='.'; + conv[5]=(xx/10)%10+'0'; + conv[6]=(xx)%10+'0'; + conv[7]=0; + return conv; +} + +// Callback for after editing PID i value +// grab the PID i value out of the temp variable; scale it; then update the PID driver +void copy_and_scalePID_i() +{ +#ifdef PIDTEMP + Ki = scalePID_i(raw_Ki); + updatePID(); +#endif +} + +// Callback for after editing PID d value +// grab the PID d value out of the temp variable; scale it; then update the PID driver +void copy_and_scalePID_d() +{ +#ifdef PIDTEMP + Kd = scalePID_d(raw_Kd); + updatePID(); +#endif +} + +#endif //ULTRA_LCD diff --git a/README.md b/README.md index 17e920ec00..5146ef2d74 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ ========================== Marlin 3D Printer Firmware ========================== -[![Coverity Scan Build Status](https://scan.coverity.com/projects/2224/badge.svg)](https://scan.coverity.com/projects/2224) - +[![Coverity Scan Build Status](https://scan.coverity.com/projects/2224/badge.svg)](https://scan.coverity.com/projects/2224) + Marlin has a GPL license because I believe in open development. Please do not use this code in products (3D printers, CNC etc) that are closed source or are crippled by a patent. @@ -47,6 +47,7 @@ Features: * PID tuning * CoreXY kinematics (www.corexy.com/theory.html) * Delta kinematics +* SCARA kinematics * Dual X-carriage support for multiple extruder systems * Configurable serial port to support connection of wireless adaptors. * Automatic operation of extruder/cold-end cooling fans based on nozzle temperature @@ -207,15 +208,15 @@ M Codes * M140 - Set bed target temp * M190 - Sxxx Wait for bed current temp to reach target temp. Waits only when heating * Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling -* M200 D- set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters). +* M200 D- set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters). * M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000) * M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!! * M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec * M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) im mm/sec^2 also sets minimum segment time in ms (B20000) to prevent buffer underruns and M20 minimum feedrate * M205 - advanced settings: minimum travel speed S=while printing T=travel only, B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk, E=maximum E jerk * M206 - set additional homeing offset -* M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop], stays in mm regardless of M200 setting -* M208 - set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/min] +* M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop], stays in mm regardless of M200 setting +* M208 - set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/min] * M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction. * M218 - set hotend offset (in mm): T X Y * M220 S- set speed factor override percentage From fae7d08698bf88eb9264dcf46f6722dd7bb1f091 Mon Sep 17 00:00:00 2001 From: cocktailyogi Date: Mon, 23 Jun 2014 18:16:42 +0200 Subject: [PATCH 2/9] fixed wrong compiler-option --- Marlin/Marlin_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 9164808cd1..65842bd879 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -1251,7 +1251,7 @@ void process_commands() return; } break; -#ifdef SCARA //disable arc support +#ifndef SCARA //disable arc support case 2: // G2 - CW ARC if(Stopped == false) { get_arc_coordinates(); From f0cab611dbb5d9707742e7d46118a99da1b7b6ba Mon Sep 17 00:00:00 2001 From: cocktailyogi Date: Tue, 24 Jun 2014 18:43:36 +0200 Subject: [PATCH 3/9] 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 65842bd879..4e7ed80101 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 b3a8cd1d2b..7fd7c571fb 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 From c51a6f94e01e60984114f0685ff07cfe1585189d Mon Sep 17 00:00:00 2001 From: Pablo Clemente Date: Thu, 12 Jun 2014 18:43:16 +0200 Subject: [PATCH 4/9] Fixed stop print LCD function on M104 --- Marlin/Marlin_main.cpp | 6 ++++-- Marlin/ultralcd.cpp | 3 +++ Marlin/ultralcd.h | 2 ++ 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index c4afca7f68..a9a29dde60 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -1945,14 +1945,16 @@ void process_commands() /* See if we are heating up or cooling down */ target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling + + forced_heating_stop = true; #ifdef TEMP_RESIDENCY_TIME long residencyStart; residencyStart = -1; /* continue to loop until we have reached the target temp _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */ - while((residencyStart == -1) || - (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL))) ) { + while((forced_heating_stop == true)&&((residencyStart == -1) || + (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL)))) ) { #else while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) ) { #endif //TEMP_RESIDENCY_TIME diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index f09dd410d6..4f5aaac998 100644 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -19,6 +19,7 @@ int absPreheatHotendTemp; int absPreheatHPBTemp; int absPreheatFanSpeed; +boolean forced_heating_stop = true ; #ifdef ULTIPANEL static float manual_feedrate[] = MANUAL_FEEDRATE; @@ -256,6 +257,8 @@ static void lcd_sdcard_stop() enquecommand_P(PSTR(SD_FINISHED_RELEASECOMMAND)); } autotempShutdown(); + + forced_heating_stop = false; } /* Menu implementation */ diff --git a/Marlin/ultralcd.h b/Marlin/ultralcd.h index f4570f6a58..b4195f3975 100644 --- a/Marlin/ultralcd.h +++ b/Marlin/ultralcd.h @@ -42,6 +42,8 @@ extern int absPreheatHotendTemp; extern int absPreheatHPBTemp; extern int absPreheatFanSpeed; + + extern boolean forced_heating_stop; void lcd_buzz(long duration,uint16_t freq); bool lcd_clicked(); From f9f54019abe2dd4a9eba7d00a08d07adc0fd52e1 Mon Sep 17 00:00:00 2001 From: Pablo Clemente Date: Fri, 13 Jun 2014 08:39:58 +0200 Subject: [PATCH 5/9] Inverted state logic for forced_heating_stop variable --- Marlin/Marlin_main.cpp | 4 ++-- Marlin/ultralcd.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index a9a29dde60..315a9bb481 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -1946,14 +1946,14 @@ void process_commands() /* See if we are heating up or cooling down */ target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling - forced_heating_stop = true; + forced_heating_stop = false; #ifdef TEMP_RESIDENCY_TIME long residencyStart; residencyStart = -1; /* continue to loop until we have reached the target temp _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */ - while((forced_heating_stop == true)&&((residencyStart == -1) || + while((forced_heating_stop == false)&&((residencyStart == -1) || (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL)))) ) { #else while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) ) { diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index 4f5aaac998..289b84e969 100644 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -19,7 +19,7 @@ int absPreheatHotendTemp; int absPreheatHPBTemp; int absPreheatFanSpeed; -boolean forced_heating_stop = true ; +boolean forced_heating_stop = false ; #ifdef ULTIPANEL static float manual_feedrate[] = MANUAL_FEEDRATE; @@ -258,7 +258,7 @@ static void lcd_sdcard_stop() } autotempShutdown(); - forced_heating_stop = false; + forced_heating_stop = true; } /* Menu implementation */ From 2096188ac3a42ac63d8abe98fe17aa1eda9af9ab Mon Sep 17 00:00:00 2001 From: Pablo Clemente Date: Mon, 30 Jun 2014 15:12:13 +0200 Subject: [PATCH 6/9] Changed the type of variable to bool, the name to "cancel_heatup", flags implementation and added this fix to M190 gcode too. --- Marlin/Marlin_main.cpp | 9 +++++---- Marlin/ultralcd.cpp | 6 +++--- Marlin/ultralcd.h | 2 +- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 315a9bb481..3e44168208 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -1946,14 +1946,14 @@ void process_commands() /* See if we are heating up or cooling down */ target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling - forced_heating_stop = false; + cancel_heatup = false; #ifdef TEMP_RESIDENCY_TIME long residencyStart; residencyStart = -1; /* continue to loop until we have reached the target temp _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */ - while((forced_heating_stop == false)&&((residencyStart == -1) || + while((!cancel_heatup)&&((residencyStart == -1) || (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL)))) ) { #else while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) ) { @@ -2010,10 +2010,11 @@ void process_commands() CooldownNoWait = false; } codenum = millis(); - + + cancel_heatup = false; target_direction = isHeatingBed(); // true if heating, false if cooling - while ( target_direction ? (isHeatingBed()) : (isCoolingBed()&&(CooldownNoWait==false)) ) + while ( (target_direction)&&(!cancel_heatup) ? (isHeatingBed()) : (isCoolingBed()&&(CooldownNoWait==false)) ) { if(( millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up. { diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index 289b84e969..bb7dd0faa8 100644 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -19,7 +19,7 @@ int absPreheatHotendTemp; int absPreheatHPBTemp; int absPreheatFanSpeed; -boolean forced_heating_stop = false ; +bool cancel_heatup = false ; #ifdef ULTIPANEL static float manual_feedrate[] = MANUAL_FEEDRATE; @@ -195,7 +195,7 @@ static void lcd_status_screen() currentMenu = lcd_main_menu; encoderPosition = 0; lcd_quick_feedback(); - lcd_implementation_init(); // to maybe revive the LCD if static electricity killed it. + lcd_implementation_init(); // to maybe revive the LCD if static electricity killed it. } #ifdef ULTIPANEL_FEEDMULTIPLY @@ -258,7 +258,7 @@ static void lcd_sdcard_stop() } autotempShutdown(); - forced_heating_stop = true; + cancel_heatup = true; } /* Menu implementation */ diff --git a/Marlin/ultralcd.h b/Marlin/ultralcd.h index b4195f3975..9bf685805d 100644 --- a/Marlin/ultralcd.h +++ b/Marlin/ultralcd.h @@ -43,7 +43,7 @@ extern int absPreheatHPBTemp; extern int absPreheatFanSpeed; - extern boolean forced_heating_stop; + extern bool cancel_heatup; void lcd_buzz(long duration,uint16_t freq); bool lcd_clicked(); From 50b4e86f75c5168336ca75186d2bb1878ad72179 Mon Sep 17 00:00:00 2001 From: alexborro Date: Mon, 30 Jun 2014 15:22:37 -0300 Subject: [PATCH 7/9] Add "Thermal Runaway Protection" feature This is a feature to protect your printer from burn up in flames if it has a thermistor coming off place (this happened to a friend of mine recently and motivated me writing this feature). The issue: If a thermistor come off, it will read a lower temperature than actual. The system will turn the heater on forever, burning up the filament and anything else around. After the temperature reaches the target for the first time, this feature will start measuring for how long the current temperature stays below the target minus _HYSTERESIS (set_temperature - THERMAL_RUNAWAY_PROTECTION_HYSTERESIS). If it stays longer than _PERIOD, it means the thermistor temperature cannot catch up with the target, so something *may be* wrong. Then, to be on the safe side, the system will he halt. Bear in mind the count down will just start AFTER the first time the thermistor temperature is over the target, so you will have no problem if your extruder heater takes 2 minutes to hit the target on heating. --- Marlin/Configuration.h | 38 +++++++++++++++++++++++ Marlin/temperature.cpp | 68 ++++++++++++++++++++++++++++++++++++++++++ Marlin/temperature.h | 11 +++++++ 3 files changed, 117 insertions(+) diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index 66bf69052d..46d6b96dd1 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -251,6 +251,44 @@ #define EXTRUDE_MINTEMP 170 #define EXTRUDE_MAXLENGTH (X_MAX_LENGTH+Y_MAX_LENGTH) //prevent extrusion of very large distances. +/*================== Thermal Runaway Protection ============================== +This is a feature to protect your printer from burn up in flames if it has +a thermistor coming off place (this happened to a friend of mine recently and +motivated me writing this feature). + +The issue: If a thermistor come off, it will read a lower temperature than actual. +The system will turn the heater on forever, burning up the filament and anything +else around. + +After the temperature reaches the target for the first time, this feature will +start measuring for how long the current temperature stays below the target +minus _HYSTERESIS (set_temperature - THERMAL_RUNAWAY_PROTECTION_HYSTERESIS). + +If it stays longer than _PERIOD, it means the thermistor temperature +cannot catch up with the target, so something *may be* wrong. Then, to be on the +safe side, the system will he halt. + +Bear in mind the count down will just start AFTER the first time the +thermistor temperature is over the target, so you will have no problem if +your extruder heater takes 2 minutes to hit the target on heating. + +*/ +// If you want to enable this feature for all your extruder heaters, +// uncomment the 2 defines below: + +// Parameters for all extruder heaters +//#define THERMAL_RUNAWAY_PROTECTION_PERIOD 40 //in seconds +//#define THERMAL_RUNAWAY_PROTECTION_HYSTERESIS 4 // in degree Celsius + +// If you want to enable this feature for your bed heater, +// uncomment the 2 defines below: + +// Parameters for the bed heater +//#define THERMAL_RUNAWAY_PROTECTION_BED_PERIOD 20 //in seconds +//#define THERMAL_RUNAWAY_PROTECTION_BED_HYSTERESIS 2 // in degree Celsius +//=========================================================================== + + //=========================================================================== //=============================Mechanical Settings=========================== //=========================================================================== diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp index aac6ca66f5..a10c255af9 100644 --- a/Marlin/temperature.cpp +++ b/Marlin/temperature.cpp @@ -416,6 +416,10 @@ void manage_heater() for(int e = 0; e < EXTRUDERS; e++) { + #ifdef THERMAL_RUNAWAY_PROTECTION_PERIOD && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0 + thermal_runaway_protection(&thermal_runaway_state_machine[e], &thermal_runaway_timer[e], current_temperature[e], target_temperature[e], e, THERMAL_RUNAWAY_PROTECTION_PERIOD, THERMAL_RUNAWAY_PROTECTION_HYSTERESIS); + #endif + #ifdef PIDTEMP pid_input = current_temperature[e]; @@ -526,6 +530,10 @@ void manage_heater() #if TEMP_SENSOR_BED != 0 + #ifdef THERMAL_RUNAWAY_PROTECTION_PERIOD && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0 + thermal_runaway_protection(&thermal_runaway_bed_state_machine, &thermal_runaway_bed_timer, current_temperature_bed, target_temperature_bed, 9, THERMAL_RUNAWAY_PROTECTION_BED_PERIOD, THERMAL_RUNAWAY_PROTECTION_BED_HYSTERESIS); + #endif + #ifdef PIDTEMPBED pid_input = current_temperature_bed; @@ -896,6 +904,66 @@ void setWatch() #endif } +#ifdef THERMAL_RUNAWAY_PROTECTION_PERIOD && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0 +void thermal_runaway_protection(int *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc) +{ +/* + SERIAL_ECHO_START; + SERIAL_ECHO("Thermal Thermal Runaway Running. Heater ID:"); + SERIAL_ECHO(heater_id); + SERIAL_ECHO(" ; State:"); + SERIAL_ECHO(*state); + SERIAL_ECHO(" ; Timer:"); + SERIAL_ECHO(*timer); + SERIAL_ECHO(" ; Temperature:"); + SERIAL_ECHO(temperature); + SERIAL_ECHO(" ; Target Temp:"); + SERIAL_ECHO(target_temperature); + SERIAL_ECHOLN(""); +*/ + if ((target_temperature == 0) || thermal_runaway) + { + *state = 0; + *timer = 0; + return; + } + switch (*state) + { + case 0: // "Heater Inactive" state + if (target_temperature > 0) *state = 1; + break; + case 1: // "First Heating" state + if (temperature >= target_temperature) *state = 2; + break; + case 2: // "Temperature Stable" state + if (temperature >= (target_temperature - hysteresis_degc)) + { + *timer = millis(); + } + else if ( (millis() - *timer) > period_seconds*1000) + { + SERIAL_ERROR_START; + SERIAL_ERRORLNPGM("Thermal Runaway, system stopped! Heater_ID: "); + SERIAL_ERRORLN((int)heater_id); + LCD_ALERTMESSAGEPGM("THERMAL RUNAWAY"); + thermal_runaway = true; + while(1) + { + disable_heater(); + disable_x(); + disable_y(); + disable_z(); + disable_e0(); + disable_e1(); + disable_e2(); + manage_heater(); + lcd_update(); + } + } + break; + } +} +#endif void disable_heater() { diff --git a/Marlin/temperature.h b/Marlin/temperature.h index a8580def56..df2b5deacf 100644 --- a/Marlin/temperature.h +++ b/Marlin/temperature.h @@ -154,6 +154,17 @@ void disable_heater(); void setWatch(); void updatePID(); +#ifdef THERMAL_RUNAWAY_PROTECTION_PERIOD && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0 +void thermal_runaway_protection(int *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc); +static int thermal_runaway_state_machine[3]; // = {0,0,0}; +static unsigned long thermal_runaway_timer[3]; // = {0,0,0}; +static bool thermal_runaway = false; + #if TEMP_SENSOR_BED != 0 + static int thermal_runaway_bed_state_machine; + static unsigned long thermal_runaway_bed_timer; + #endif +#endif + FORCE_INLINE void autotempShutdown(){ #ifdef AUTOTEMP if(autotemp_enabled) From 8bf3139bc8dfc46499bab3d6cb685c3c9a663b35 Mon Sep 17 00:00:00 2001 From: Pablo Clemente Date: Tue, 1 Jul 2014 16:45:03 +0200 Subject: [PATCH 8/9] Changed the declaration of the variable to Marlin_main.cpp to fix issue on commit #965 --- Marlin/Marlin_main.cpp | 2 ++ Marlin/ultralcd.cpp | 2 -- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 3e44168208..0367eb46c1 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -276,6 +276,8 @@ int EtoPPressure=0; float delta_segments_per_second= DELTA_SEGMENTS_PER_SECOND; #endif +bool cancel_heatup = false ; + //=========================================================================== //=============================Private Variables============================= //=========================================================================== diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index bb7dd0faa8..18c85887ac 100644 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -19,8 +19,6 @@ int absPreheatHotendTemp; int absPreheatHPBTemp; int absPreheatFanSpeed; -bool cancel_heatup = false ; - #ifdef ULTIPANEL static float manual_feedrate[] = MANUAL_FEEDRATE; #endif // ULTIPANEL From ad3b770c1ac53371bcb952c3588c8618785b0fa4 Mon Sep 17 00:00:00 2001 From: cocktailyogi Date: Sun, 20 Jul 2014 13:55:13 +0200 Subject: [PATCH 9/9] updated examples --- .../SCARA/Configuration.h | 37 +++++++++++++++++++ .../SCARA/Configuration_adv.h | 12 +++--- 2 files changed, 43 insertions(+), 6 deletions(-) diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h index 7fd7c571fb..6b74b0c20b 100644 --- a/Marlin/example_configurations/SCARA/Configuration.h +++ b/Marlin/example_configurations/SCARA/Configuration.h @@ -292,6 +292,43 @@ #define EXTRUDE_MINTEMP 150 #define EXTRUDE_MAXLENGTH (X_MAX_LENGTH+Y_MAX_LENGTH) //prevent extrusion of very large distances. +/*================== Thermal Runaway Protection ============================== +This is a feature to protect your printer from burn up in flames if it has +a thermistor coming off place (this happened to a friend of mine recently and +motivated me writing this feature). + +The issue: If a thermistor come off, it will read a lower temperature than actual. +The system will turn the heater on forever, burning up the filament and anything +else around. + +After the temperature reaches the target for the first time, this feature will +start measuring for how long the current temperature stays below the target +minus _HYSTERESIS (set_temperature - THERMAL_RUNAWAY_PROTECTION_HYSTERESIS). + +If it stays longer than _PERIOD, it means the thermistor temperature +cannot catch up with the target, so something *may be* wrong. Then, to be on the +safe side, the system will he halt. + +Bear in mind the count down will just start AFTER the first time the +thermistor temperature is over the target, so you will have no problem if +your extruder heater takes 2 minutes to hit the target on heating. + +*/ +// If you want to enable this feature for all your extruder heaters, +// uncomment the 2 defines below: + +// Parameters for all extruder heaters +//#define THERMAL_RUNAWAY_PROTECTION_PERIOD 40 //in seconds +//#define THERMAL_RUNAWAY_PROTECTION_HYSTERESIS 4 // in degree Celsius + +// If you want to enable this feature for your bed heater, +// uncomment the 2 defines below: + +// Parameters for the bed heater +//#define THERMAL_RUNAWAY_PROTECTION_BED_PERIOD 20 //in seconds +//#define THERMAL_RUNAWAY_PROTECTION_BED_HYSTERESIS 2 // in degree Celsius +//=========================================================================== + //=========================================================================== //=============================Mechanical Settings=========================== //=========================================================================== diff --git a/Marlin/example_configurations/SCARA/Configuration_adv.h b/Marlin/example_configurations/SCARA/Configuration_adv.h index e500d3d51b..7a01bccb6c 100644 --- a/Marlin/example_configurations/SCARA/Configuration_adv.h +++ b/Marlin/example_configurations/SCARA/Configuration_adv.h @@ -8,7 +8,7 @@ #ifdef BED_LIMIT_SWITCHING #define BED_HYSTERESIS 2 //only disable heating if T>target+BED_HYSTERESIS and enable heating if T>target-BED_HYSTERESIS #endif -#define BED_CHECK_INTERVAL 5000 //ms between checks in bang-bang control +#define BED_CHECK_INTERVAL 3000 //ms between checks in bang-bang control //// Heating sanity check: // This waits for the watch period in milliseconds whenever an M104 or M109 increases the target temperature @@ -47,10 +47,10 @@ // extruder run-out prevention. //if the machine is idle, and the temperature over MINTEMP, every couple of SECONDS some filament is extruded //#define EXTRUDER_RUNOUT_PREVENT -#define EXTRUDER_RUNOUT_MINTEMP 190 +#define EXTRUDER_RUNOUT_MINTEMP 180 #define EXTRUDER_RUNOUT_SECONDS 30. #define EXTRUDER_RUNOUT_ESTEPS 14. //mm filament -#define EXTRUDER_RUNOUT_SPEED 1500. //extrusion speed +#define EXTRUDER_RUNOUT_SPEED 180. //extrusion speed #define EXTRUDER_RUNOUT_EXTRUDE 100 //These defines help to calibrate the AD595 sensor in case you get wrong temperature measurements. @@ -345,13 +345,13 @@ // Hooke's law says: force = k * distance // Bernoulli's principle says: v ^ 2 / 2 + g . h + pressure / density = constant // so: v ^ 2 is proportional to number of steps we advance the extruder -//#define ADVANCE +#define ADVANCE #ifdef ADVANCE #define EXTRUDER_ADVANCE_K .0 #define D_FILAMENT 1.75 - #define STEPS_MM_E 836 + #define STEPS_MM_E 1000 #define EXTRUTION_AREA (0.25 * D_FILAMENT * D_FILAMENT * 3.14159) #define STEPS_PER_CUBIC_MM_E (axis_steps_per_unit[E_AXIS]/ EXTRUTION_AREA) @@ -420,7 +420,7 @@ const unsigned int dropsegments=5; //everything with less than this number of st #ifdef FWRETRACT #define MIN_RETRACT 0.1 //minimum extruded mm to accept a automatic gcode retraction attempt #define RETRACT_LENGTH 3 //default retract length (positive mm) - #define RETRACT_FEEDRATE 30 //default feedrate for retracting (mm/s) + #define RETRACT_FEEDRATE 35 //default feedrate for retracting (mm/s) #define RETRACT_ZLIFT 0 //default retract Z-lift #define RETRACT_RECOVER_LENGTH 0 //default additional recover length (mm, added to retract length when recovering) #define RETRACT_RECOVER_FEEDRATE 8 //default feedrate for recovering from retraction (mm/s)