diff --git a/main.cpp b/main.cpp deleted file mode 100644 index 6ddad2ef6..000000000 --- a/main.cpp +++ /dev/null @@ -1,551 +0,0 @@ -//! @file - -#include "main.h" -#include -#include -#include -#include -#include "shr16.h" -#include "adc.h" -#include "uart.h" -#include "spi.h" -#include "tmc2130.h" -#include "abtn3.h" -#include "mmctl.h" -#include "motion.h" -#include "Buttons.h" -#include -#include "permanent_storage.h" - - -// public variables: -int8_t sys_state = 0; -uint8_t sys_signals = 0; -bool fsensor_triggered = false; -bool unloadatBoot = false; -bool mmuFSensorLoading = false; -bool duplicateTCmd = false; -bool load_filament_at_toolChange = false; - -uint8_t tmc2130_mode = NORMAL_MODE; // STEALTH_MODE; - -static char echo[32]; - -#if (UART_COM == 0) -FILE *uart_com = uart0io; -#elif (UART_COM == 1) -FILE *uart_com = uart1io; -#endif //(UART_COM == 0) - -extern "C" void process_commands(FILE *inout); - -//! @brief Initialization after reset -//! -//! button | action -//! ------ | ------ -//! middle | enter setup -//! right | continue after error -//! -//! LED indication of states -//! -//! RG | RG | RG | RG | RG | meaning -//! -- | -- | -- | -- | -- | ------------------------ -//! 00 | 00 | 00 | 00 | 0b | Shift register initialized -//! 00 | 00 | 00 | 0b | 00 | uart initialized -//! 00 | 00 | 0b | 00 | 00 | spi initialized -//! 00 | 0b | 00 | 00 | 00 | tmc2130 initialized -//! 0b | 00 | 00 | 00 | 00 | A/D converter initialized -//! b0 | b0 | b0 | b0 | b0 | Error, filament detected, still present -//! 0b | 0b | 0b | 0b | 0b | Error, filament detected, no longer present, continue by right button click -//! -//! @n R - Red LED -//! @n G - Green LED -//! @n 1 - active -//! @n 0 - inactive -//! @n b - blinking -void setup() -{ - - shr16_init(); // shift register - led_blink(0); - delay(1000); // wait for boot ok printer - - uart0_init(); //uart0 - uart1_init(); //uart1 - led_blink(1); - - -#if (UART_STD == 0) - stdin = uart0io; // stdin = uart0 - stdout = uart0io; // stdout = uart0 -#elif(UART_STD == 1) - stdin = uart1io; // stdin = uart1 - stdout = uart1io; // stdout = uart1 -#endif //(UART_STD == 1) - - - bool requestMenu = false; - - fprintf_P(uart_com, PSTR("start\n")); //startup message - - spi_init(); - led_blink(2); - - //tmc2130_init(HOMING_MODE); // trinamic, homing - led_blink(3); - - - adc_init(); // ADC - led_blink(4); - - init_Pulley(); - - if (buttonClicked() == Btn::middle) { - requestMenu = true; - } - - // if FINDA is sensing filament do not home - while (digitalRead(A1)) { - while (Btn::right != buttonClicked()) { - if (digitalRead(A1)) { - shr16_set_led(0x2aa); - } else { - shr16_set_led(0x155); - } - delay(300); - shr16_set_led(0x000); - delay(300); - } - } - - home(); - // TODO 2: add reading previously stored mode (stealth/normal) from eeprom - - //tmc2130_init(tmc2130_mode); // trinamic, initialize all axes - - - // check if to goto the settings menu - if (requestMenu) { - setupMenu(); - } -} - -//! @brief Select filament menu -//! -//! Select filament by pushing left and right button, park position can be also selected. -//! -//! button | action -//! ------ | ------ -//! left | select previous filament -//! right | select next filament -//! -//! LED indication of states -//! -//! RG | RG | RG | RG | RG | meaning -//! -- | -- | -- | -- | -- | ------------------------ -//! 01 | 00 | 00 | 00 | 00 | filament 1 -//! 00 | 01 | 00 | 00 | 00 | filament 2 -//! 00 | 00 | 01 | 00 | 00 | filament 3 -//! 00 | 00 | 00 | 01 | 00 | filament 4 -//! 00 | 00 | 00 | 00 | 01 | filament 5 -//! 00 | 00 | 00 | 00 | bb | park position -//! -//! @n R - Red LED -//! @n G - Green LED -//! @n 1 - active -//! @n 0 - inactive -//! @n b - blinking -void manual_extruder_selector() -{ - shr16_set_led(1 << 2 * (4 - active_extruder)); - -#ifdef TESTING_STEALTH - if (buttonClicked() != Btn::none) { - switch (buttonClicked()) { - case Btn::right: - if (active_extruder < EXTRUDERS) { - select_extruder(active_extruder + 1); - } - break; - case Btn::left: - if (active_extruder > 0) { - select_extruder(active_extruder - 1); - } - break; - default: - break; - } - } -#else - if ((Btn::left | Btn::right) & buttonClicked()) { - switch (buttonClicked()) { - case Btn::right: - if (active_extruder < EXTRUDERS) { - select_extruder(active_extruder + 1); - } - break; - case Btn::left: - if (active_extruder > 0) { - select_extruder(active_extruder - 1); - } - break; - default: - break; - } - } -#endif - - if (active_extruder == 5) { - shr16_set_led(2 << 2 * 0); - delay(50); - shr16_set_led(1 << 2 * 0); - delay(50); - } -} - - -//! @brief main loop -//! -//! It is possible to manually select filament and feed it when not printing. -//! -//! button | action -//! ------ | ------ -//! middle | feed filament -//! -//! @copydoc manual_extruder_selector() -void loop() -{ - process_commands(uart_com); - - if (!isPrinting) { - manual_extruder_selector(); -#ifndef TESTING_STEALTH - if (Btn::middle == buttonClicked() && active_extruder < 5) { - shr16_set_led(2 << 2 * (4 - active_extruder)); - if (Btn::middle == buttonClicked()) { - feed_filament(); - } - } - } -#endif -} - -extern "C" { - void process_commands(FILE *inout) - { - static char line[32]; - static int count = 0; - int c = -1; - if (count < 32) { - if ((c = getc(inout)) >= 0) { - if (c == '\r') { - c = 0; - } - if (c == '\n') { - c = 0; - } - line[count++] = c; - } - } else { - count = 0; - //overflow - } - int value = 0; - int value0 = 0; - - if ((count > 0) && (c == 0)) { - //line received - //printf_P(PSTR("line received: '%s' %d\n"), line, count); - if (strstr(line, "EE") != NULL) { - for (int i = 0; i < 32; i++) { - line[i] = echo[i]; - } - count = 0; - } else if (sscanf_P(line, PSTR("P%d"), &value) > 0) { - if (value == 0) { // Read finda - fprintf_P(inout, PSTR("%dok\n"), digitalRead(A1)); - } - } else { //if (strstr(line, "P0") == NULL) { - for (int i = 0; i < 32; i++) { - echo[i] = line[i]; - } - count = 0; - //delay(10); // delay so MK3 comms is not floaded - if (strstr(line, "T0") != NULL) { - fprintf_P(inout, PSTR("T0\n")); - return; - } - if (strstr(line, "T1") != NULL) { - fprintf_P(inout, PSTR("T1\n")); - return; - } - if (strstr(line, "T2") != NULL) { - fprintf_P(inout, PSTR("T2\n")); - return; - } - if (strstr(line, "T3") != NULL) { - fprintf_P(inout, PSTR("T3\n")); - return; - } - if (strstr(line, "T4") != NULL) { - fprintf_P(inout, PSTR("T4\n")); - return; - } - if (strstr(line, "L0") != NULL) { - fprintf_P(inout, PSTR("L0\n")); - return; - } - if (strstr(line, "L1") != NULL) { - fprintf_P(inout, PSTR("L1\n")); - return; - } - if (strstr(line, "L2") != NULL) { - fprintf_P(inout, PSTR("L2\n")); - return; - } - if (strstr(line, "L3") != NULL) { - fprintf_P(inout, PSTR("L3\n")); - return; - } - if (strstr(line, "L4") != NULL) { - fprintf_P(inout, PSTR("L4\n")); - return; - } - if (strstr(line, "C0") != NULL) { - fprintf_P(inout, PSTR("C0\n")); - return; - } - if (strstr(line, "U0") != NULL) { - fprintf_P(inout, PSTR("U0\n")); - return; - } - if (strstr(line, "E0") != NULL) { - fprintf_P(inout, PSTR("E0\n")); - return; - } - if (strstr(line, "E1") != NULL) { - fprintf_P(inout, PSTR("E1\n")); - return; - } - if (strstr(line, "E2") != NULL) { - fprintf_P(inout, PSTR("E2\n")); - return; - } - if (strstr(line, "E3") != NULL) { - fprintf_P(inout, PSTR("E3\n")); - return; - } - if (strstr(line, "E4") != NULL) { - fprintf_P(inout, PSTR("E4\n")); - return; - } - if (strstr(line, "R0") != NULL) { - fprintf_P(inout, PSTR("R0\n")); - return; - } - if (strstr(line, "FS") != NULL) { - fprintf_P(inout, PSTR("FS\n")); - return; - } - } - count = 0; - - if (sscanf_P(line, PSTR("T%d"), &value) > 0x00) { - //T-code scanned - if ((value >= 0) && (value < EXTRUDERS)) { - if ((active_extruder == value) & (isFilamentLoaded)) { - duplicateTCmd = true; - fprintf_P(inout, PSTR("ok\n")); - } else { - mmuFSensorLoading = true; - duplicateTCmd = false; - toolChange(value); - if (load_filament_at_toolChange) { - fprintf_P(inout, PSTR("fl\n")); - load_filament_withSensor(); - load_filament_at_toolChange = false; - fprintf_P(inout, PSTR("ok\n")); - } //else fprintf_P(inout, PSTR("nk\n")); - } - } - } else if (sscanf_P(line, PSTR("L%d"), &value) > 0) { - // Load filament - if ((value >= 0) && (value < EXTRUDERS) && !isFilamentLoaded) { - - select_extruder(value); - delay(10); - feed_filament(); - delay(100); - fprintf_P(inout, PSTR("ok\n")); - } - } else if (sscanf_P(line, PSTR("M%d"), &value) > 0) { - // M0: set to normal mode; M1: set to stealth mode - switch (value) { - case 0: - tmc2130_mode = NORMAL_MODE; - break; - case 1: - tmc2130_mode = STEALTH_MODE; - break; - default: - return; - } - //init all axes - tmc2130_init(tmc2130_mode); - fprintf_P(inout, PSTR("ok\n")); - } else if (sscanf_P(line, PSTR("U%d"), &value) > 0) { // Unload filament - unload_filament_withSensor(); - delay(200); - fprintf_P(inout, PSTR("ok\n")); - isPrinting = false; - trackToolChanges = 0; - } else if (sscanf_P(line, PSTR("X%d"), &value) > 0) { - if (value == 0) { // MMU reset - wdt_enable(WDTO_15MS); - } - } else if (sscanf_P(line, PSTR("S%d"), &value) > 0) { - if (value == 0) { // return ok - fprintf_P(inout, PSTR("ok\n")); - } else if (value == 1) { // Read version - fprintf_P(inout, PSTR("%dok\n"), FW_VERSION); - } else if (value == 2) { // Read build nr - fprintf_P(inout, PSTR("%dok\n"), FW_BUILDNR); - } - } else if (strstr(line, "FS") > 0) { - fsensor_triggered = true; - //fprintf_P(inout, PSTR("ok\n")); - } else if (sscanf_P(line, PSTR("F%d %d"), &value, &value0) > 0) { - if (((value >= 0) && (value < EXTRUDERS)) && ((value0 >= 0) && (value0 <= 2))) { - filament_type[value] = value0; - fprintf_P(inout, PSTR("ok\n")); - } - } else if (sscanf_P(line, PSTR("C%d"), &value) > 0) { - if (value == 0) // C0 continue loading current filament (used after T-code), maybe add different code for - // each extruder (the same way as T-codes) in the future? - { - if (!duplicateTCmd) { - load_filament_into_extruder(); - fprintf_P(inout, PSTR("ok\n")); - } else fprintf_P(inout, PSTR("ok\n")); - } - } else if (sscanf_P(line, PSTR("E%d"), &value) > 0) { - if ((value >= 0) && (value < EXTRUDERS)) { // Ex: eject filament - eject_filament(value); - fprintf_P(inout, PSTR("ok\n")); - } - } else if (sscanf_P(line, PSTR("R%d"), &value) > 0) { - if (value == 0) { // R0: recover after eject filament - recover_after_eject(); - fprintf_P(inout, PSTR("ok\n")); - } - } else if (!mmuFSensorLoading && fsensor_triggered) { - fsensor_triggered = false; - fprintf_P(inout, PSTR("ok\n")); - } - } - } -} // extern C - -void process_signals() -{ - // what to do here? -} - -void fault_handler(Fault id) -{ - while (1) { - shr16_set_led(id + 1); - delay(1000); - shr16_set_led(0); - delay(2000); - } -} - -//**************************************************************************************************** -//* this routine is the common routine called for fixing the filament issues (loading or unloading) -//**************************************************************************************************** -void fixTheProblem(void) { - - engage_filament_pulley(false); // park the idler stepper motor - tmc2130_disable_axis(AX_SEL, tmc2130_mode); // turn OFF the selector stepper motor - tmc2130_disable_axis(AX_IDL, tmc2130_mode); // turn OFF the idler stepper motor - - while ((Btn::middle != buttonClicked()) || digitalRead(A1)) { - // wait until key is entered to proceed (this is to allow for operator intervention) - delay(100); - shr16_set_led(0x000); - delay(100); - if (digitalRead(A1)) { - shr16_set_led(2 << 2 * (4 - active_extruder)); - } else shr16_set_led(1 << 2 * (4 - active_extruder)); - } - - tmc2130_init_axis(AX_SEL, tmc2130_mode); // turn ON the selector stepper motor - tmc2130_init_axis(AX_IDL, tmc2130_mode); // turn ON the idler stepper motor - home(true); // Home and return to previous active extruder -} - -bool load_filament_withSensor() -{ - fsensor_triggered = false; - loop: - { - engage_filament_pulley(true); // get in contact with filament - tmc2130_init_axis(AX_PUL, tmc2130_mode); - - unsigned long startTime, currentTime; - bool tag = false; - - // load filament until FINDA senses end of the filament, means correctly loaded into the selector - // we can expect something like 570 steps to get in sensor, try 1000 incase user is feeding to pulley - - if (moveSmooth(AX_PUL, 1000, 650, false, false, ACC_NORMAL, true) == MR_Success) { // Check if filament makes it to the FINDA - moveSmooth(AX_PUL, BOWDEN_LENGTH, MAX_SPEED_PUL, false, false, ACC_FEED_NORMAL); // Load filament down to MK3-FSensor - - startTime = millis(); - process_commands(uart_com); // Run through serial read buffer so fsensor_triggered can be updated - - while (tag == false) { - currentTime = millis(); - if ((currentTime - startTime) > 5000) { // After min bowden length load slow until MK3-FSensor trips - fixTheProblem(); - goto loop; - } - - move_pulley(1,MAX_SPEED_PUL); - process_commands(uart_com); - if (fsensor_triggered == true) tag = true; - } - moveSmooth(AX_PUL, STEPS_MK3FSensor_To_Bondtech, 385,false, false); // Load from MK3-FSensor to Bontech gears, ready for loading into extruder with C0 command - shr16_set_led(0x000); // Clear all 10 LEDs on MMU unit - shr16_set_led(1 << 2 * (4 - active_extruder)); - isFilamentLoaded = true; // filament loaded - mmuFSensorLoading = false; - return true; - } - fixTheProblem(); - goto loop; - } -} - -/** - * @brief unload_filament_withSensor - * unloads filament from extruder - filament is above Bondtech gears - */ -bool unload_filament_withSensor() -{ - bool _return = false; - tmc2130_init_axis(AX_PUL, tmc2130_mode); - //tmc2130_init_axis(AX_IDL, tmc2130_mode); - engage_filament_pulley(true); // get in contact with filament - - moveSmooth(AX_PUL, (BOWDEN_LENGTH * -1), MAX_SPEED_PUL - (MAX_SPEED_PUL/5), false, false, ACC_FEED_NORMAL); // unload to before FINDA - if (moveSmooth(AX_PUL, -2000, 650, false, false, ACC_NORMAL) == MR_Success) { // move to trigger FINDA - moveSmooth(AX_PUL, FILAMENT_PARKING_STEPS, 650, false, false, ACC_NORMAL); // move to filament parking position - } - if (digitalRead(A1)) fixTheProblem(); // If -1000 steps didn't trigger FINDA - isFilamentLoaded = false; // update global variable filament unloaded - tmc2130_disable_axis(AX_PUL, tmc2130_mode); - engage_filament_pulley(false); - return true; -} diff --git a/mmctl.cpp b/mmctl.cpp deleted file mode 100644 index 5f74daf47..000000000 --- a/mmctl.cpp +++ /dev/null @@ -1,146 +0,0 @@ -// mmctl.cpp - multimaterial switcher control -#include "main.h" -#include -#include -#include -#include -#include "shr16.h" -#include "spi.h" -#include "tmc2130.h" -#include "mmctl.h" -#include "motion.h" -#include "Buttons.h" - -// public variables: -int active_extruder = -1; // extruder channel, 0...4 -int previous_extruder = -1; -bool isFilamentLoaded = false; -bool isIdlerParked = false; -bool isPrinting = false; -bool isHomed = false; -bool homedOnUnload = false; - -// private variables: -static int toolChanges = 0; -int trackToolChanges = 0; - -bool feed_filament(void) -{ - bool _loaded = false; - - int _c = 0; - engage_filament_pulley(true); - - while (!_loaded) { - - if (moveSmooth(AX_PUL, 4000, 650, false, true, ACC_NORMAL, true) == MR_Success) { - moveSmooth(AX_PUL, FILAMENT_PARKING_STEPS, 650, false, false, ACC_NORMAL); - shr16_set_led(1 << 2 * (4 - active_extruder)); - _loaded = true; - break; - } else { - if (_c < 2) { - fixTheProblem(); - engage_filament_pulley(true); - } else { - _loaded = false; - break; - } - _c++; - } - } - tmc2130_disable_axis(AX_PUL, tmc2130_mode); - engage_filament_pulley(false); - return _loaded; -} - -bool toolChange(int new_extruder) -{ - bool _return = false; - isPrinting = true; - - shr16_set_led(2 << 2 * (4 - active_extruder)); - - previous_extruder = active_extruder; - active_extruder = new_extruder; - - if (previous_extruder == active_extruder) { - if (!isFilamentLoaded) { - shr16_set_led(2 << 2 * (4 - active_extruder)); - load_filament_at_toolChange = true; - _return = true; - } else { - _return = true; // nothing really happened - } - } else { - if (isFilamentLoaded) { - unload_filament_withSensor(); //failed unload. unload filament first - } - if (!isFilamentLoaded) { - if (trackToolChanges == TOOLSYNC) { // Home every period TOOLSYNC - home(true); - // move idler and selector to new filament position - } else if (!homedOnUnload) set_positions(previous_extruder, active_extruder); - toolChanges++; - trackToolChanges ++; - shr16_set_led(2 << 2 * (4 - active_extruder)); - load_filament_at_toolChange = true; - homedOnUnload = false; - _return = true; - } - } - - shr16_set_led(0x000); - shr16_set_led(1 << 2 * (4 - active_extruder)); - return _return; -} - -//! @brief select extruder -//! -//! Known limitation is, that if extruder 5 - service position was selected before -//! it is not possible to select any other extruder than extruder 4. -//! -//! @param new_extruder Extruder to be selected -//! @return -bool select_extruder(int new_extruder) -{ - if (digitalRead(A1)) return false; - - int previous_extruder = active_extruder; - active_extruder = new_extruder; - - bool _return = false; - if (!isHomed) { - home(); - } - - shr16_set_led(2 << 2 * (4 - active_extruder)); - - if (previous_extruder == active_extruder) { - if (!isFilamentLoaded) { - _return = true; - } - } else { - set_positions(previous_extruder, active_extruder); // move idler and selector to new filament position - engage_filament_pulley(false); - _return = true; - } - - - shr16_set_led(0x000); - shr16_set_led(1 << 2 * (4 - active_extruder)); - return _return; -} - -void led_blink(int _no) -{ - shr16_set_led(1 << 2 * _no); - delay(40); - shr16_set_led(0x000); - delay(20); - shr16_set_led(1 << 2 * _no); - delay(40); - - shr16_set_led(0x000); - delay(10); -} diff --git a/motion.cpp b/motion.cpp deleted file mode 100644 index be98be9be..000000000 --- a/motion.cpp +++ /dev/null @@ -1,492 +0,0 @@ -#include "motion.h" -#include "shr16.h" -#include "tmc2130.h" -#include -#include -#include -#include -#include "main.h" -#include "uart.h" -#include "mmctl.h" -#include "Buttons.h" -#include "permanent_storage.h" -#include "config.h" - -// public variables: -int8_t filament_type[EXTRUDERS] = { -1, -1, -1, -1, -1}; - -// private constants: -// selector homes on the right end. afterwards it is moved to extruder 0 -static const int SELECTOR_STEPS_AFTER_HOMING = -3700; -static const int IDLER_STEPS_AFTER_HOMING = -138; - -static const int IDLER_FULL_TRAVEL_STEPS = 1420; // 16th micro steps -// after homing: 1420 into negative direction -// and 130 steps into positive direction - -static const int SELECTOR_STEPS = 2800 / (EXTRUDERS - 1); -static const int IDLER_STEPS = 1420 / (EXTRUDERS - 1); // full travel = 1420 16th micro steps -const int IDLER_PARKING_STEPS = (IDLER_STEPS / 2) + 40; // 217 - -const int BOWDEN_LENGTH = 8000; -const int STEPS_MK3FSensor_To_Bondtech = 390; -const int FILAMENT_PARKING_STEPS = -320; -const int EXTRA_STEPS_SELECTOR_SERVICE = 150; - -static const int EJECT_PULLEY_STEPS = 2500; - -// private variables: - -static int selector_steps_for_eject = 0; - -static int idler_steps_for_eject = 0; - -// private functions: -static int set_idler_direction(int steps); -static int set_selector_direction(int steps); -static int set_pulley_direction(int steps); - -void set_positions(int _current_extruder, int _next_extruder) -{ - delay(50); - int _idler_steps = (_current_extruder - _next_extruder) * IDLER_STEPS; - if (_next_extruder == EXTRUDERS) _idler_steps = (_current_extruder - (_next_extruder - 1)) * IDLER_STEPS; - if (_current_extruder == EXTRUDERS) _idler_steps = ((_current_extruder - 1) - _next_extruder) * IDLER_STEPS; - // steps to move to new position of idler and selector - move_idler(_idler_steps); // remove this, when abs coordinates are implemented! - - if (_next_extruder > 0) { - int _selector_steps = ((_current_extruder - _next_extruder) * SELECTOR_STEPS) * -1; - if (_next_extruder == EXTRUDERS) _selector_steps += EXTRA_STEPS_SELECTOR_SERVICE; - if (_current_extruder == EXTRUDERS) _selector_steps -= EXTRA_STEPS_SELECTOR_SERVICE; - move_selector(_selector_steps); - } else { - moveSmooth(AX_SEL, 100, 2000, false); - for (int c = 2; c > 0; c--) { // touch end 2 times - moveSmooth(AX_SEL, -4000, 2000, false); - if (c > 1) { - moveSmooth(AX_SEL, 100, 2000, false); - } - } - moveSmooth(AX_SEL, 33, 2000, false); - } -} - -/** - * @brief Eject Filament - * move selector sideways and push filament forward little bit, so user can catch it, - * unpark idler at the end to user can pull filament out - * @param extruder: extruder channel (0..4) - */ -void eject_filament(int extruder) -{ - int selector_position = 0; - - int8_t selector_offset_for_eject = 0; - int8_t idler_offset_for_eject = 0; - - // if there is still filament detected by PINDA unload it first - if (isFilamentLoaded) { - unload_filament_withSensor(); - } - - - engage_filament_pulley(true); - tmc2130_init_axis(AX_PUL, tmc2130_mode); - - - // if we are want to eject fil 0-2, move seelctor to position 4 (right), if we want to eject filament 3 - 4, move - // selector to position 0 (left) - // maybe we can also move selector to service position in the future? - if (extruder <= 2) { - selector_position = 4; - } else { - selector_position = 0; - } - - // count offset (number of positions) for desired selector and idler position for ejecting - selector_offset_for_eject = active_extruder - selector_position; - idler_offset_for_eject = active_extruder - extruder; - - // count number of desired steps for selector and idler and store it in static variable - selector_steps_for_eject = (selector_offset_for_eject * SELECTOR_STEPS) * -1; - idler_steps_for_eject = idler_offset_for_eject * IDLER_STEPS; - - // move selector and idler to new position - move_idler(idler_steps_for_eject); // remove this, with when abs coordinates are implemented! - move_selector(selector_steps_for_eject); - - // push filament forward - move_pulley(EJECT_PULLEY_STEPS, 666); - - // unpark idler so user can easily remove filament - engage_filament_pulley(false); - tmc2130_disable_axis(AX_PUL, tmc2130_mode); - isFilamentLoaded = false; // ensure MMU knows it doesn't have filament loaded so next T? command works -} - -void recover_after_eject() -{ - while (digitalRead(A1)) fixTheProblem(); - move_idler(-idler_steps_for_eject); // TODO 1: remove this, when abs coordinates are implemented! - move_selector(-selector_steps_for_eject); -} - -/** - * @brief load_filament_intoExtruder - * loads filament after confirmed by printer into the Bontech - * pulley gears so they can grab them. - * We reduce here stepwise the motor current, to prevent grinding into the - * filament as good as possible. - * - * TODO 1: this procedure is most important for high reliability. - * The speed must be set accordingly to the settings in the slicer - */ -void load_filament_into_extruder() -{ - uint8_t current_running_normal[3] = CURRENT_RUNNING_NORMAL; - uint8_t current_running_stealth[3] = CURRENT_RUNNING_STEALTH; - uint8_t current_holding_normal[3] = CURRENT_HOLDING_NORMAL; - uint8_t current_holding_stealth[3] = CURRENT_HOLDING_STEALTH; - - engage_filament_pulley(true); // if idler is in parked position un-park him get in contact with filament - - tmc2130_init_axis(AX_PUL, tmc2130_mode); - move_pulley(150, 385); - - // set current to 75% - if (tmc2130_mode == NORMAL_MODE) { - tmc2130_init_axis_current_normal(AX_PUL, current_holding_normal[AX_PUL], - current_running_normal[AX_PUL] - (current_running_normal[AX_PUL] / 4) ); - } else { - tmc2130_init_axis_current_stealth(AX_PUL, current_holding_stealth[AX_PUL], - current_running_stealth[AX_PUL] - (current_running_stealth[AX_PUL] / 4) ); - } - move_pulley(170, 385); - - // set current to 25% - if (tmc2130_mode == NORMAL_MODE) { - tmc2130_init_axis_current_normal(AX_PUL, current_holding_normal[AX_PUL], - current_running_normal[AX_PUL] / 4); - } else { - tmc2130_init_axis_current_stealth(AX_PUL, current_holding_stealth[AX_PUL], - current_running_stealth[AX_PUL] / 4); - } - moveSmooth(AX_PUL, 452, 455, true, true, ACC_NORMAL, false, true); - - - // reset currents - if (tmc2130_mode == NORMAL_MODE) { - tmc2130_init_axis_current_normal(AX_PUL, current_holding_normal[AX_PUL], - current_running_normal[AX_PUL]); - } else { - tmc2130_init_axis_current_stealth(AX_PUL, current_holding_stealth[AX_PUL], - current_running_stealth[AX_PUL]); - } - tmc2130_disable_axis(AX_PUL, tmc2130_mode); -} - -void init_Pulley() -{ - float _speed = 3000; - - // TODO 1: replace with move-commands - - for (int i = 50; i > 0; i--) { - moveSmooth(AX_PUL, 1, 0, false); - delayMicroseconds(_speed); - shr16_set_led(1 << 2 * (int)(i / 50)); // TODO 2: What the heck? - } - - for (int i = 50; i > 0; i--) { - moveSmooth(AX_PUL, -1, 0, false); - delayMicroseconds(_speed); - shr16_set_led(1 << 2 * (4 - (int)(i / 50))); // TODO 2: What the heck? - } -} - -/** - * @brief engage_filament_pulley - * Turns the idler drum to engage or disengage the filament pully - * @param engage - * If true, pully can drive the filament afterwards - * if false, idler will be parked, so the filament can move freely - */ -void engage_filament_pulley(bool engage) -{ - if (isIdlerParked && engage) { // get idler in contact with filament - move_idler(IDLER_PARKING_STEPS); - isIdlerParked = false; - } else if (!isIdlerParked && !engage) { // park idler so filament can move freely - move_idler(IDLER_PARKING_STEPS * -1); - isIdlerParked = true; - } -} - -void reset_engage_filament_pulley(bool previouslyEngaged) //reset after mid op homing -{ - if (isIdlerParked && previouslyEngaged) { // get idler in contact with filament - move_idler(IDLER_PARKING_STEPS); - isIdlerParked = false; - } else if (!isIdlerParked && !previouslyEngaged) { // park idler so filament can move freely - move_idler(IDLER_PARKING_STEPS * -1); - isIdlerParked = true; - } -} - -void home(bool doToolSync) -{ - tmc2130_init(HOMING_MODE); // trinamic, homing - bool previouslyEngaged = isIdlerParked; - homeIdlerSmooth(); - homeSelectorSmooth(); - tmc2130_init(tmc2130_mode); // trinamic, normal - //tmc2130_init_axis(AX_IDL, tmc2130_mode); - //tmc2130_init_axis(AX_SEL, tmc2130_mode); - - shr16_set_led(0x155); // All five red - - isIdlerParked = false; - delay(50); // delay to release the stall detection - engage_filament_pulley(false); - shr16_set_led(0x000); // All five off - - isFilamentLoaded = false; - shr16_set_led(1 << 2 * (4 - active_extruder)); - - isHomed = true; - - if (doToolSync) { - set_positions(0, active_extruder); // move idler and selector to new filament position - reset_engage_filament_pulley(previouslyEngaged); - trackToolChanges = 0; - } else active_extruder = 0; -} - -void move_idler(int steps, uint16_t speed) -{ - if (speed > MAX_SPEED_IDL) { - speed = MAX_SPEED_IDL; - } - moveSmooth(AX_IDL, steps, MAX_SPEED_IDL, true, true, ACC_IDL_NORMAL); -} - -/** - * @brief move_selector - * Strictly prevent selector movement, when filament is in FINDA - * @param steps, number of micro steps - */ -void move_selector(int steps, uint16_t speed) -{ - if (speed > MAX_SPEED_SEL) { - speed = MAX_SPEED_SEL; - } - if (tmc2130_mode == STEALTH_MODE) { - if (speed > MAX_SPEED_STEALTH_SEL) { - speed = MAX_SPEED_STEALTH_SEL; - } - } - if (digitalRead(A1) == false) { - moveSmooth(AX_SEL, steps, speed); - } -} - -void move_pulley(int steps, uint16_t speed) -{ - moveSmooth(AX_PUL, steps, speed, false, true); -} - -/** - * @brief set_idler_direction - * @param steps: positive = towards engaging filament nr 1, - * negative = towards engaging filament nr 5. - * @return abs(steps) - */ -int set_idler_direction(int steps) -{ - if (steps < 0) { - steps = steps * -1; - shr16_set_dir(shr16_get_dir() & ~4); - } else { - shr16_set_dir(shr16_get_dir() | 4); - } - return steps; -} - -/** - * @brief set_selector_direction - * Sets the direction bit on the motor driver and returns positive number of steps - * @param steps: positive = to the right (towards filament 5), - * negative = to the left (towards filament 1) - * @return abs(steps) - */ -int set_selector_direction(int steps) -{ - if (steps < 0) { - steps = steps * -1; - shr16_set_dir(shr16_get_dir() & ~2); - } else { - shr16_set_dir(shr16_get_dir() | 2); - } - return steps; -} - -/** - * @brief set_pulley_direction - * @param steps, positive (push) or negative (pull) - * @return abs(steps) - */ -int set_pulley_direction(int steps) -{ - if (steps < 0) { - steps = steps * -1; - shr16_set_dir(shr16_get_dir() | 1); - } else { - shr16_set_dir(shr16_get_dir() & ~1); - } - return steps; -} - -MotReturn homeSelectorSmooth() -{ - for (int c = 2; c > 0; c--) { // touch end 2 times - moveSmooth(AX_SEL, 4000, 2000, false); // 3000 is too fast, 2500 works, decreased to 2000 for production - if (c > 1) { - moveSmooth(AX_SEL, -300, 2000, false); - } - } - - return moveSmooth(AX_SEL, SELECTOR_STEPS_AFTER_HOMING, MAX_SPEED_SEL, false); -} - -MotReturn homeIdlerSmooth() -{ - for (int c = 2; c > 0; c--) { // touch end 3 times - moveSmooth(AX_IDL, 2000, 3000, false); - if (c > 1) { - moveSmooth(AX_IDL, -350, MAX_SPEED_IDL, false); - } - } - return moveSmooth(AX_IDL, IDLER_STEPS_AFTER_HOMING, MAX_SPEED_IDL, false); -} - -/** - * @brief moveTest - * @param axis, index of axis, use AX_PUL, AX_SEL or AX_IDL - * @param steps, number of micro steps to move - * @param speed, max. speed - * @param rehomeOnFail: flag, by default true, set to false - * in homing commands, to prevent endless loops and stack overflow. - * @return - */ -// TODO 3: compensate delay for computation time, to get accurate speeds -MotReturn moveSmooth(uint8_t axis, int steps, int speed, bool rehomeOnFail, bool withStallDetection, float acc, bool withFindaDetection, bool disengageAtEnd) -{ - MotReturn ret = MR_Success; - if (withFindaDetection) ret = MR_Failed; - - if (tmc2130_mode == STEALTH_MODE) { - withStallDetection = false; - } - - float vMax = speed; - float v0 = 200; // steps/s, minimum speed - float v = v0; // current speed - int accSteps = 0; // number of steps for acceleration - int stepsDone = 0; - int stepsLeft = 0; - - switch (axis) { - case AX_PUL: - stepsLeft = set_pulley_direction(steps); - if (disengageAtEnd) { set_idler_direction(IDLER_PARKING_STEPS * -1); stepsLeft += (IDLER_PARKING_STEPS/2); }// 217 and steps left plus half - tmc2130_init_axis(AX_PUL, tmc2130_mode); - break; - case AX_IDL: - stepsLeft = set_idler_direction(steps); - break; - case AX_SEL: - stepsLeft = set_selector_direction(steps); - break; - } - - enum State { - Accelerate = 0, - ConstVelocity = 1, - Decelerate = 2, - }; - - State st = Accelerate; - - while (stepsLeft) { - switch (axis) { - case AX_PUL: - PIN_STP_PUL_HIGH; - PIN_STP_PUL_LOW; - if ((stepsLeft <= IDLER_PARKING_STEPS) && disengageAtEnd) { PIN_STP_IDL_HIGH; PIN_STP_IDL_LOW; } // Park AX_IDL for last parking steps - if ((stepsLeft >= (IDLER_PARKING_STEPS/2)) && disengageAtEnd){ PIN_STP_PUL_HIGH; PIN_STP_PUL_LOW; } // finish AX_PUL move half way through AX_IDL parking - if (withStallDetection && digitalRead(A3)) { // stall detected - delay(50); // delay to release the stall detection - return MR_Failed; - } - if (withFindaDetection && ( steps > 0 ) && digitalRead(A1)) return MR_Success; - if (withFindaDetection && ( steps < 0 ) && (digitalRead(A1) == false)) return MR_Success; - break; - case AX_IDL: - PIN_STP_IDL_HIGH; - PIN_STP_IDL_LOW; - if (withStallDetection && digitalRead(A5)) { // stall detected - delay(50); // delay to release the stall detection - if (rehomeOnFail) fixTheProblem(); - else return MR_Failed; - } - break; - case AX_SEL: - PIN_STP_SEL_HIGH; - PIN_STP_SEL_LOW; - if (withStallDetection && digitalRead(A4)) { // stall detected - delay(50); // delay to release the stall detection - if (rehomeOnFail) fixTheProblem(); - else return MR_Failed; - } - break; - } - - stepsDone++; - stepsLeft--; - - float dt = 1 / v; - delayMicroseconds(1e6 * dt); - - switch (st) { - case Accelerate: - v += acc * dt; - if (v >= vMax) { - accSteps = stepsDone; - st = ConstVelocity; - - v = vMax; - } else if (stepsDone > stepsLeft) { - accSteps = stepsDone; - st = Decelerate; - - } - break; - case ConstVelocity: { - if (stepsLeft <= accSteps) { - st = Decelerate; - } - } - break; - case Decelerate: { - v -= acc * dt; - if (v < v0) { - v = v0; - } - } - break; - } - } - if (disengageAtEnd) isIdlerParked = true; - return ret; -} diff --git a/motion.h b/motion.h deleted file mode 100644 index 1b075108a..000000000 --- a/motion.h +++ /dev/null @@ -1,40 +0,0 @@ -// motion.h - -#ifndef _MOTION_h -#define _MOTION_h - -#include "config.h" -#include -#include - - -extern int8_t filament_type[EXTRUDERS]; -extern const int IDLER_PARKING_STEPS; -extern const int BOWDEN_LENGTH; -extern const int STEPS_MK3FSensor_To_Bondtech; -extern const int FILAMENT_PARKING_STEPS; - -void home(bool doToolSync = false); -void engage_filament_pulley(bool engage); -void reset_engage_filament_pulley(bool previouslyEngaged); - -void load_filament_into_extruder(); - -void set_positions(int _current_extruder, int _next_extruder); - -void init_Pulley(); - -void move_idler(int steps, uint16_t speed = MAX_SPEED_IDL); -void move_selector(int steps, uint16_t speed = MAX_SPEED_SEL); -void move_pulley(int steps, uint16_t speed = MAX_SPEED_PUL); - -void eject_filament(int extruder); -void recover_after_eject(); - -enum MotReturn {MR_Success, MR_FailedAndRehomed, MR_Failed}; -MotReturn homeSelectorSmooth(); -MotReturn moveSmooth(uint8_t axis, int steps, int speed,bool rehomeOnFail = true, - bool withStallDetection = true, float ACC = ACC_NORMAL, bool withFindaDetection = false, bool disengageAtEnd = false); -MotReturn homeIdlerSmooth(); -MotReturn homeSelectorSmooth(); -#endif