fsensor.cpp 7.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291
  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. //#include "LiquidCrystal_Prusa.h"
  10. //extern LiquidCrystal_Prusa lcd;
  11. #define FSENSOR_ERR_MAX 5 //filament sensor max error count
  12. #define FSENSOR_INT_PIN 63 //filament sensor interrupt pin PK1
  13. #define FSENSOR_INT_PIN_MSK 0x02 //filament sensor interrupt pin mask (bit1)
  14. #define FSENSOR_CHUNK_LEN 280 //filament sensor chunk length in steps
  15. extern void stop_and_save_print_to_ram(float z_move, float e_move);
  16. extern void restore_print_from_ram_and_continue(float e_move);
  17. extern int8_t FSensorStateMenu;
  18. void fsensor_stop_and_save_print()
  19. {
  20. stop_and_save_print_to_ram(0, 0); //XYZE - no change
  21. }
  22. void fsensor_restore_print_and_continue()
  23. {
  24. restore_print_from_ram_and_continue(0); //XYZ = orig, E - no change
  25. }
  26. //uint8_t fsensor_int_pin = FSENSOR_INT_PIN;
  27. uint8_t fsensor_int_pin_old = 0;
  28. int16_t fsensor_chunk_len = FSENSOR_CHUNK_LEN;
  29. bool fsensor_enabled = true;
  30. bool fsensor_not_responding = false;
  31. //bool fsensor_ignore_error = true;
  32. bool fsensor_M600 = false;
  33. uint8_t fsensor_err_cnt = 0;
  34. int16_t fsensor_st_cnt = 0;
  35. uint8_t fsensor_log = 1;
  36. //autoload enable/disable flag
  37. bool fsensor_autoload_enabled = false;
  38. uint16_t fsensor_autoload_y = 0;
  39. uint8_t fsensor_autoload_c = 0;
  40. uint32_t fsensor_autoload_last_millis = 0;
  41. uint8_t fsensor_autoload_sum = 0;
  42. void fsensor_block()
  43. {
  44. fsensor_enabled = false;
  45. }
  46. void fsensor_unblock() {
  47. fsensor_enabled = (eeprom_read_byte((uint8_t*)EEPROM_FSENSOR) == 0x01);
  48. }
  49. bool fsensor_enable()
  50. {
  51. // puts_P(PSTR("fsensor_enable\n"));
  52. int pat9125 = pat9125_init();
  53. // printf_P(PSTR("PAT9125_init:%d\n"), pat9125);
  54. if (pat9125)
  55. fsensor_not_responding = false;
  56. else
  57. fsensor_not_responding = true;
  58. fsensor_enabled = pat9125?true:false;
  59. // fsensor_ignore_error = true;
  60. fsensor_M600 = false;
  61. fsensor_err_cnt = 0;
  62. eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, fsensor_enabled?0x01:0x00);
  63. FSensorStateMenu = fsensor_enabled?1:0;
  64. return fsensor_enabled;
  65. }
  66. void fsensor_disable()
  67. {
  68. // puts_P(PSTR("fsensor_disable\n"));
  69. fsensor_enabled = false;
  70. eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, 0x00);
  71. FSensorStateMenu = 0;
  72. }
  73. void pciSetup(byte pin)
  74. {
  75. *digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin)); // enable pin
  76. PCIFR |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt
  77. PCICR |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group
  78. }
  79. void fsensor_setup_interrupt()
  80. {
  81. // uint8_t fsensor_int_pin = FSENSOR_INT_PIN;
  82. // uint8_t fsensor_int_pcmsk = digitalPinToPCMSKbit(pin);
  83. // uint8_t fsensor_int_pcicr = digitalPinToPCICRbit(pin);
  84. pinMode(FSENSOR_INT_PIN, OUTPUT);
  85. digitalWrite(FSENSOR_INT_PIN, LOW);
  86. fsensor_int_pin_old = 0;
  87. pciSetup(FSENSOR_INT_PIN);
  88. }
  89. void fsensor_autoload_check_start(void)
  90. {
  91. // puts_P(PSTR("fsensor_autoload_check_start\n"));
  92. pat9125_update_y(); //update sensor
  93. fsensor_autoload_y = pat9125_y; //save current y value
  94. fsensor_autoload_c = 0; //reset number of changes counter
  95. fsensor_autoload_sum = 0;
  96. fsensor_autoload_last_millis = millis();
  97. fsensor_autoload_enabled = true;
  98. fsensor_err_cnt = 0;
  99. }
  100. void fsensor_autoload_check_stop(void)
  101. {
  102. // puts_P(PSTR("fsensor_autoload_check_stop\n"));
  103. fsensor_autoload_sum = 0;
  104. fsensor_autoload_enabled = false;
  105. fsensor_err_cnt = 0;
  106. }
  107. bool fsensor_check_autoload(void)
  108. {
  109. uint8_t fsensor_autoload_c_old = fsensor_autoload_c;
  110. if ((millis() - fsensor_autoload_last_millis) < 25) return false;
  111. fsensor_autoload_last_millis = millis();
  112. pat9125_update_y(); //update sensor
  113. int16_t dy = fsensor_autoload_y - pat9125_y;
  114. if (dy) //? y value is different
  115. {
  116. if (dy < 0) //? delta-y value is positive (inserting)
  117. {
  118. fsensor_autoload_sum -= dy;
  119. fsensor_autoload_c += 3; //increment change counter by 3
  120. }
  121. else if (fsensor_autoload_c > 1)
  122. fsensor_autoload_c -= 2; //decrement change counter by 2
  123. fsensor_autoload_y = pat9125_y; //save current value
  124. }
  125. else if (fsensor_autoload_c > 0)
  126. fsensor_autoload_c--;
  127. if (fsensor_autoload_c == 0) fsensor_autoload_sum = 0;
  128. // if (fsensor_autoload_c != fsensor_autoload_c_old)
  129. // printf_P(PSTR("fsensor_check_autoload dy=%d c=%d sum=%d\n"), dy, fsensor_autoload_c, fsensor_autoload_sum);
  130. if ((fsensor_autoload_c >= 15) && (fsensor_autoload_sum > 30))
  131. return true;
  132. return false;
  133. }
  134. ISR(PCINT2_vect)
  135. {
  136. // puts("PCINT2\n");
  137. if (!((fsensor_int_pin_old ^ PINK) & FSENSOR_INT_PIN_MSK)) return;
  138. fsensor_int_pin_old = PINK;
  139. static bool _lock = false;
  140. if (_lock) return;
  141. _lock = true;
  142. // return;
  143. int st_cnt = fsensor_st_cnt;
  144. fsensor_st_cnt = 0;
  145. sei();
  146. /* *digitalPinToPCMSK(fsensor_int_pin) &= ~bit(digitalPinToPCMSKbit(fsensor_int_pin));
  147. digitalWrite(fsensor_int_pin, HIGH);
  148. *digitalPinToPCMSK(fsensor_int_pin) |= bit(digitalPinToPCMSKbit(fsensor_int_pin));*/
  149. if (!pat9125_update_y())
  150. {
  151. #ifdef DEBUG_FSENSOR_LOG
  152. puts_P(PSTR("pat9125 not responding.\n"));
  153. #endif //DEBUG_FSENSOR_LOG
  154. fsensor_disable();
  155. fsensor_not_responding = true;
  156. }
  157. if (st_cnt != 0)
  158. {
  159. #ifdef DEBUG_FSENSOR_LOG
  160. if (fsensor_log)
  161. {
  162. MYSERIAL.print("cnt=");
  163. MYSERIAL.print(st_cnt, DEC);
  164. MYSERIAL.print(" dy=");
  165. MYSERIAL.print(pat9125_y, DEC);
  166. }
  167. #endif //DEBUG_FSENSOR_LOG
  168. if (st_cnt != 0)
  169. {
  170. if( (pat9125_y == 0) || ((pat9125_y > 0) && (st_cnt < 0)) || ((pat9125_y < 0) && (st_cnt > 0)))
  171. { //invalid movement
  172. if (st_cnt > 0) //only positive movements
  173. fsensor_err_cnt++;
  174. #ifdef DEBUG_FSENSOR_LOG
  175. if (fsensor_log)
  176. {
  177. MYSERIAL.print("\tNG ! err=");
  178. MYSERIAL.println(fsensor_err_cnt, DEC);
  179. }
  180. #endif //DEBUG_FSENSOR_LOG
  181. }
  182. else
  183. { //propper movement
  184. if (fsensor_err_cnt > 0)
  185. fsensor_err_cnt--;
  186. // fsensor_err_cnt = 0;
  187. #ifdef DEBUG_FSENSOR_LOG
  188. if (fsensor_log)
  189. {
  190. MYSERIAL.print("\tOK err=");
  191. MYSERIAL.println(fsensor_err_cnt, DEC);
  192. }
  193. #endif //DEBUG_FSENSOR_LOG
  194. }
  195. }
  196. else
  197. { //no movement
  198. #ifdef DEBUG_FSENSOR_LOG
  199. if (fsensor_log)
  200. MYSERIAL.println("\tOK 0");
  201. #endif //DEBUG_FSENSOR_LOG
  202. }
  203. }
  204. pat9125_y = 0;
  205. _lock = false;
  206. return;
  207. }
  208. void fsensor_st_block_begin(block_t* bl)
  209. {
  210. if (!fsensor_enabled) return;
  211. if (((fsensor_st_cnt > 0) && (bl->direction_bits & 0x8)) ||
  212. ((fsensor_st_cnt < 0) && !(bl->direction_bits & 0x8)))
  213. {
  214. if (_READ(63)) _WRITE(63, LOW);
  215. else _WRITE(63, HIGH);
  216. }
  217. // PINK |= FSENSOR_INT_PIN_MSK; //toggle pin
  218. // _WRITE(fsensor_int_pin, LOW);
  219. }
  220. void fsensor_st_block_chunk(block_t* bl, int cnt)
  221. {
  222. if (!fsensor_enabled) return;
  223. fsensor_st_cnt += (bl->direction_bits & 0x8)?-cnt:cnt;
  224. if ((fsensor_st_cnt >= fsensor_chunk_len) || (fsensor_st_cnt <= -fsensor_chunk_len))
  225. {
  226. if (_READ(63)) _WRITE(63, LOW);
  227. else _WRITE(63, HIGH);
  228. }
  229. // PINK |= FSENSOR_INT_PIN_MSK; //toggle pin
  230. // _WRITE(fsensor_int_pin, LOW);
  231. }
  232. void fsensor_update()
  233. {
  234. if (!fsensor_enabled) return;
  235. if (fsensor_err_cnt > FSENSOR_ERR_MAX)
  236. {
  237. fsensor_stop_and_save_print();
  238. fsensor_err_cnt = 0;
  239. enquecommand_front_P((PSTR("G1 E-3 F200")));
  240. process_commands();
  241. cmdqueue_pop_front();
  242. st_synchronize();
  243. enquecommand_front_P((PSTR("G1 E3 F200")));
  244. process_commands();
  245. cmdqueue_pop_front();
  246. st_synchronize();
  247. if (fsensor_err_cnt == 0)
  248. {
  249. fsensor_restore_print_and_continue();
  250. }
  251. else
  252. {
  253. eeprom_update_byte((uint8_t*)EEPROM_FERROR_COUNT, eeprom_read_byte((uint8_t*)EEPROM_FERROR_COUNT) + 1);
  254. eeprom_update_word((uint16_t*)EEPROM_FERROR_COUNT_TOT, eeprom_read_word((uint16_t*)EEPROM_FERROR_COUNT_TOT) + 1);
  255. enquecommand_front_P((PSTR("M600")));
  256. fsensor_M600 = true;
  257. fsensor_enabled = false;
  258. }
  259. }
  260. }
  261. #endif //PAT9125