josh
/
Prusa-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

unload_filament_withSensor rewrite 

Pending Testing.
This commit is contained in:
TheZeroBeast 2018-11-20 11:35:22 +10:00 committed by GitHub
parent e297131789
commit f6a8eb56d7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 1229 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

551
main.cpp Normal file
View File

@ -0,0 +1,551 @@
//! @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 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;
}

146
mmctl.cpp Normal file
View File

@ -0,0 +1,146 @@
// 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;
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);
}

492
motion.cpp Normal file
View File

@ -0,0 +1,492 @@
#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; // 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;
}

40
motion.h Normal file
View File

@ -0,0 +1,40 @@
// motion.h
#ifndef _MOTION_h
#define _MOTION_h
#include "config.h"
#include <inttypes.h>
#include <stdbool.h>
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