1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
|
#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
|