From f6a8eb56d75f50852b0bfec4653a091156bf6bb8 Mon Sep 17 00:00:00 2001 From: TheZeroBeast Date: Tue, 20 Nov 2018 11:35:22 +1000 Subject: [PATCH] unload_filament_withSensor rewrite Pending Testing. --- main.cpp | 551 +++++++++++++++++++++++++++++++++++++++++++++++++++++ mmctl.cpp | 146 ++++++++++++++ motion.cpp | 492 +++++++++++++++++++++++++++++++++++++++++++++++ motion.h | 40 ++++ 4 files changed, 1229 insertions(+) create mode 100644 main.cpp create mode 100644 mmctl.cpp create mode 100644 motion.cpp create mode 100644 motion.h diff --git a/main.cpp b/main.cpp new file mode 100644 index 000000000..6ddad2ef6 --- /dev/null +++ b/main.cpp @@ -0,0 +1,551 @@ +//! @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 new file mode 100644 index 000000000..5f74daf47 --- /dev/null +++ b/mmctl.cpp @@ -0,0 +1,146 @@ +// 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 new file mode 100644 index 000000000..be98be9be --- /dev/null +++ b/motion.cpp @@ -0,0 +1,492 @@ +#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 new file mode 100644 index 000000000..1b075108a --- /dev/null +++ b/motion.h @@ -0,0 +1,40 @@ +// 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