General cleanup, code reduction in Marlin_main.cpp
This commit is contained in:
parent
e282d69f63
commit
a8538bd7ce
|
@ -1323,7 +1323,7 @@ inline bool code_value_bool() { return code_value_byte() > 0; }
|
||||||
float code_value_temp_diff() { return code_value_float(); }
|
float code_value_temp_diff() { return code_value_float(); }
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
inline millis_t code_value_millis() { return code_value_ulong(); }
|
FORCE_INLINE millis_t code_value_millis() { return code_value_ulong(); }
|
||||||
inline millis_t code_value_millis_from_seconds() { return code_value_float() * 1000; }
|
inline millis_t code_value_millis_from_seconds() { return code_value_float() * 1000; }
|
||||||
|
|
||||||
bool code_seen(char code) {
|
bool code_seen(char code) {
|
||||||
|
@ -3219,7 +3219,6 @@ inline void gcode_G28() {
|
||||||
}
|
}
|
||||||
|
|
||||||
int8_t px, py;
|
int8_t px, py;
|
||||||
float z;
|
|
||||||
|
|
||||||
switch (state) {
|
switch (state) {
|
||||||
case MeshReport:
|
case MeshReport:
|
||||||
|
@ -3317,24 +3316,22 @@ inline void gcode_G28() {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if (code_seen('Z')) {
|
if (code_seen('Z')) {
|
||||||
z = code_value_axis_units(Z_AXIS);
|
mbl.z_values[py][px] = code_value_axis_units(Z_AXIS);
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
SERIAL_PROTOCOLLNPGM("Z not entered.");
|
SERIAL_PROTOCOLLNPGM("Z not entered.");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
mbl.z_values[py][px] = z;
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MeshSetZOffset:
|
case MeshSetZOffset:
|
||||||
if (code_seen('Z')) {
|
if (code_seen('Z')) {
|
||||||
z = code_value_axis_units(Z_AXIS);
|
mbl.z_offset = code_value_axis_units(Z_AXIS);
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
SERIAL_PROTOCOLLNPGM("Z not entered.");
|
SERIAL_PROTOCOLLNPGM("Z not entered.");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
mbl.z_offset = z;
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MeshReset:
|
case MeshReset:
|
||||||
|
@ -3866,15 +3863,12 @@ inline void gcode_G92() {
|
||||||
#if ENABLED(ULTIPANEL)
|
#if ENABLED(ULTIPANEL)
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* M0: // M0 - Unconditional stop - Wait for user button press on LCD
|
* M0: Unconditional stop - Wait for user button press on LCD
|
||||||
* M1: // M1 - Conditional stop - Wait for user button press on LCD
|
* M1: Conditional stop - Wait for user button press on LCD
|
||||||
*/
|
*/
|
||||||
inline void gcode_M0_M1() {
|
inline void gcode_M0_M1() {
|
||||||
char* args = current_command_args;
|
char* args = current_command_args;
|
||||||
|
|
||||||
uint8_t test_value = 12;
|
|
||||||
SERIAL_ECHOPAIR("TEST", test_value);
|
|
||||||
|
|
||||||
millis_t codenum = 0;
|
millis_t codenum = 0;
|
||||||
bool hasP = false, hasS = false;
|
bool hasP = false, hasS = false;
|
||||||
if (code_seen('P')) {
|
if (code_seen('P')) {
|
||||||
|
@ -4096,35 +4090,34 @@ inline void gcode_M31() {
|
||||||
* S<byte> Pin status from 0 - 255
|
* S<byte> Pin status from 0 - 255
|
||||||
*/
|
*/
|
||||||
inline void gcode_M42() {
|
inline void gcode_M42() {
|
||||||
if (code_seen('S')) {
|
if (!code_seen('S')) return;
|
||||||
int pin_status = code_value_int();
|
|
||||||
if (pin_status < 0 || pin_status > 255) return;
|
|
||||||
|
|
||||||
int pin_number = code_seen('P') ? code_value_int() : LED_PIN;
|
int pin_status = code_value_int();
|
||||||
if (pin_number < 0) return;
|
if (pin_status < 0 || pin_status > 255) return;
|
||||||
|
|
||||||
for (uint8_t i = 0; i < COUNT(sensitive_pins); i++)
|
int pin_number = code_seen('P') ? code_value_int() : LED_PIN;
|
||||||
if (pin_number == sensitive_pins[i]) return;
|
if (pin_number < 0) return;
|
||||||
|
|
||||||
pinMode(pin_number, OUTPUT);
|
for (uint8_t i = 0; i < COUNT(sensitive_pins); i++)
|
||||||
digitalWrite(pin_number, pin_status);
|
if (pin_number == sensitive_pins[i]) return;
|
||||||
analogWrite(pin_number, pin_status);
|
|
||||||
|
|
||||||
#if FAN_COUNT > 0
|
pinMode(pin_number, OUTPUT);
|
||||||
switch (pin_number) {
|
digitalWrite(pin_number, pin_status);
|
||||||
#if HAS_FAN0
|
analogWrite(pin_number, pin_status);
|
||||||
case FAN_PIN: fanSpeeds[0] = pin_status; break;
|
|
||||||
#endif
|
|
||||||
#if HAS_FAN1
|
|
||||||
case FAN1_PIN: fanSpeeds[1] = pin_status; break;
|
|
||||||
#endif
|
|
||||||
#if HAS_FAN2
|
|
||||||
case FAN2_PIN: fanSpeeds[2] = pin_status; break;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
} // code_seen('S')
|
#if FAN_COUNT > 0
|
||||||
|
switch (pin_number) {
|
||||||
|
#if HAS_FAN0
|
||||||
|
case FAN_PIN: fanSpeeds[0] = pin_status; break;
|
||||||
|
#endif
|
||||||
|
#if HAS_FAN1
|
||||||
|
case FAN1_PIN: fanSpeeds[1] = pin_status; break;
|
||||||
|
#endif
|
||||||
|
#if HAS_FAN2
|
||||||
|
case FAN2_PIN: fanSpeeds[2] = pin_status; break;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
#if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
|
#if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
|
||||||
|
@ -5534,11 +5527,9 @@ inline void gcode_M220() {
|
||||||
* M221: Set extrusion percentage (M221 T0 S95)
|
* M221: Set extrusion percentage (M221 T0 S95)
|
||||||
*/
|
*/
|
||||||
inline void gcode_M221() {
|
inline void gcode_M221() {
|
||||||
if (code_seen('S')) {
|
if (get_target_extruder_from_command(221)) return;
|
||||||
int sval = code_value_int();
|
if (code_seen('S'))
|
||||||
if (get_target_extruder_from_command(221)) return;
|
extruder_multiplier[target_extruder] = code_value_int();
|
||||||
extruder_multiplier[target_extruder] = sval;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -5590,28 +5581,27 @@ inline void gcode_M226() {
|
||||||
#if HAS_SERVOS
|
#if HAS_SERVOS
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* M280: Get or set servo position. P<index> S<angle>
|
* M280: Get or set servo position. P<index> [S<angle>]
|
||||||
*/
|
*/
|
||||||
inline void gcode_M280() {
|
inline void gcode_M280() {
|
||||||
int servo_index = code_seen('P') ? code_value_int() : -1;
|
if (!code_seen('P')) return;
|
||||||
int servo_position = 0;
|
int servo_index = code_value_int();
|
||||||
if (code_seen('S')) {
|
if (servo_index >= 0 && servo_index < NUM_SERVOS) {
|
||||||
servo_position = code_value_int();
|
if (code_seen('S'))
|
||||||
if (servo_index >= 0 && servo_index < NUM_SERVOS)
|
MOVE_SERVO(servo_index, code_value_int());
|
||||||
MOVE_SERVO(servo_index, servo_position);
|
|
||||||
else {
|
else {
|
||||||
SERIAL_ERROR_START;
|
SERIAL_ECHO_START;
|
||||||
SERIAL_ERROR("Servo ");
|
SERIAL_ECHOPGM(" Servo ");
|
||||||
SERIAL_ERROR(servo_index);
|
SERIAL_ECHO(servo_index);
|
||||||
SERIAL_ERRORLN(" out of range");
|
SERIAL_ECHOPGM(": ");
|
||||||
|
SERIAL_ECHOLN(servo[servo_index].read());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if (servo_index >= 0) {
|
else {
|
||||||
SERIAL_ECHO_START;
|
SERIAL_ERROR_START;
|
||||||
SERIAL_ECHOPGM(" Servo ");
|
SERIAL_ERROR("Servo ");
|
||||||
SERIAL_ECHO(servo_index);
|
SERIAL_ERROR(servo_index);
|
||||||
SERIAL_ECHOPGM(": ");
|
SERIAL_ERRORLN(" out of range");
|
||||||
SERIAL_ECHOLN(servo[servo_index].read());
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -5864,11 +5854,9 @@ inline void gcode_M303() {
|
||||||
* M365: SCARA calibration: Scaling factor, X, Y, Z axis
|
* M365: SCARA calibration: Scaling factor, X, Y, Z axis
|
||||||
*/
|
*/
|
||||||
inline void gcode_M365() {
|
inline void gcode_M365() {
|
||||||
for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
|
for (int8_t i = X_AXIS; i <= Z_AXIS; i++)
|
||||||
if (code_seen(axis_codes[i])) {
|
if (code_seen(axis_codes[i]))
|
||||||
axis_scaling[i] = code_value_float();
|
axis_scaling[i] = code_value_float();
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // SCARA
|
#endif // SCARA
|
||||||
|
|
Loading…
Reference in a new issue