|
@@ -1323,7 +1323,7 @@ inline bool code_value_bool() { return code_value_byte() > 0; }
|
1323
|
1323
|
float code_value_temp_diff() { return code_value_float(); }
|
1324
|
1324
|
#endif
|
1325
|
1325
|
|
1326
|
|
-inline millis_t code_value_millis() { return code_value_ulong(); }
|
|
1326
|
+FORCE_INLINE millis_t code_value_millis() { return code_value_ulong(); }
|
1327
|
1327
|
inline millis_t code_value_millis_from_seconds() { return code_value_float() * 1000; }
|
1328
|
1328
|
|
1329
|
1329
|
bool code_seen(char code) {
|
|
@@ -3219,7 +3219,6 @@ inline void gcode_G28() {
|
3219
|
3219
|
}
|
3220
|
3220
|
|
3221
|
3221
|
int8_t px, py;
|
3222
|
|
- float z;
|
3223
|
3222
|
|
3224
|
3223
|
switch (state) {
|
3225
|
3224
|
case MeshReport:
|
|
@@ -3317,24 +3316,22 @@ inline void gcode_G28() {
|
3317
|
3316
|
return;
|
3318
|
3317
|
}
|
3319
|
3318
|
if (code_seen('Z')) {
|
3320
|
|
- z = code_value_axis_units(Z_AXIS);
|
|
3319
|
+ mbl.z_values[py][px] = code_value_axis_units(Z_AXIS);
|
3321
|
3320
|
}
|
3322
|
3321
|
else {
|
3323
|
3322
|
SERIAL_PROTOCOLLNPGM("Z not entered.");
|
3324
|
3323
|
return;
|
3325
|
3324
|
}
|
3326
|
|
- mbl.z_values[py][px] = z;
|
3327
|
3325
|
break;
|
3328
|
3326
|
|
3329
|
3327
|
case MeshSetZOffset:
|
3330
|
3328
|
if (code_seen('Z')) {
|
3331
|
|
- z = code_value_axis_units(Z_AXIS);
|
|
3329
|
+ mbl.z_offset = code_value_axis_units(Z_AXIS);
|
3332
|
3330
|
}
|
3333
|
3331
|
else {
|
3334
|
3332
|
SERIAL_PROTOCOLLNPGM("Z not entered.");
|
3335
|
3333
|
return;
|
3336
|
3334
|
}
|
3337
|
|
- mbl.z_offset = z;
|
3338
|
3335
|
break;
|
3339
|
3336
|
|
3340
|
3337
|
case MeshReset:
|
|
@@ -3866,15 +3863,12 @@ inline void gcode_G92() {
|
3866
|
3863
|
#if ENABLED(ULTIPANEL)
|
3867
|
3864
|
|
3868
|
3865
|
/**
|
3869
|
|
- * M0: // M0 - Unconditional stop - Wait for user button press on LCD
|
3870
|
|
- * M1: // M1 - Conditional stop - Wait for user button press on LCD
|
|
3866
|
+ * M0: Unconditional stop - Wait for user button press on LCD
|
|
3867
|
+ * M1: Conditional stop - Wait for user button press on LCD
|
3871
|
3868
|
*/
|
3872
|
3869
|
inline void gcode_M0_M1() {
|
3873
|
3870
|
char* args = current_command_args;
|
3874
|
3871
|
|
3875
|
|
- uint8_t test_value = 12;
|
3876
|
|
- SERIAL_ECHOPAIR("TEST", test_value);
|
3877
|
|
-
|
3878
|
3872
|
millis_t codenum = 0;
|
3879
|
3873
|
bool hasP = false, hasS = false;
|
3880
|
3874
|
if (code_seen('P')) {
|
|
@@ -4096,35 +4090,34 @@ inline void gcode_M31() {
|
4096
|
4090
|
* S<byte> Pin status from 0 - 255
|
4097
|
4091
|
*/
|
4098
|
4092
|
inline void gcode_M42() {
|
4099
|
|
- if (code_seen('S')) {
|
4100
|
|
- int pin_status = code_value_int();
|
4101
|
|
- if (pin_status < 0 || pin_status > 255) return;
|
|
4093
|
+ if (!code_seen('S')) return;
|
4102
|
4094
|
|
4103
|
|
- int pin_number = code_seen('P') ? code_value_int() : LED_PIN;
|
4104
|
|
- if (pin_number < 0) return;
|
|
4095
|
+ int pin_status = code_value_int();
|
|
4096
|
+ if (pin_status < 0 || pin_status > 255) return;
|
4105
|
4097
|
|
4106
|
|
- for (uint8_t i = 0; i < COUNT(sensitive_pins); i++)
|
4107
|
|
- if (pin_number == sensitive_pins[i]) return;
|
|
4098
|
+ int pin_number = code_seen('P') ? code_value_int() : LED_PIN;
|
|
4099
|
+ if (pin_number < 0) return;
|
4108
|
4100
|
|
4109
|
|
- pinMode(pin_number, OUTPUT);
|
4110
|
|
- digitalWrite(pin_number, pin_status);
|
4111
|
|
- analogWrite(pin_number, pin_status);
|
|
4101
|
+ for (uint8_t i = 0; i < COUNT(sensitive_pins); i++)
|
|
4102
|
+ if (pin_number == sensitive_pins[i]) return;
|
4112
|
4103
|
|
4113
|
|
- #if FAN_COUNT > 0
|
4114
|
|
- switch (pin_number) {
|
4115
|
|
- #if HAS_FAN0
|
4116
|
|
- case FAN_PIN: fanSpeeds[0] = pin_status; break;
|
4117
|
|
- #endif
|
4118
|
|
- #if HAS_FAN1
|
4119
|
|
- case FAN1_PIN: fanSpeeds[1] = pin_status; break;
|
4120
|
|
- #endif
|
4121
|
|
- #if HAS_FAN2
|
4122
|
|
- case FAN2_PIN: fanSpeeds[2] = pin_status; break;
|
4123
|
|
- #endif
|
4124
|
|
- }
|
4125
|
|
- #endif
|
|
4104
|
+ pinMode(pin_number, OUTPUT);
|
|
4105
|
+ digitalWrite(pin_number, pin_status);
|
|
4106
|
+ analogWrite(pin_number, pin_status);
|
4126
|
4107
|
|
4127
|
|
- } // code_seen('S')
|
|
4108
|
+ #if FAN_COUNT > 0
|
|
4109
|
+ switch (pin_number) {
|
|
4110
|
+ #if HAS_FAN0
|
|
4111
|
+ case FAN_PIN: fanSpeeds[0] = pin_status; break;
|
|
4112
|
+ #endif
|
|
4113
|
+ #if HAS_FAN1
|
|
4114
|
+ case FAN1_PIN: fanSpeeds[1] = pin_status; break;
|
|
4115
|
+ #endif
|
|
4116
|
+ #if HAS_FAN2
|
|
4117
|
+ case FAN2_PIN: fanSpeeds[2] = pin_status; break;
|
|
4118
|
+ #endif
|
|
4119
|
+ }
|
|
4120
|
+ #endif
|
4128
|
4121
|
}
|
4129
|
4122
|
|
4130
|
4123
|
#if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
|
|
@@ -5534,11 +5527,9 @@ inline void gcode_M220() {
|
5534
|
5527
|
* M221: Set extrusion percentage (M221 T0 S95)
|
5535
|
5528
|
*/
|
5536
|
5529
|
inline void gcode_M221() {
|
5537
|
|
- if (code_seen('S')) {
|
5538
|
|
- int sval = code_value_int();
|
5539
|
|
- if (get_target_extruder_from_command(221)) return;
|
5540
|
|
- extruder_multiplier[target_extruder] = sval;
|
5541
|
|
- }
|
|
5530
|
+ if (get_target_extruder_from_command(221)) return;
|
|
5531
|
+ if (code_seen('S'))
|
|
5532
|
+ extruder_multiplier[target_extruder] = code_value_int();
|
5542
|
5533
|
}
|
5543
|
5534
|
|
5544
|
5535
|
/**
|
|
@@ -5590,28 +5581,27 @@ inline void gcode_M226() {
|
5590
|
5581
|
#if HAS_SERVOS
|
5591
|
5582
|
|
5592
|
5583
|
/**
|
5593
|
|
- * M280: Get or set servo position. P<index> S<angle>
|
|
5584
|
+ * M280: Get or set servo position. P<index> [S<angle>]
|
5594
|
5585
|
*/
|
5595
|
5586
|
inline void gcode_M280() {
|
5596
|
|
- int servo_index = code_seen('P') ? code_value_int() : -1;
|
5597
|
|
- int servo_position = 0;
|
5598
|
|
- if (code_seen('S')) {
|
5599
|
|
- servo_position = code_value_int();
|
5600
|
|
- if (servo_index >= 0 && servo_index < NUM_SERVOS)
|
5601
|
|
- MOVE_SERVO(servo_index, servo_position);
|
|
5587
|
+ if (!code_seen('P')) return;
|
|
5588
|
+ int servo_index = code_value_int();
|
|
5589
|
+ if (servo_index >= 0 && servo_index < NUM_SERVOS) {
|
|
5590
|
+ if (code_seen('S'))
|
|
5591
|
+ MOVE_SERVO(servo_index, code_value_int());
|
5602
|
5592
|
else {
|
5603
|
|
- SERIAL_ERROR_START;
|
5604
|
|
- SERIAL_ERROR("Servo ");
|
5605
|
|
- SERIAL_ERROR(servo_index);
|
5606
|
|
- SERIAL_ERRORLN(" out of range");
|
|
5593
|
+ SERIAL_ECHO_START;
|
|
5594
|
+ SERIAL_ECHOPGM(" Servo ");
|
|
5595
|
+ SERIAL_ECHO(servo_index);
|
|
5596
|
+ SERIAL_ECHOPGM(": ");
|
|
5597
|
+ SERIAL_ECHOLN(servo[servo_index].read());
|
5607
|
5598
|
}
|
5608
|
5599
|
}
|
5609
|
|
- else if (servo_index >= 0) {
|
5610
|
|
- SERIAL_ECHO_START;
|
5611
|
|
- SERIAL_ECHOPGM(" Servo ");
|
5612
|
|
- SERIAL_ECHO(servo_index);
|
5613
|
|
- SERIAL_ECHOPGM(": ");
|
5614
|
|
- SERIAL_ECHOLN(servo[servo_index].read());
|
|
5600
|
+ else {
|
|
5601
|
+ SERIAL_ERROR_START;
|
|
5602
|
+ SERIAL_ERROR("Servo ");
|
|
5603
|
+ SERIAL_ERROR(servo_index);
|
|
5604
|
+ SERIAL_ERRORLN(" out of range");
|
5615
|
5605
|
}
|
5616
|
5606
|
}
|
5617
|
5607
|
|
|
@@ -5864,11 +5854,9 @@ inline void gcode_M303() {
|
5864
|
5854
|
* M365: SCARA calibration: Scaling factor, X, Y, Z axis
|
5865
|
5855
|
*/
|
5866
|
5856
|
inline void gcode_M365() {
|
5867
|
|
- for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
|
5868
|
|
- if (code_seen(axis_codes[i])) {
|
|
5857
|
+ for (int8_t i = X_AXIS; i <= Z_AXIS; i++)
|
|
5858
|
+ if (code_seen(axis_codes[i]))
|
5869
|
5859
|
axis_scaling[i] = code_value_float();
|
5870
|
|
- }
|
5871
|
|
- }
|
5872
|
5860
|
}
|
5873
|
5861
|
|
5874
|
5862
|
#endif // SCARA
|