diff --git a/Firmware/main.cpp b/Firmware/main.cpp new file mode 100644 index 000000000..e6a76ab19 --- /dev/null +++ b/Firmware/main.cpp @@ -0,0 +1,612 @@ +//! @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 fixedTheProblem = false; +bool fixTheProblems = 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) == 1) { + while (Btn::right != buttonClicked()) { + if (digitalRead(A1) == 1) { + 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 (fixTheProblems == true) { + fprintf_P(inout, PSTR("not_ok\n")); + count = 0; + } else if (fixedTheProblem == true) { + fixTheProblems = false; + fixedTheProblem = false; + fprintf_P(inout, PSTR("ok\n")); + count = 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 (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; + } + if (strstr(line, "FL") != NULL) { + fprintf_P(inout, PSTR("FS\n")); + return; + } + } + count = 0; + + if (sscanf_P(line, PSTR("T%d"), &value) > 0) { + //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); + fprintf_P(inout, PSTR("FL\n")); + if (load_filament_at_toolChange){ + load_filament_withSensor(); + load_filament_at_toolChange = false; + } + } + } + } 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("P%d"), &value) > 0) { + if (value == 0) { // Read finda + fprintf_P(inout, PSTR("%dok\n"), digitalRead(A1)); + } + } 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")); + } + } + } + } +} // 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) { + fixedTheProblem = false; + fixTheProblems = true; + engage_filament_pulley(false); // park the idler stepper motor + delay(50); + tmc2130_disable_axis(AX_SEL, tmc2130_mode); + + while (!(Btn::middle == buttonClicked() && digitalRead(A1) == 1)) { + // wait until key is entered to proceed (this is to allow for operator intervention) + /*if (Btn::middle == buttonClicked() && digitalRead(A1) == 0) { + break; + }*/ + delay(100); + shr16_set_led(0x000); + delay(100); + if (digitalRead(A1) == 1) { + shr16_set_led(2 << 2 * (4 - active_extruder)); + } else { + shr16_set_led(1 << 2 * (4 - active_extruder)); + } + process_commands(uart_com); + } + + tmc2130_init_axis(AX_SEL, tmc2130_mode); // turn ON the selector stepper motor + + homeSelectorSmooth(); + reset_positions(AX_SEL, 0, active_extruder, ACC_NORMAL); + isFilamentLoaded = false; + delay(10); // wait for 10 millisecond + fixedTheProblem = true; +} + +bool load_filament_withSensor() +{ +loop: + { + 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); + uint8_t current_loading_normal[3] = CURRENT_LOADING_NORMAL; + uint8_t current_loading_stealth[3] = CURRENT_LOADING_STEALTH; + 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; + 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 + + if (tmc2130_mode == NORMAL_MODE) { + tmc2130_init_axis_current_normal(AX_PUL, current_holding_normal[AX_PUL], + current_loading_normal[AX_PUL]); + } else { + tmc2130_init_axis_current_normal(AX_PUL, current_holding_stealth[AX_PUL], + current_loading_stealth[AX_PUL]); + } + + if (moveSmooth(AX_PUL, 2500, 650, false, false, ACC_NORMAL, true) == MR_Success) { + 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_normal(AX_PUL, current_holding_stealth[AX_PUL], + current_running_stealth[AX_PUL]); + } + moveSmooth(AX_PUL, 8500, MAX_SPEED_PUL, false, false, ACC_FEED_NORMAL); + + startTime = millis(); + fsensor_triggered = false; + process_commands(uart_com); + + while (tag == false) { + currentTime = millis(); + if ((currentTime - startTime) > 12000) { + fixTheProblem(); + goto loop; + } + + move_pulley(2,MAX_SPEED_PUL); + process_commands(uart_com); + if (fsensor_triggered == true) tag = true; + delayMicroseconds(500); ///RMM:TODO changed to 300 from 600us on 3 Nov 18 + } + + mmuFSensorLoading = false; + fsensor_triggered = false; + //delayMicroseconds(600); + moveSmooth(AX_PUL, STEPS_MK3FSensor_To_Bondtech, 350,false, false); + isFilamentLoaded = true; // filament loaded + shr16_set_led(0x000); + shr16_set_led(2 << 2 * (4 - active_extruder)); + return true; + } + fixTheProblem(); + goto loop; + shr16_set_led(0x000); + shr16_set_led(2 << 2 * (4 - active_extruder)); + return false; + } +} + +/** + * @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); // if idler is in parked position un-park him get in contact with filament + + moveSmooth(AX_PUL, -400, 350, false, false); + switch (moveSmooth(AX_PUL, -12000, MAX_SPEED_PUL - (MAX_SPEED_PUL/5), false, false, ACC_FEED_NORMAL, true)) { + case MR_Success: + moveSmooth(AX_PUL, -50, 650, false, false, ACC_NORMAL); + moveSmooth(AX_PUL, 600, 650, false, false, ACC_NORMAL, true); + moveSmooth(AX_PUL, -600, 650, false, false, ACC_NORMAL); + if (digitalRead(A1) == 1) { + fixTheProblem(); + return; + } + isFilamentLoaded = false; // filament unloaded + _return = true; + break; + default: + fixTheProblem(); + } + tmc2130_disable_axis(AX_PUL, tmc2130_mode); + engage_filament_pulley(false); + return _return; +} diff --git a/Firmware/main.h b/Firmware/main.h new file mode 100644 index 000000000..7780f0192 --- /dev/null +++ b/Firmware/main.h @@ -0,0 +1,41 @@ +#ifndef _MAIN_H +#define _MAIN_H + +#include +#include "config.h" +#include "uart.h" + +void manual_extruder_selector(); + +// system state +extern int8_t sys_state; + +// signals from interrupt to main loop +extern uint8_t sys_signals; +extern bool load_filament_at_toolChange; +void process_signals(); +bool load_filament_withSensor(); +bool unload_filament_withSensor(); +void fixTheProblem(); + +extern uint8_t tmc2130_mode; +extern bool fsensor_triggered; + +// get state of signal (main loop or interrupt) +#define SIG_GET(id) (sys_signals & (1 << id)) +// set state of signal (interrupt only) +#define SIG_SET(id) (sys_signals |= (1 << id)) +// get state of signal (main loop only) +#define SIG_CLR(id) \ + asm("cli"); \ + sys_signals &= ~(1 << id); \ + asm("sei") + +typedef enum eFault {FAULT_IDLER_INIT_0, FAULT_IDLER_INIT_1, FAULT_IDLER_INIT_2, + FAULT_SELECTOR_INIT_0, FAULT_SELECTOR_INIT_1, FAULT_SELECTOR_INIT_2, + FAULT_PULLEY_INIT_0, FAULT_PULLEY_INIT_1, FAULT_PULLEY_INIT_2, + } Fault; + +void fault_handler(Fault id); + +#endif //_MAIN_H diff --git a/Firmware/mmctl.cpp b/Firmware/mmctl.cpp new file mode 100644 index 000000000..0dfc188ec --- /dev/null +++ b/Firmware/mmctl.cpp @@ -0,0 +1,186 @@ +// 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; + +// private variables: +static int toolChanges = 0; +int trackToolChanges = 0; + +bool feed_filament() +{ + bool _loaded = false; + + uint8_t current_loading_normal[3] = CURRENT_LOADING_NORMAL; + uint8_t current_loading_stealth[3] = CURRENT_LOADING_STEALTH; + 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; + + + int _c = 0; + engage_filament_pulley(true); + while (!_loaded) { + if (tmc2130_mode == NORMAL_MODE) { + tmc2130_init_axis_current_normal(AX_PUL, current_holding_normal[AX_PUL], + current_loading_normal[AX_PUL]); + } else { + tmc2130_init_axis_current_normal(AX_PUL, current_holding_stealth[AX_PUL], + current_loading_stealth[AX_PUL]); + } + + if (moveSmooth(AX_PUL, 4000, 650, false, true, ACC_NORMAL, true) == MR_Success) { + 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_normal(AX_PUL, current_holding_stealth[AX_PUL], + current_running_stealth[AX_PUL]); + } + moveSmooth(AX_PUL, -600, 650, false, false, ACC_NORMAL); + shr16_set_led(1 << 2 * (4 - active_extruder)); + _loaded = true; + break; + } else { + if (_c < 2) fixTheProblem(); + else 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; + + if (active_extruder == 5) { + active_extruder = 4; + move_selector(-700); // service position + } + + 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_withSensor(); // just load filament if not loaded + _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(true); + set_positions(0, active_extruder); // move idler and selector to new filament position + delay(50); + engage_filament_pulley(true); + } else { + set_positions(previous_extruder, active_extruder); // move idler and selector to new filament position + } + toolChanges++; + trackToolChanges ++; + shr16_set_led(2 << 2 * (4 - active_extruder)); + //load_filament_withSensor(); + load_filament_at_toolChange = true; + _return = true; + } + } + + shr16_set_led(0x000); + shr16_set_led(2 << 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) == 1) 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 { + if (new_extruder == EXTRUDERS) { + move_selector(700); // move to service position + } else { + if (previous_extruder == EXTRUDERS) { + move_selector(-700); // move back from service position + } 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; +} + +bool service_position() +{ + // TODO 2: fixme, when abs-coords are implemented + move_selector(600); // TODO 1: check if 600 is ok! + return true; +} + +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/Firmware/motion.cpp b/Firmware/motion.cpp new file mode 100644 index 000000000..ecf5d120d --- /dev/null +++ b/Firmware/motion.cpp @@ -0,0 +1,539 @@ +#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; // + +static const int BOWDEN_LENGTH = 1000; +const int STEPS_MK3FSensor_To_Bondtech = 360; + +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) +{ + // steps to move to new position of idler and selector + int _idler_steps = (_current_extruder - _next_extruder) * IDLER_STEPS; + + 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; + 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); + } +} + +bool reset_positions(uint8_t axis, int _current_extruder_pos, int _new_extruder_pos, float acc) +{ + // steps to move axis to new position of idler and selector independantly + int steps = 0; + bool _return = false; + + if (axis == AX_SEL) { + + if (digitalRead(A1) == 1) { + isFilamentLoaded = true; + return false; + } + int new_AX_SEL = -1; + int cur_AX_SEL = -1; + if (_new_extruder_pos == EXTRUDERS) { + int new_AX_SEL = EXTRUDERS - 1; + steps = (((_current_extruder_pos - new_AX_SEL) * SELECTOR_STEPS) * -1) + 700; // amount to service position + } else { + if (_current_extruder_pos == EXTRUDERS) { + int cur_AX_SEL = EXTRUDERS - 1; + steps = (((cur_AX_SEL - _new_extruder_pos) * SELECTOR_STEPS) * -1)-700; // Return from service position + } else { + steps = ((_current_extruder_pos - _new_extruder_pos) * SELECTOR_STEPS) * -1; + } + } + if (moveSmooth(AX_SEL, steps, MAX_SPEED_SEL, true, true, acc) == MR_Success) _return = true; + } else if (axis == AX_IDL) { + int new_AX_IDL = -1; + if (_new_extruder_pos == EXTRUDERS) { + new_AX_IDL = EXTRUDERS - 1; + } else new_AX_IDL = _new_extruder_pos; + steps = ((_current_extruder_pos - new_AX_IDL) * IDLER_STEPS); + isIdlerParked = false; + if (moveSmooth(AX_IDL, steps, MAX_SPEED_IDL, true, true, acc) == MR_Success) _return = true; + delay(50); + engage_filament_pulley(false); + } + isFilamentLoaded = false; + return _return; +} + +/** + * @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); +} + +void recover_after_eject() +{ + // restore state before eject filament + tmc2130_init_axis(AX_PUL, tmc2130_mode); + + + // pull back filament + engage_filament_pulley(true); + move_pulley(-EJECT_PULLEY_STEPS); + engage_filament_pulley(false); + + move_idler(-idler_steps_for_eject); // TODO 1: remove this, when abs coordinates are implemented! + move_selector(-selector_steps_for_eject); + + tmc2130_disable_axis(AX_PUL, tmc2130_mode); +} + +/** + * @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); + } + move_pulley(452, 455); + + + // 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); + engage_filament_pulley(false); +} + +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 home(bool doToolSync) +{ + homeIdlerSmooth(); + homeSelectorSmooth(); + shr16_set_led(0x155); + + isHomed = true; + if (doToolSync == true) { + int new_extruder = active_extruder; + active_extruder = 0; + if (active_extruder != new_extruder) { + //set_positions(active_extruder, new_extruder); // move idler and selector to new filament position + trackToolChanges = 0; + } + } else active_extruder = 0; + + isIdlerParked = false; + engage_filament_pulley(false); + shr16_set_led(0x000); + + isFilamentLoaded = false; + shr16_set_led(1 << 2 * (4 - active_extruder)); +} + +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 (isFilamentInFinda() == 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, 8000, 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 +// TODO 3: add callback or another parameter, which can stop the motion +// (e.g. for testing FINDA, timeout, soft stall guard limits, push buttons...) +MotReturn moveSmooth(uint8_t axis, int steps, int speed, bool rehomeOnFail, bool withStallDetection, float acc, bool withFindaDetection) +{ + MotReturn ret = MR_Success; + + 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); + 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 (withStallDetection && digitalRead(A3) == 1) { // stall detected + delay(50); // delay to release the stall detection + return MR_Failed; + } + if (withFindaDetection && steps > 0 && digitalRead(A1) == 1) return MR_Success; + if (withFindaDetection && steps < 0 && digitalRead(A1) == 0) return MR_Success; + break; + case AX_IDL: + PIN_STP_IDL_HIGH; + PIN_STP_IDL_LOW; + if (withStallDetection && digitalRead(A5) == 1) { // stall detected + delay(50); // delay to release the stall detection + if (rehomeOnFail) { + if (homeIdlerSmooth() == MR_Success) { + delay(50); + reset_positions(AX_IDL, 0, active_extruder); + return MR_FailedAndRehomed; + } else { + return MR_Failed; + } + } else { + return MR_Failed; + } + } + break; + case AX_SEL: + PIN_STP_SEL_HIGH; + PIN_STP_SEL_LOW; + if (withStallDetection && digitalRead(A4) == 1) { // stall detected + delay(50); // delay to release the stall detection + if (rehomeOnFail) { + if (homeSelectorSmooth() == MR_Success) { + reset_positions(AX_SEL, 0, active_extruder); + return MR_FailedAndRehomed; + } else { + return MR_Failed; + } + } 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; + } + } + return ret; +} + +bool isFilamentInFinda() +{ + return digitalRead(A1) == 1; +}