Merge remote-tracking branch 'origin/MMU-MK3-FSensorBuild-Patch1' into MMU-MK3-FSensorBuild-Patch1
This commit is contained in:
Rob McKenzie
commit
6584dfe872
|
|
@ -0,0 +1,612 @@
|
||||||
|
//! @file
|
||||||
|
|
||||||
|
#include "main.h"
|
||||||
|
#include <Arduino.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <avr/io.h>
|
||||||
|
#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 <avr/wdt.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,41 @@
|
||||||
|
#ifndef _MAIN_H
|
||||||
|
#define _MAIN_H
|
||||||
|
|
||||||
|
#include <inttypes.h>
|
||||||
|
#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
|
||||||
|
|
@ -0,0 +1,186 @@
|
||||||
|
// mmctl.cpp - multimaterial switcher control
|
||||||
|
#include "main.h"
|
||||||
|
#include <Arduino.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <avr/io.h>
|
||||||
|
#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);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,539 @@
|
||||||
|
#include "motion.h"
|
||||||
|
#include "shr16.h"
|
||||||
|
#include "tmc2130.h"
|
||||||
|
#include <avr/io.h>
|
||||||
|
#include <avr/pgmspace.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <Arduino.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue