diff --git a/lib/Marlin/Configuration.h b/lib/Marlin/Configuration.h index aefa066..6066f92 100644 --- a/lib/Marlin/Configuration.h +++ b/lib/Marlin/Configuration.h @@ -131,7 +131,7 @@ //#define SWIFT_TEST_MODE #define HW_VER "3.3" -#define SW_VER_BASE "3.1.17" +#define SW_VER_BASE "3.1.18" #ifdef SWIFT_TEST_MODE #define SW_VER SW_VER_BASE"_t" diff --git a/lib/Marlin/Marlin_main.cpp b/lib/Marlin/Marlin_main.cpp index 4bcc752..627a851 100644 --- a/lib/Marlin/Marlin_main.cpp +++ b/lib/Marlin/Marlin_main.cpp @@ -1720,7 +1720,7 @@ void get_pos_from_polor(float pos[], float polor[]) } -void gcode_get_destination_polor() +uint8_t gcode_get_destination_polor() { float polor_destination[NUM_AXIS]; float current_polor[NUM_AXIS]; @@ -1744,13 +1744,26 @@ void gcode_get_destination_polor() debugPrint("des polor:%f %f %f\r\n", polor_destination[0], polor_destination[1],polor_destination[2]); - get_pos_from_polor(destination, polor_destination); + float test_destination[NUM_AXIS] = {0.0}; + get_pos_from_polor(test_destination, polor_destination); + debugPrint("des: %f, %f, %f\r\n", test_destination[0], test_destination[1], test_destination[2]); + + float angle[3]; + + if (inverse_kinematics(test_destination, angle) != 0) + return E_OUT_OF_RANGE; + + debugPrint("angle: %f, %f, %f\r\n", angle[0], angle[1], angle[2]); + - debugPrint("destination:%f %f %f\r\n", destination[0], destination[1], destination[2]); + LOOP_XYZE(i) + destination[i] = test_destination[i]; + + debugPrint("destination:%f %f %f\r\n", destination[0], destination[1], destination[2]); if (code_seen('F') && code_value_linear_units() > 0.0) feedrate_mm_m = code_value_linear_units(); @@ -1828,9 +1841,14 @@ unsigned char inverse_kinematics(const float in_cartesian[3], float angle[3]) { zIn = (z - MATH_L1) / MATH_LOWER_ARM; - if (x < 0.1) + float s = sqrt(x*x + y*y); + + if (s - get_front_end_offset() < POLAR_MODULE_LENGTH_MIN) + return -1; + + if (x < 0.01) { - x = 0.1; + x = 0.01; } // Calculate value of theta 1: the rotation angle @@ -2955,16 +2973,33 @@ static void homeaxis(AxisEnum axis) { * - Set to current for missing axis codes * - Set the feedrate, if included */ -void gcode_get_destination() { +uint8_t gcode_get_destination() { + + float test_destination[NUM_AXIS] = { 0.0 }; + + LOOP_XYZE(i) { if (code_seen(axis_codes[i])) { - destination[i] = code_value_axis_units(i) + (axis_relative_modes[i] || relative_mode ? current_position[i] : 0); + test_destination[i] = code_value_axis_units(i) + (axis_relative_modes[i] || relative_mode ? current_position[i] : 0); } else - destination[i] = current_position[i]; + test_destination[i] = current_position[i]; } + debugPrint("des: %f, %f, %f\r\n", test_destination[0], test_destination[1], test_destination[2]); + + float angle[3]; + + if (inverse_kinematics(test_destination, angle) != 0) + return E_OUT_OF_RANGE; + + debugPrint("angle: %f, %f, %f\r\n", angle[0], angle[1], angle[2]); + + + LOOP_XYZE(i) + destination[i] = test_destination[i]; + if (code_seen('F') && code_value_linear_units() > 0.0) feedrate_mm_m = code_value_linear_units(); @@ -2977,6 +3012,8 @@ void gcode_get_destination() { #if ENABLED(MIXING_EXTRUDER) && ENABLED(DIRECT_MIXING_IN_G1) gcode_get_mix(); #endif + + return E_OK; } void unknown_command_error() { @@ -3022,9 +3059,12 @@ void unknown_command_error() { /** * G0, G1: Coordinated movement of X Y Z E axes */ -inline void gcode_G0_G1() { +inline uint8_t gcode_G0_G1() { if (IsRunning()) { - gcode_get_destination(); // For X Y Z E F + uint8_t result = gcode_get_destination(); // For X Y Z E F + + if (result != E_OK) + return result; #if ENABLED(FWRETRACT) @@ -7464,13 +7504,15 @@ void process_next_command() { #ifdef UARM_SWIFT uarm_gcode_G0(); #endif - gcode_G0_G1(); + needReply = 1; + result = gcode_G0_G1(); break; case 1: #ifdef UARM_SWIFT uarm_gcode_G1(); #endif - gcode_G0_G1(); + needReply = 1; + result = gcode_G0_G1(); break; // G2, G3 @@ -7570,7 +7612,8 @@ void process_next_command() { break; case 2201: - gcode_get_destination_polor(); + needReply = 1; + result = gcode_get_destination_polor(); break; // rotate stepper motor @@ -7606,7 +7649,8 @@ void process_next_command() { { bool relative_mode_backup = relative_mode; relative_mode = true; - gcode_G0_G1(); + needReply = 1; + result = gcode_G0_G1(); relative_mode = relative_mode_backup; } break; @@ -7615,11 +7659,15 @@ void process_next_command() { { bool relative_mode_backup = relative_mode; relative_mode = true; - gcode_get_destination_polor(); + needReply = 1; + result = gcode_get_destination_polor(); relative_mode = relative_mode_backup; } break; + default: + code_is_good = false; + break; #endif // UARM_SWIFT } @@ -8280,6 +8328,10 @@ void process_next_command() { uarm_gcode_M2500(); break; + default: + code_is_good = false; + break; + #endif //UARM_SWIFT } @@ -8387,6 +8439,11 @@ void process_next_command() { result = uarm_gcode_P2250(replyBuf); break; #endif + + default: + code_is_good = false; + break; + } break; diff --git a/lib/Marlin/uArmParams.h b/lib/Marlin/uArmParams.h index 926f011..6d9ff2c 100644 --- a/lib/Marlin/uArmParams.h +++ b/lib/Marlin/uArmParams.h @@ -114,6 +114,8 @@ #define LOWER_UPPER_MAX_ANGLE 151 #define LOWER_UPPER_MIN_ANGLE 10 +#define POLAR_MODULE_LENGTH_MIN 72 + #define BT_NAME_MAX_LEN 11 diff --git a/lib/Marlin/uArmSwift.cpp b/lib/Marlin/uArmSwift.cpp index 364cacd..add6c63 100644 --- a/lib/Marlin/uArmSwift.cpp +++ b/lib/Marlin/uArmSwift.cpp @@ -295,7 +295,7 @@ void uarm_gcode_G0() - debugPrint("laser off"); + debugPrint("laser off\r\n"); // turn off laser analogWrite(FAN_PIN, 0); diff --git a/update.log b/update.log index 6be553e..67283d4 100644 --- a/update.log +++ b/update.log @@ -1,3 +1,6 @@ +v3.1.18 20170725 +Return E22 when position unreachable + v3.1.17 20170725 Do no switch to absolute mode in G2204/G2205