|
|
@ -379,7 +379,7 @@ bool target_direction; |
|
|
|
#endif |
|
|
|
|
|
|
|
#if NUM_SERVOS > 0 |
|
|
|
Servo servos[NUM_SERVOS]; |
|
|
|
Servo servo[NUM_SERVOS]; |
|
|
|
#endif |
|
|
|
|
|
|
|
#ifdef CHDK |
|
|
@ -534,28 +534,28 @@ void suicide() { |
|
|
|
|
|
|
|
void servo_init() { |
|
|
|
#if NUM_SERVOS >= 1 && HAS_SERVO_0 |
|
|
|
servos[0].attach(SERVO0_PIN); |
|
|
|
servo[0].attach(SERVO0_PIN); |
|
|
|
#endif |
|
|
|
#if NUM_SERVOS >= 2 && HAS_SERVO_1 |
|
|
|
servos[1].attach(SERVO1_PIN); |
|
|
|
servo[1].attach(SERVO1_PIN); |
|
|
|
#endif |
|
|
|
#if NUM_SERVOS >= 3 && HAS_SERVO_2 |
|
|
|
servos[2].attach(SERVO2_PIN); |
|
|
|
servo[2].attach(SERVO2_PIN); |
|
|
|
#endif |
|
|
|
#if NUM_SERVOS >= 4 && HAS_SERVO_3 |
|
|
|
servos[3].attach(SERVO3_PIN); |
|
|
|
servo[3].attach(SERVO3_PIN); |
|
|
|
#endif |
|
|
|
|
|
|
|
// Set position of Servo Endstops that are defined
|
|
|
|
#ifdef SERVO_ENDSTOPS |
|
|
|
for (int i = 0; i < 3; i++) |
|
|
|
if (servo_endstops[i] >= 0) |
|
|
|
servos[servo_endstops[i]].write(servo_endstop_angles[i * 2 + 1]); |
|
|
|
servo[servo_endstops[i]].write(servo_endstop_angles[i * 2 + 1]); |
|
|
|
#endif |
|
|
|
|
|
|
|
#if SERVO_LEVELING |
|
|
|
delay(PROBE_SERVO_DEACTIVATION_DELAY); |
|
|
|
servos[servo_endstops[Z_AXIS]].detach(); |
|
|
|
servo[servo_endstops[Z_AXIS]].detach(); |
|
|
|
#endif |
|
|
|
} |
|
|
|
|
|
|
@ -889,7 +889,7 @@ void get_command() { |
|
|
|
} |
|
|
|
|
|
|
|
bool code_has_value() { |
|
|
|
char c = *(strchr_pointer + 1); |
|
|
|
char c = strchr_pointer[1]; |
|
|
|
return (c >= '0' && c <= '9') || c == '-' || c == '+' || c == '.'; |
|
|
|
} |
|
|
|
|
|
|
@ -1255,12 +1255,12 @@ inline void set_destination_to_current() { memcpy(destination, current_position, |
|
|
|
// Engage Z Servo endstop if enabled
|
|
|
|
if (servo_endstops[Z_AXIS] >= 0) { |
|
|
|
#if SERVO_LEVELING |
|
|
|
servos[servo_endstops[Z_AXIS]].attach(0); |
|
|
|
servo[servo_endstops[Z_AXIS]].attach(0); |
|
|
|
#endif |
|
|
|
servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]); |
|
|
|
servo[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]); |
|
|
|
#if SERVO_LEVELING |
|
|
|
delay(PROBE_SERVO_DEACTIVATION_DELAY); |
|
|
|
servos[servo_endstops[Z_AXIS]].detach(); |
|
|
|
servo[servo_endstops[Z_AXIS]].detach(); |
|
|
|
#endif |
|
|
|
} |
|
|
|
|
|
|
@ -1319,14 +1319,14 @@ inline void set_destination_to_current() { memcpy(destination, current_position, |
|
|
|
#endif |
|
|
|
|
|
|
|
#if SERVO_LEVELING |
|
|
|
servos[servo_endstops[Z_AXIS]].attach(0); |
|
|
|
servo[servo_endstops[Z_AXIS]].attach(0); |
|
|
|
#endif |
|
|
|
|
|
|
|
servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]); |
|
|
|
servo[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]); |
|
|
|
|
|
|
|
#if SERVO_LEVELING |
|
|
|
delay(PROBE_SERVO_DEACTIVATION_DELAY); |
|
|
|
servos[servo_endstops[Z_AXIS]].detach(); |
|
|
|
servo[servo_endstops[Z_AXIS]].detach(); |
|
|
|
#endif |
|
|
|
} |
|
|
|
|
|
|
@ -1520,7 +1520,7 @@ static void homeaxis(AxisEnum axis) { |
|
|
|
#endif |
|
|
|
{ |
|
|
|
if (servo_endstops[axis] > -1) |
|
|
|
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]); |
|
|
|
servo[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]); |
|
|
|
} |
|
|
|
|
|
|
|
#endif // SERVO_ENDSTOPS && !Z_PROBE_SLED
|
|
|
@ -1598,7 +1598,7 @@ static void homeaxis(AxisEnum axis) { |
|
|
|
// Retract Servo endstop if enabled
|
|
|
|
#ifdef SERVO_ENDSTOPS |
|
|
|
if (servo_endstops[axis] > -1) |
|
|
|
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]); |
|
|
|
servo[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]); |
|
|
|
#endif |
|
|
|
|
|
|
|
#if SERVO_LEVELING && !defined(Z_PROBE_SLED) |
|
|
@ -3990,12 +3990,12 @@ inline void gcode_M226() { |
|
|
|
servo_position = code_value(); |
|
|
|
if ((servo_index >= 0) && (servo_index < NUM_SERVOS)) { |
|
|
|
#if SERVO_LEVELING |
|
|
|
servos[servo_index].attach(0); |
|
|
|
servo[servo_index].attach(0); |
|
|
|
#endif |
|
|
|
servos[servo_index].write(servo_position); |
|
|
|
servo[servo_index].write(servo_position); |
|
|
|
#if SERVO_LEVELING |
|
|
|
delay(PROBE_SERVO_DEACTIVATION_DELAY); |
|
|
|
servos[servo_index].detach(); |
|
|
|
servo[servo_index].detach(); |
|
|
|
#endif |
|
|
|
} |
|
|
|
else { |
|
|
@ -4010,7 +4010,7 @@ inline void gcode_M226() { |
|
|
|
SERIAL_PROTOCOL(" Servo "); |
|
|
|
SERIAL_PROTOCOL(servo_index); |
|
|
|
SERIAL_PROTOCOL(": "); |
|
|
|
SERIAL_PROTOCOL(servos[servo_index].read()); |
|
|
|
SERIAL_PROTOCOL(servo[servo_index].read()); |
|
|
|
SERIAL_EOL; |
|
|
|
} |
|
|
|
} |
|
|
|