fsensor.cpp 7.2 KB

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