fsensor.cpp 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410
  1. #include "Marlin.h"
  2. #ifdef PAT9125
  3. #include "fsensor.h"
  4. #include "pat9125.h"
  5. #include "stepper.h"
  6. #include "planner.h"
  7. #include "fastio.h"
  8. #include "cmdqueue.h"
  9. const char ERRMSG_PAT9125_NOT_RESP[] PROGMEM = "PAT9125 not responding (%d)!\n";
  10. #define FSENSOR_INT_PIN 63 //filament sensor interrupt pin PK1
  11. #define FSENSOR_INT_PIN_MSK 0x02 //filament sensor interrupt pin mask (bit1)
  12. extern void stop_and_save_print_to_ram(float z_move, float e_move);
  13. extern void restore_print_from_ram_and_continue(float e_move);
  14. extern int8_t FSensorStateMenu;
  15. void fsensor_stop_and_save_print(void)
  16. {
  17. stop_and_save_print_to_ram(0, 0); //XYZE - no change
  18. }
  19. void fsensor_restore_print_and_continue(void)
  20. {
  21. restore_print_from_ram_and_continue(0); //XYZ = orig, E - no change
  22. }
  23. //uint8_t fsensor_int_pin = FSENSOR_INT_PIN;
  24. uint8_t fsensor_int_pin_old = 0;
  25. int16_t fsensor_chunk_len = FSENSOR_CHUNK_LEN;
  26. //enabled = initialized and sampled every chunk event
  27. bool fsensor_enabled = true;
  28. //runout watching is done in fsensor_update (called from main loop)
  29. bool fsensor_watch_runout = true;
  30. //not responding - is set if any communication error occured durring initialization or readout
  31. bool fsensor_not_responding = false;
  32. //number of errors, updated in ISR
  33. uint8_t fsensor_err_cnt = 0;
  34. //variable for accumolating step count
  35. int16_t fsensor_st_cnt = 0;
  36. //log flag: 0=log disabled, 1=log enabled
  37. uint8_t fsensor_log = 1;
  38. ////////////////////////////////////////////////////////////////////////////////
  39. //filament autoload variables
  40. //autoload feature enabled
  41. bool fsensor_autoload_enabled = true;
  42. //autoload watching enable/disable flag
  43. bool fsensor_watch_autoload = false;
  44. //
  45. uint16_t fsensor_autoload_y = 0;
  46. //
  47. uint8_t fsensor_autoload_c = 0;
  48. //
  49. uint32_t fsensor_autoload_last_millis = 0;
  50. //
  51. uint8_t fsensor_autoload_sum = 0;
  52. ////////////////////////////////////////////////////////////////////////////////
  53. //filament optical quality meassurement variables
  54. //meassurement enable/disable flag
  55. bool fsensor_oq_meassure = false;
  56. //skip-chunk counter, for accurate meassurement is necesary to skip first chunk...
  57. uint8_t fsensor_oq_skipchunk;
  58. //sum of steps in positive direction movements
  59. uint16_t fsensor_oq_st_sum;
  60. //sum of deltas in positive direction movements
  61. uint16_t fsensor_oq_yd_sum;
  62. //sum of errors durring meassurement
  63. uint16_t fsensor_oq_er_sum;
  64. //max error counter value durring meassurement
  65. uint8_t fsensor_oq_er_max;
  66. //minimum delta value
  67. uint16_t fsensor_oq_yd_min;
  68. //maximum delta value
  69. uint16_t fsensor_oq_yd_max;
  70. void fsensor_init(void)
  71. {
  72. int pat9125 = pat9125_init();
  73. printf_P(_N("PAT9125_init:%d\n"), pat9125);
  74. uint8_t fsensor = eeprom_read_byte((uint8_t*)EEPROM_FSENSOR);
  75. fsensor_autoload_enabled=eeprom_read_byte((uint8_t*)EEPROM_FSENS_AUTOLOAD_ENABLED);
  76. if (!pat9125)
  77. {
  78. fsensor = 0; //disable sensor
  79. fsensor_not_responding = true;
  80. }
  81. else
  82. {
  83. fsensor_not_responding = false;
  84. }
  85. puts_P(PSTR("FSensor "));
  86. if (fsensor)
  87. {
  88. fsensor_enable();
  89. puts_P(PSTR("ENABLED\n"));
  90. }
  91. else
  92. {
  93. fsensor_disable();
  94. puts_P(PSTR("DISABLED\n"));
  95. }
  96. }
  97. bool fsensor_enable(void)
  98. {
  99. uint8_t pat9125 = pat9125_init();
  100. printf_P(PSTR("PAT9125_init:%hhu\n"), pat9125);
  101. if (pat9125)
  102. fsensor_not_responding = false;
  103. else
  104. fsensor_not_responding = true;
  105. fsensor_enabled = pat9125?true:false;
  106. fsensor_watch_runout = true;
  107. fsensor_oq_meassure = false;
  108. fsensor_err_cnt = 0;
  109. eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, fsensor_enabled?0x01:0x00);
  110. FSensorStateMenu = fsensor_enabled?1:0;
  111. return fsensor_enabled;
  112. }
  113. void fsensor_disable(void)
  114. {
  115. fsensor_enabled = false;
  116. eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, 0x00);
  117. FSensorStateMenu = 0;
  118. }
  119. void fsensor_autoload_set(bool State)
  120. {
  121. fsensor_autoload_enabled = State;
  122. eeprom_update_byte((unsigned char *)EEPROM_FSENS_AUTOLOAD_ENABLED, fsensor_autoload_enabled);
  123. }
  124. void pciSetup(byte pin)
  125. {
  126. *digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin)); // enable pin
  127. PCIFR |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt
  128. PCICR |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group
  129. }
  130. void fsensor_autoload_check_start(void)
  131. {
  132. if (!fsensor_enabled) return;
  133. if (!fsensor_autoload_enabled) return;
  134. if (fsensor_watch_autoload) return;
  135. if (!pat9125_update_y()) //update sensor
  136. {
  137. fsensor_disable();
  138. fsensor_not_responding = true;
  139. fsensor_watch_autoload = false;
  140. printf_P(ERRMSG_PAT9125_NOT_RESP, 3);
  141. return;
  142. }
  143. fsensor_autoload_y = pat9125_y; //save current y value
  144. fsensor_autoload_c = 0; //reset number of changes counter
  145. fsensor_autoload_sum = 0;
  146. fsensor_autoload_last_millis = millis();
  147. fsensor_watch_runout = false;
  148. fsensor_watch_autoload = true;
  149. fsensor_err_cnt = 0;
  150. }
  151. void fsensor_autoload_check_stop(void)
  152. {
  153. if (!fsensor_enabled) return;
  154. if (!fsensor_autoload_enabled) return;
  155. if (!fsensor_watch_autoload) return;
  156. fsensor_autoload_sum = 0;
  157. fsensor_watch_autoload = false;
  158. fsensor_watch_runout = true;
  159. fsensor_err_cnt = 0;
  160. }
  161. bool fsensor_check_autoload(void)
  162. {
  163. if (!fsensor_enabled) return false;
  164. if (!fsensor_autoload_enabled) return false;
  165. if (!fsensor_watch_autoload)
  166. {
  167. fsensor_autoload_check_start();
  168. return false;
  169. }
  170. uint8_t fsensor_autoload_c_old = fsensor_autoload_c;
  171. if ((millis() - fsensor_autoload_last_millis) < 25) return false;
  172. fsensor_autoload_last_millis = millis();
  173. if (!pat9125_update_y())
  174. {
  175. fsensor_disable();
  176. fsensor_not_responding = true;
  177. printf_P(ERRMSG_PAT9125_NOT_RESP, 2);
  178. return false; //update sensor
  179. }
  180. int16_t dy = fsensor_autoload_y - pat9125_y;
  181. if (dy) //? y value is different
  182. {
  183. if (dy < 0) //? delta-y value is positive (inserting)
  184. {
  185. fsensor_autoload_sum -= dy;
  186. fsensor_autoload_c += 3; //increment change counter by 3
  187. }
  188. else if (fsensor_autoload_c > 1)
  189. fsensor_autoload_c -= 2; //decrement change counter by 2
  190. fsensor_autoload_y = pat9125_y; //save current value
  191. }
  192. else if (fsensor_autoload_c > 0)
  193. fsensor_autoload_c--;
  194. if (fsensor_autoload_c == 0) fsensor_autoload_sum = 0;
  195. // if (fsensor_autoload_c != fsensor_autoload_c_old)
  196. // printf_P(PSTR("fsensor_check_autoload dy=%d c=%d sum=%d\n"), dy, fsensor_autoload_c, fsensor_autoload_sum);
  197. if ((fsensor_autoload_c >= 15) && (fsensor_autoload_sum > 30))
  198. return true;
  199. return false;
  200. }
  201. void fsensor_oq_meassure_start(void)
  202. {
  203. fsensor_oq_skipchunk = 1;
  204. fsensor_oq_st_sum = 0;
  205. fsensor_oq_yd_sum = 0;
  206. fsensor_oq_er_sum = 0;
  207. fsensor_oq_er_max = 0;
  208. fsensor_oq_yd_min = FSENSOR_OQ_MAX_YD;
  209. fsensor_oq_yd_max = 0;
  210. pat9125_update_y();
  211. pat9125_y = 0;
  212. fsensor_watch_runout = false;
  213. fsensor_oq_meassure = true;
  214. }
  215. void fsensor_oq_meassure_stop(void)
  216. {
  217. printf_P(PSTR("fsensor_oq_meassure_stop\n"));
  218. printf_P(_N(" st_sum=%u yd_sum=%u er_sum=%u er_max=%hhu\n"), fsensor_oq_st_sum, fsensor_oq_yd_sum, fsensor_oq_er_sum, fsensor_oq_er_max);
  219. printf_P(_N(" yd_min=%u yd_max=%u yd_avg=%u\n"), fsensor_oq_yd_min, fsensor_oq_yd_max, fsensor_oq_yd_sum * FSENSOR_CHUNK_LEN / fsensor_oq_st_sum);
  220. fsensor_oq_meassure = false;
  221. fsensor_err_cnt = 0;
  222. fsensor_watch_runout = true;
  223. }
  224. bool fsensor_oq_result(void)
  225. {
  226. printf(_N("fsensor_oq_result\n"));
  227. if (fsensor_oq_er_sum > FSENSOR_OQ_MAX_ER) return false;
  228. printf(_N(" er_sum OK\n"));
  229. uint8_t yd_avg = fsensor_oq_yd_sum * FSENSOR_CHUNK_LEN / fsensor_oq_st_sum;
  230. if ((yd_avg < FSENSOR_OQ_MIN_YD) || (yd_avg > FSENSOR_OQ_MAX_YD)) return false;
  231. printf(_N(" yd_avg OK\n"));
  232. if (fsensor_oq_yd_max > (yd_avg * FSENSOR_OQ_MAX_PD)) return false;
  233. printf(_N(" yd_max OK\n"));
  234. if (fsensor_oq_yd_min < (yd_avg / FSENSOR_OQ_MAX_ND)) return false;
  235. printf(_N(" yd_min OK\n"));
  236. return true;
  237. }
  238. ISR(PCINT2_vect)
  239. {
  240. if (!((fsensor_int_pin_old ^ PINK) & FSENSOR_INT_PIN_MSK)) return;
  241. fsensor_int_pin_old = PINK;
  242. static bool _lock = false;
  243. if (_lock) return;
  244. _lock = true;
  245. int st_cnt = fsensor_st_cnt;
  246. fsensor_st_cnt = 0;
  247. sei();
  248. uint8_t old_err_cnt = fsensor_err_cnt;
  249. if (!pat9125_update_y())
  250. {
  251. fsensor_disable();
  252. fsensor_not_responding = true;
  253. printf_P(ERRMSG_PAT9125_NOT_RESP, 1);
  254. }
  255. if (st_cnt != 0)
  256. { //movement
  257. if (st_cnt > 0) //positive movement
  258. {
  259. if (pat9125_y <= 0)
  260. {
  261. fsensor_err_cnt++;
  262. }
  263. else
  264. {
  265. if (fsensor_err_cnt)
  266. fsensor_err_cnt--;
  267. }
  268. if (fsensor_oq_meassure)
  269. {
  270. if (fsensor_oq_skipchunk)
  271. fsensor_oq_skipchunk--;
  272. else
  273. {
  274. if (st_cnt == FSENSOR_CHUNK_LEN)
  275. {
  276. if (fsensor_oq_yd_min > pat9125_y) fsensor_oq_yd_min = (fsensor_oq_yd_min + pat9125_y) / 2;
  277. if (fsensor_oq_yd_max < pat9125_y) fsensor_oq_yd_max = (fsensor_oq_yd_max + pat9125_y) / 2;
  278. }
  279. fsensor_oq_st_sum += st_cnt;
  280. fsensor_oq_yd_sum += pat9125_y;
  281. if (fsensor_err_cnt > old_err_cnt)
  282. fsensor_oq_er_sum += (fsensor_err_cnt - old_err_cnt);
  283. if (fsensor_oq_er_max < fsensor_err_cnt)
  284. fsensor_oq_er_max = fsensor_err_cnt;
  285. }
  286. }
  287. }
  288. else //negative movement
  289. {
  290. }
  291. }
  292. else
  293. { //no movement
  294. }
  295. #ifdef DEBUG_FSENSOR_LOG
  296. if (fsensor_log)
  297. {
  298. printf_P(_N("FSENSOR cnt=%d dy=%d err=%d %S\n"), st_cnt, pat9125_y, fsensor_err_cnt, (fsensor_err_cnt > old_err_cnt)?_N("NG!"):_N("OK"));
  299. printf_P(_N("FSENSOR st_sum=%lu yd_sum=%lu er_sum=%u er_max=%u\n"), fsensor_oq_st_sum, fsensor_oq_yd_sum, fsensor_oq_er_sum, fsensor_oq_er_max);
  300. }
  301. #endif //DEBUG_FSENSOR_LOG
  302. pat9125_y = 0;
  303. _lock = false;
  304. return;
  305. }
  306. void fsensor_st_block_begin(block_t* bl)
  307. {
  308. if (!fsensor_enabled) return;
  309. if (((fsensor_st_cnt > 0) && (bl->direction_bits & 0x8)) ||
  310. ((fsensor_st_cnt < 0) && !(bl->direction_bits & 0x8)))
  311. {
  312. if (_READ(63)) _WRITE(63, LOW);
  313. else _WRITE(63, HIGH);
  314. }
  315. }
  316. void fsensor_st_block_chunk(block_t* bl, int cnt)
  317. {
  318. if (!fsensor_enabled) return;
  319. fsensor_st_cnt += (bl->direction_bits & 0x8)?-cnt:cnt;
  320. if ((fsensor_st_cnt >= fsensor_chunk_len) || (fsensor_st_cnt <= -fsensor_chunk_len))
  321. {
  322. if (_READ(63)) _WRITE(63, LOW);
  323. else _WRITE(63, HIGH);
  324. }
  325. }
  326. void fsensor_update(void)
  327. {
  328. if (fsensor_enabled && fsensor_watch_runout)
  329. if (fsensor_err_cnt > FSENSOR_ERR_MAX)
  330. {
  331. fsensor_stop_and_save_print();
  332. fsensor_err_cnt = 0;
  333. enquecommand_front_P((PSTR("G1 E-3 F200")));
  334. process_commands();
  335. cmdqueue_pop_front();
  336. st_synchronize();
  337. enquecommand_front_P((PSTR("G1 E3 F200")));
  338. process_commands();
  339. cmdqueue_pop_front();
  340. st_synchronize();
  341. if (fsensor_err_cnt == 0)
  342. {
  343. fsensor_restore_print_and_continue();
  344. }
  345. else
  346. {
  347. eeprom_update_byte((uint8_t*)EEPROM_FERROR_COUNT, eeprom_read_byte((uint8_t*)EEPROM_FERROR_COUNT) + 1);
  348. eeprom_update_word((uint16_t*)EEPROM_FERROR_COUNT_TOT, eeprom_read_word((uint16_t*)EEPROM_FERROR_COUNT_TOT) + 1);
  349. enquecommand_front_P((PSTR("M600")));
  350. fsensor_watch_runout = false;
  351. }
  352. }
  353. }
  354. void fsensor_setup_interrupt(void)
  355. {
  356. // uint8_t fsensor_int_pin = FSENSOR_INT_PIN;
  357. // uint8_t fsensor_int_pcmsk = digitalPinToPCMSKbit(pin);
  358. // uint8_t fsensor_int_pcicr = digitalPinToPCICRbit(pin);
  359. pinMode(FSENSOR_INT_PIN, OUTPUT);
  360. digitalWrite(FSENSOR_INT_PIN, LOW);
  361. fsensor_int_pin_old = 0;
  362. pciSetup(FSENSOR_INT_PIN);
  363. }
  364. #endif //PAT9125