
PAT9125 HWI2C (Wire) support, fsensor implementation into separate file build number = 102
206 lines
5.1 KiB
C++
206 lines
5.1 KiB
C++
#include "Marlin.h"
|
|
|
|
#ifdef PAT9125
|
|
|
|
#include "fsensor.h"
|
|
#include "pat9125.h"
|
|
#include "planner.h"
|
|
|
|
extern void stop_and_save_print_to_ram(float z_move, float e_move);
|
|
extern void restore_print_from_ram_and_continue(float e_move);
|
|
extern long st_get_position(uint8_t axis);
|
|
|
|
|
|
void fsensor_stop_and_save_print()
|
|
{
|
|
// stop_and_save_print_to_ram(10, -0.8); //XY - no change, Z 10mm up, E 0.8mm in
|
|
stop_and_save_print_to_ram(0, 0); //XYZE - no change
|
|
}
|
|
|
|
void fsensor_restore_print_and_continue()
|
|
{
|
|
restore_print_from_ram_and_continue(0); //XYZ = orig, E - no change
|
|
}
|
|
|
|
|
|
bool fsensor_enabled = true;
|
|
bool fsensor_ignore_error = true;
|
|
bool fsensor_M600 = false;
|
|
long fsensor_prev_pos_e = 0;
|
|
uint8_t fsensor_err_cnt = 0;
|
|
|
|
#define FSENS_ESTEPS 280 //extruder resolution [steps/mm]
|
|
//#define FSENS_MINDEL 560 //filament sensor min delta [steps] (3mm)
|
|
#define FSENS_MINDEL 280 //filament sensor min delta [steps] (3mm)
|
|
#define FSENS_MINFAC 3 //filament sensor minimum factor [count/mm]
|
|
//#define FSENS_MAXFAC 50 //filament sensor maximum factor [count/mm]
|
|
#define FSENS_MAXFAC 40 //filament sensor maximum factor [count/mm]
|
|
//#define FSENS_MAXERR 2 //filament sensor max error count
|
|
#define FSENS_MAXERR 5 //filament sensor max error count
|
|
|
|
extern int8_t FSensorStateMenu;
|
|
|
|
|
|
void fsensor_enable()
|
|
{
|
|
MYSERIAL.println("fsensor_enable");
|
|
pat9125_y = 0;
|
|
fsensor_prev_pos_e = st_get_position(E_AXIS);
|
|
fsensor_err_cnt = 0;
|
|
fsensor_enabled = true;
|
|
fsensor_ignore_error = true;
|
|
fsensor_M600 = false;
|
|
eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, 0xFF);
|
|
FSensorStateMenu = 1;
|
|
}
|
|
|
|
void fsensor_disable()
|
|
{
|
|
MYSERIAL.println("fsensor_disable");
|
|
fsensor_enabled = false;
|
|
eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, 0x00);
|
|
FSensorStateMenu = 0;
|
|
}
|
|
|
|
#include "LiquidCrystal.h"
|
|
extern LiquidCrystal lcd;
|
|
|
|
//bool pat9125_change = ((old_x != pat9125_x) || (old_y != pat9125_y));
|
|
|
|
uint8_t fsensor_int_pin = 63;
|
|
int16_t fsensor_steps_e = 0;
|
|
int16_t fsensor_y_old = 0;
|
|
|
|
|
|
|
|
void pciSetup(byte pin)
|
|
{
|
|
*digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin)); // enable pin
|
|
PCIFR |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt
|
|
PCICR |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group
|
|
}
|
|
|
|
void fsensor_setup_interrupt()
|
|
{
|
|
uint8_t fsensor_int_pin = 63;
|
|
|
|
pinMode(fsensor_int_pin, OUTPUT);
|
|
digitalWrite(fsensor_int_pin, HIGH);
|
|
|
|
pciSetup(fsensor_int_pin);
|
|
}
|
|
|
|
void fsensor_interrupt_raise()
|
|
{
|
|
digitalWrite(fsensor_int_pin, LOW);
|
|
}
|
|
|
|
ISR(PCINT2_vect)
|
|
{
|
|
sei();
|
|
SERIAL_ECHOLNPGM("PCINT2");
|
|
*digitalPinToPCMSK(fsensor_int_pin) &= ~bit(digitalPinToPCMSKbit(fsensor_int_pin));
|
|
digitalWrite(fsensor_int_pin, HIGH);
|
|
*digitalPinToPCMSK(fsensor_int_pin) |= bit(digitalPinToPCMSKbit(fsensor_int_pin));
|
|
// PCIFR |= bit (digitalPinToPCICRbit(fsensor_int_pin)); // clear any outstanding interrupt
|
|
// pat9125_update();
|
|
pat9125_update();
|
|
MYSERIAL.print("steps_e=");
|
|
MYSERIAL.print(fsensor_steps_e, DEC);
|
|
MYSERIAL.print(" dy=");
|
|
MYSERIAL.println(pat9125_y - fsensor_y_old, DEC);
|
|
fsensor_y_old = pat9125_y;
|
|
}
|
|
|
|
void fsensor_st_end_block(block_t* bl)
|
|
{
|
|
// return;
|
|
fsensor_steps_e = bl->steps_e;
|
|
digitalWrite(fsensor_int_pin, LOW);
|
|
}
|
|
|
|
void fsensor_st_new_block(block_t* bl)
|
|
{
|
|
// return;
|
|
// fsensor_steps_e = bl->steps_e;
|
|
// digitalWrite(fsensor_int_pin, LOW);
|
|
}
|
|
|
|
void fsensor_update()
|
|
{
|
|
// return;
|
|
if (!fsensor_enabled) return;
|
|
long pos_e = st_get_position(E_AXIS); //current position
|
|
int old_x = pat9125_x;
|
|
int old_y = pat9125_y;
|
|
pat9125_update();
|
|
/* bool pat9125_change = ((old_x != pat9125_x) || (old_y != pat9125_y));
|
|
static uint32_t checktime = 0;
|
|
pat9125_change |= (millis() - checktime > 250);
|
|
#ifdef DEBUG_PAT9125_COUNTERS
|
|
if (pat9125_change)
|
|
{
|
|
lcd.setCursor(2, 3);
|
|
lcd.print(pat9125_x, DEC);
|
|
lcd.print(' ');
|
|
lcd.print(pat9125_y, DEC);
|
|
lcd.print(' ');
|
|
lcd.print(pos_e, DEC);
|
|
lcd.print(' ');
|
|
checktime = millis();
|
|
}
|
|
#endif DEBUG_PAT9125_COUNTERS
|
|
return;*/
|
|
|
|
long del_e = pos_e - fsensor_prev_pos_e; //delta
|
|
if (abs(del_e) < FSENS_MINDEL) return;
|
|
float de = ((float)del_e / FSENS_ESTEPS);
|
|
int cmin = de * FSENS_MINFAC;
|
|
int cmax = de * FSENS_MAXFAC;
|
|
int cnt = -pat9125_y;
|
|
fsensor_prev_pos_e = pos_e;
|
|
pat9125_y = 0;
|
|
bool err = false;
|
|
if ((del_e > 0) && ((cnt < cmin) || (cnt > cmax))) err = true;
|
|
if ((del_e < 0) && ((cnt > cmin) || (cnt < cmax))) err = true;
|
|
if (err)
|
|
fsensor_err_cnt++;
|
|
else
|
|
fsensor_err_cnt = 0;
|
|
|
|
/**/
|
|
MYSERIAL.print("pos_e=");
|
|
MYSERIAL.print(pos_e);
|
|
MYSERIAL.print(" de=");
|
|
MYSERIAL.print(de);
|
|
MYSERIAL.print(" cmin=");
|
|
MYSERIAL.print((int)cmin);
|
|
MYSERIAL.print(" cmax=");
|
|
MYSERIAL.print((int)cmax);
|
|
MYSERIAL.print(" cnt=");
|
|
MYSERIAL.print((int)cnt);
|
|
MYSERIAL.print(" err=");
|
|
MYSERIAL.println((int)fsensor_err_cnt);/**/
|
|
|
|
// return;
|
|
|
|
if (fsensor_err_cnt > FSENS_MAXERR)
|
|
{
|
|
MYSERIAL.println("fsensor_update (fsensor_err_cnt > FSENS_MAXERR)");
|
|
if (fsensor_ignore_error)
|
|
{
|
|
MYSERIAL.println("fsensor_update - error ignored)");
|
|
fsensor_ignore_error = false;
|
|
}
|
|
else
|
|
{
|
|
MYSERIAL.println("fsensor_update - ERROR!!!");
|
|
fsensor_stop_and_save_print();
|
|
enquecommand_front_P((PSTR("M600")));
|
|
fsensor_M600 = true;
|
|
fsensor_enabled = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif //PAT9125
|