xyzcal.cpp 23 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805
  1. //xyzcal.cpp - xyz calibration with image processing
  2. #include "Configuration_prusa.h"
  3. #ifdef NEW_XYZCAL
  4. #include "xyzcal.h"
  5. #include <avr/wdt.h>
  6. #include "stepper.h"
  7. #include "temperature.h"
  8. #include "sm4.h"
  9. #define XYZCAL_PINDA_HYST_MIN 20 //50um
  10. #define XYZCAL_PINDA_HYST_MAX 100 //250um
  11. #define XYZCAL_PINDA_HYST_DIF 5 //12.5um
  12. #define ENABLE_FANCHECK_INTERRUPT() EIMSK |= (1<<7)
  13. #define DISABLE_FANCHECK_INTERRUPT() EIMSK &= ~(1<<7)
  14. #define _PINDA ((READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)?1:0)
  15. #define DBG(args...) printf_P(args)
  16. //#define DBG(args...)
  17. #ifndef _n
  18. #define _n PSTR
  19. #endif //_n
  20. #define _X ((int16_t)count_position[X_AXIS])
  21. #define _Y ((int16_t)count_position[Y_AXIS])
  22. #define _Z ((int16_t)count_position[Z_AXIS])
  23. #define _E ((int16_t)count_position[E_AXIS])
  24. #define X_PLUS 0
  25. #define X_MINUS 1
  26. #define Y_PLUS 0
  27. #define Y_MINUS 1
  28. #define Z_PLUS 0
  29. #define Z_MINUS 1
  30. /// 10000 = 1 mm/s
  31. #define MAX_DELAY 10000
  32. #define Z_ACCEL 300
  33. #define XY_ACCEL 1000
  34. #define _PI 3.14159265F
  35. /// \returns positive value always
  36. #define ABS(a) \
  37. ({ __typeof__ (a) _a = (a); \
  38. _a >= 0 ? _a : (-_a); })
  39. /// \returns maximum of the two
  40. #define MAX(a, b) \
  41. ({ __typeof__ (a) _a = (a); \
  42. __typeof__ (b) _b = (b); \
  43. _a >= _b ? _a : _b; })
  44. /// \returns minimum of the two
  45. #define MIN(a, b) \
  46. ({ __typeof__ (a) _a = (a); \
  47. __typeof__ (b) _b = (b); \
  48. _a <= _b ? _a : _b; })
  49. /// swap values
  50. #define SWAP(a, b) \
  51. ({ __typeof__ (a) c = (a); \
  52. a = (b); \
  53. b = c; })
  54. /// Saturates value
  55. /// \returns min if value is less than min
  56. /// \returns max if value is more than min
  57. /// \returns value otherwise
  58. #define CLAMP(value, min, max) \
  59. ({ __typeof__ (value) a_ = (value); \
  60. __typeof__ (min) min_ = (min); \
  61. __typeof__ (max) max_ = (max); \
  62. ( a_ < min_ ? min_ : (a_ <= max_ ? a_ : max_)); })
  63. /// position types
  64. typedef int16_t pos_i16_t;
  65. typedef long pos_i32_t;
  66. typedef float pos_mm_t;
  67. typedef int16_t usteps_t;
  68. uint8_t check_pinda_0();
  69. uint8_t check_pinda_1();
  70. void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
  71. uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd);
  72. uint8_t round_to_u8(float f){
  73. return (uint8_t)(f + .5f);
  74. }
  75. uint16_t round_to_u16(float f){
  76. return (uint16_t)(f + .5f);
  77. }
  78. int16_t round_to_i16(float f){
  79. return (int16_t)(f + .5f);
  80. }
  81. /// converts millimeters to integer position
  82. pos_i16_t mm_2_pos(pos_mm_t mm){
  83. return (pos_i16_t)(0.5f + mm * 100);
  84. }
  85. /// converts integer position to millimeters
  86. pos_mm_t pos_2_mm(pos_i16_t pos){
  87. return pos * 0.01f;
  88. }
  89. pos_mm_t pos_2_mm(float pos){
  90. return pos * 0.01f;
  91. }
  92. void xyzcal_meassure_enter(void)
  93. {
  94. DBG(_n("xyzcal_meassure_enter\n"));
  95. disable_heater();
  96. DISABLE_TEMPERATURE_INTERRUPT();
  97. #if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
  98. DISABLE_FANCHECK_INTERRUPT();
  99. #endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
  100. DISABLE_STEPPER_DRIVER_INTERRUPT();
  101. #ifdef WATCHDOG
  102. wdt_disable();
  103. #endif //WATCHDOG
  104. sm4_stop_cb = 0;
  105. sm4_update_pos_cb = xyzcal_update_pos;
  106. sm4_calc_delay_cb = xyzcal_calc_delay;
  107. }
  108. void xyzcal_meassure_leave(void)
  109. {
  110. DBG(_n("xyzcal_meassure_leave\n"));
  111. planner_abort_hard();
  112. ENABLE_TEMPERATURE_INTERRUPT();
  113. #if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
  114. ENABLE_FANCHECK_INTERRUPT();
  115. #endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
  116. ENABLE_STEPPER_DRIVER_INTERRUPT();
  117. #ifdef WATCHDOG
  118. wdt_enable(WDTO_4S);
  119. #endif //WATCHDOG
  120. sm4_stop_cb = 0;
  121. sm4_update_pos_cb = 0;
  122. sm4_calc_delay_cb = 0;
  123. }
  124. uint8_t check_pinda_0()
  125. {
  126. return _PINDA?0:1;
  127. }
  128. uint8_t check_pinda_1()
  129. {
  130. return _PINDA?1:0;
  131. }
  132. uint8_t xyzcal_dm = 0;
  133. void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t)
  134. {
  135. // DBG(_n("xyzcal_update_pos dx=%d dy=%d dz=%d dir=%02x\n"), dx, dy, dz, xyzcal_dm);
  136. if (xyzcal_dm&1) count_position[0] -= dx; else count_position[0] += dx;
  137. if (xyzcal_dm&2) count_position[1] -= dy; else count_position[1] += dy;
  138. if (xyzcal_dm&4) count_position[2] -= dz; else count_position[2] += dz;
  139. // DBG(_n(" after xyzcal_update_pos x=%ld y=%ld z=%ld\n"), count_position[0], count_position[1], count_position[2]);
  140. }
  141. uint16_t xyzcal_sm4_delay = 0;
  142. //#define SM4_ACCEL_TEST
  143. #ifdef SM4_ACCEL_TEST
  144. uint16_t xyzcal_sm4_v0 = 2000;
  145. uint16_t xyzcal_sm4_vm = 45000;
  146. uint16_t xyzcal_sm4_v = xyzcal_sm4_v0;
  147. uint16_t xyzcal_sm4_ac = 2000;
  148. uint16_t xyzcal_sm4_ac2 = (uint32_t)xyzcal_sm4_ac * 1024 / 10000;
  149. //float xyzcal_sm4_vm = 10000;
  150. #endif //SM4_ACCEL_TEST
  151. #ifdef SM4_ACCEL_TEST
  152. uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd)
  153. {
  154. uint16_t del_us = 0;
  155. if (xyzcal_sm4_v & 0xf000) //>=4096
  156. {
  157. del_us = (uint16_t)62500 / (uint16_t)(xyzcal_sm4_v >> 4);
  158. xyzcal_sm4_v += (xyzcal_sm4_ac2 * del_us + 512) >> 10;
  159. if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
  160. if (del_us > 25) return del_us - 25;
  161. }
  162. else
  163. {
  164. del_us = (uint32_t)1000000 / xyzcal_sm4_v;
  165. xyzcal_sm4_v += ((uint32_t)xyzcal_sm4_ac2 * del_us + 512) >> 10;
  166. if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
  167. if (del_us > 50) return del_us - 50;
  168. }
  169. // uint16_t del_us = (uint16_t)(((float)1000000 / xyzcal_sm4_v) + 0.5);
  170. // uint16_t del_us = (uint32_t)1000000 / xyzcal_sm4_v;
  171. // uint16_t del_us = 100;
  172. // uint16_t del_us = (uint16_t)10000 / xyzcal_sm4_v;
  173. // v += (ac * del_us + 500) / 1000;
  174. // xyzcal_sm4_v += (xyzcal_sm4_ac * del_us) / 1000;
  175. // return xyzcal_sm4_delay;
  176. // DBG(_n("xyzcal_calc_delay nd=%d dd=%d v=%d del_us=%d\n"), nd, dd, xyzcal_sm4_v, del_us);
  177. return 0;
  178. }
  179. #else //SM4_ACCEL_TEST
  180. uint16_t xyzcal_calc_delay(uint16_t, uint16_t)
  181. {
  182. return xyzcal_sm4_delay;
  183. }
  184. #endif //SM4_ACCEL_TEST
  185. /// Moves printer to absolute position [x,y,z] defined in integer position system
  186. bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda)
  187. {
  188. // DBG(_n("xyzcal_lineXYZ_to x=%d y=%d z=%d check=%d\n"), x, y, z, check_pinda);
  189. x -= (int16_t)count_position[0];
  190. y -= (int16_t)count_position[1];
  191. z -= (int16_t)count_position[2];
  192. xyzcal_dm = ((x<0)?1:0) | ((y<0)?2:0) | ((z<0)?4:0);
  193. sm4_set_dir_bits(xyzcal_dm);
  194. sm4_stop_cb = check_pinda?((check_pinda<0)?check_pinda_0:check_pinda_1):0;
  195. xyzcal_sm4_delay = delay_us;
  196. // uint32_t u = _micros();
  197. bool ret = sm4_line_xyze_ui(abs(x), abs(y), abs(z), 0) ? true : false;
  198. // u = _micros() - u;
  199. return ret;
  200. }
  201. /// Moves printer to absolute position [x,y,z] defined in millimeters
  202. bool xyzcal_lineXYZ_to_float(pos_mm_t x, pos_mm_t y, pos_mm_t z, uint16_t delay_us, int8_t check_pinda){
  203. return xyzcal_lineXYZ_to(mm_2_pos(x), mm_2_pos(y), mm_2_pos(z), delay_us, check_pinda);
  204. }
  205. bool xyzcal_spiral2(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, int16_t rotation, uint16_t delay_us, int8_t check_pinda, uint16_t* pad)
  206. {
  207. bool ret = false;
  208. float r = 0; //radius
  209. uint8_t n = 0; //point number
  210. uint16_t ad = 0; //angle [deg]
  211. float ar; //angle [rad]
  212. uint8_t dad = 0; //delta angle [deg]
  213. uint8_t dad_min = 4; //delta angle min [deg]
  214. uint8_t dad_max = 16; //delta angle max [deg]
  215. uint8_t k = 720 / (dad_max - dad_min); //delta calculation constant
  216. ad = 0;
  217. if (pad) ad = *pad % 720;
  218. DBG(_n("xyzcal_spiral2 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
  219. // lcd_set_cursor(0, 4);
  220. // char text[10];
  221. // snprintf(text, 10, "%4d", z0);
  222. // lcd_print(text);
  223. for (; ad < 720; ad++)
  224. {
  225. if (radius > 0)
  226. {
  227. dad = dad_max - (ad / k);
  228. r = (float)(((uint32_t)ad) * radius) / 720;
  229. }
  230. else
  231. {
  232. dad = dad_max - ((719 - ad) / k);
  233. r = (float)(((uint32_t)(719 - ad)) * (-radius)) / 720;
  234. }
  235. ar = (ad + rotation)* (float)_PI / 180;
  236. float _cos = cos(ar);
  237. float _sin = sin(ar);
  238. int x = (int)(cx + (_cos * r));
  239. int y = (int)(cy + (_sin * r));
  240. int z = (int)(z0 - ((float)((int32_t)dz * ad) / 720));
  241. if (xyzcal_lineXYZ_to(x, y, z, delay_us, check_pinda))
  242. {
  243. ad += dad + 1;
  244. ret = true;
  245. break;
  246. }
  247. n++;
  248. ad += dad;
  249. }
  250. if (pad) *pad = ad;
  251. // if(ret){
  252. // lcd_set_cursor(0, 4);
  253. // lcd_print(" ");
  254. // }
  255. return ret;
  256. }
  257. bool xyzcal_spiral8(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, uint16_t delay_us, int8_t check_pinda, uint16_t* pad)
  258. {
  259. bool ret = false;
  260. uint16_t ad = 0;
  261. if (pad) ad = *pad;
  262. DBG(_n("xyzcal_spiral8 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
  263. if (!ret && (ad < 720))
  264. if ((ret = xyzcal_spiral2(cx, cy, z0 - 0*dz, dz, radius, 0, delay_us, check_pinda, &ad)) != 0)
  265. ad += 0;
  266. if (!ret && (ad < 1440))
  267. if ((ret = xyzcal_spiral2(cx, cy, z0 - 1*dz, dz, -radius, 0, delay_us, check_pinda, &ad)) != 0)
  268. ad += 720;
  269. if (!ret && (ad < 2160))
  270. if ((ret = xyzcal_spiral2(cx, cy, z0 - 2*dz, dz, radius, 180, delay_us, check_pinda, &ad)) != 0)
  271. ad += 1440;
  272. if (!ret && (ad < 2880))
  273. if ((ret = xyzcal_spiral2(cx, cy, z0 - 3*dz, dz, -radius, 180, delay_us, check_pinda, &ad)) != 0)
  274. ad += 2160;
  275. if (pad) *pad = ad;
  276. return ret;
  277. }
  278. #ifdef XYZCAL_MEASSURE_PINDA_HYSTEREZIS
  279. int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples)
  280. {
  281. DBG(_n("xyzcal_meassure_pinda_hysterezis\n"));
  282. int8_t ret = -1; // PINDA signal error
  283. int16_t z = _Z;
  284. int16_t sum_up = 0;
  285. int16_t sum_dn = 0;
  286. int16_t up;
  287. int16_t dn;
  288. uint8_t sample;
  289. xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
  290. xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1);
  291. if (!_PINDA)
  292. {
  293. for (sample = 0; sample < samples; sample++)
  294. {
  295. dn = _Z;
  296. if (!xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1)) break;
  297. dn = dn - _Z;
  298. up = _Z;
  299. if (!xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1)) break;
  300. up = _Z - up;
  301. DBG(_n("%d. up=%d dn=%d\n"), sample, up, dn);
  302. sum_up += up;
  303. sum_dn += dn;
  304. if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
  305. {
  306. ret = -2; // difference between up-dn to high
  307. break;
  308. }
  309. }
  310. if (sample == samples)
  311. {
  312. up = sum_up / samples;
  313. dn = sum_dn / samples;
  314. uint16_t hyst = (up + dn) / 2;
  315. if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
  316. ret = -2; // difference between up-dn to high
  317. else if ((hyst < XYZCAL_PINDA_HYST_MIN) || (hyst > XYZCAL_PINDA_HYST_MAX))
  318. ret = -3; // hysterezis out of range
  319. else
  320. ret = hyst;
  321. }
  322. }
  323. xyzcal_lineXYZ_to(_X, _Y, z, delay_us, 0);
  324. return ret;
  325. }
  326. #endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS
  327. void accelerate(uint8_t axis, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){
  328. sm4_do_step(axis);
  329. if (acc > 0 && delay_us == min_delay_us){
  330. delayMicroseconds(delay_us);
  331. return;
  332. }
  333. // v1 = v0 + a * t
  334. // 0.01 = length of a step
  335. const float d0 = delay_us * 0.000001f;
  336. const float v1 = (0.01f / d0 + acc * d0);
  337. uint16_t d1;
  338. if (v1 <= 0.1f){
  339. d1 = MAX_DELAY; ///< already too slow so it wants to move back
  340. } else {
  341. d1 = MAX(min_delay_us, round_to_u16(0.01f / v1));
  342. }
  343. /// make sure delay has changed a bit at least
  344. if (d1 == delay_us && acc != 0){
  345. if (acc > 0)
  346. d1--;
  347. else
  348. d1++;
  349. }
  350. delayMicroseconds(d1);
  351. delay_us = d1;
  352. }
  353. uint8_t slow_down_z(uint8_t axis, uint16_t delay_us){
  354. sm4_do_step(axis);
  355. delayMicroseconds(delay_us / 3 * 4);
  356. sm4_do_step(Z_AXIS_MASK);
  357. delayMicroseconds(delay_us * 2);
  358. sm4_do_step(Z_AXIS_MASK);
  359. delayMicroseconds(delay_us * 4);
  360. return 3;
  361. }
  362. uint8_t speed_up_z(uint8_t axis, uint16_t delay_us){
  363. sm4_do_step(Z_AXIS_MASK);
  364. delayMicroseconds(delay_us * 4);
  365. sm4_do_step(Z_AXIS_MASK);
  366. delayMicroseconds(delay_us * 2);
  367. sm4_do_step(Z_AXIS_MASK);
  368. delayMicroseconds(delay_us / 3 * 4);
  369. return 3;
  370. }
  371. void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels){
  372. if(!pixels)
  373. return;
  374. int16_t z = _Z;
  375. int16_t z_trig;
  376. uint16_t line_buffer[32];
  377. uint16_t current_delay_us = MAX_DELAY;
  378. xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0);
  379. for (uint8_t r = 0; r < 32; r++){ ///< Y axis
  380. xyzcal_lineXYZ_to(_X, cy - 1024 + r * 64, z, delay_us, 0);
  381. for (int8_t d = 0; d < 2; ++d){ ///< direction
  382. xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, min_z, delay_us, 0);
  383. z = _Z;
  384. sm4_set_dir(X_AXIS, d);
  385. for (uint8_t c = 0; c < 32; c++){ ///< X axis
  386. /// move up to un-trigger (surpress hysteresis)
  387. sm4_set_dir(Z_AXIS, Z_PLUS);
  388. current_delay_us = MAX_DELAY;
  389. while (z < max_z && _PINDA){
  390. accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, delay_us);
  391. z++;
  392. }
  393. int16_t last_top_z = z;
  394. z += slow_down_z(delay_us);
  395. /// move down to trigger
  396. sm4_set_dir(Z_AXIS, Z_MINUS);
  397. /// speed up
  398. do (){
  399. if (z <= min_z || _PINDA) break;
  400. sm4_do_step(Z_AXIS_MASK);
  401. delayMicroseconds(delay_us * 4);
  402. if (z <= min_z || _PINDA) break;
  403. sm4_do_step(Z_AXIS_MASK);
  404. delayMicroseconds(delay_us * 2);
  405. if (z <= min_z || _PINDA) break;
  406. sm4_do_step(Z_AXIS_MASK);
  407. delayMicroseconds(delay_us / 3 * 4);
  408. } while (0);
  409. while (z > min_z && !_PINDA){
  410. sm4_do_step(Z_AXIS_MASK);
  411. delayMicroseconds(delay_us);
  412. z--;
  413. }
  414. z_trig = z;
  415. z -= slow_down_z(delay_us);
  416. count_position[2] = z;
  417. if (d == 0){
  418. line_buffer[c] = (uint16_t)(z_trig - min_z);
  419. } else {
  420. /// data reversed in X
  421. // DBG(_n("%04x"), (line_buffer[31 - c] + (z - min_z)) / 2);
  422. /// save average of both directions
  423. pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2);
  424. }
  425. /// move to the next point
  426. xyzcal_lineXYZ_to(((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx, _Y, (last_top_z + z) / 2, delay_us, 0);
  427. z = _Z;
  428. }
  429. }
  430. // DBG(_n("\n\n"));
  431. }
  432. }
  433. /// Returns rate of match
  434. /// max match = 132, min match = 0
  435. uint8_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t c, uint8_t r){
  436. uint8_t thr = 16;
  437. uint8_t match = 0;
  438. for (uint8_t i = 0; i < 12; ++i){
  439. for (uint8_t j = 0; j < 12; ++j){
  440. /// skip corners (3 pixels in each)
  441. if (((i == 0) || (i == 11)) && ((j < 2) || (j >= 10))) continue;
  442. if (((j == 0) || (j == 11)) && ((i < 2) || (i >= 10))) continue;
  443. const uint16_t idx = (c + j) + 32 * ((uint16_t)r + i);
  444. const bool high_pix = pixels[idx] > thr;
  445. const bool high_pat = pattern[i] & (1 << j);
  446. if (high_pix == high_pat)
  447. match++;
  448. }
  449. }
  450. return match;
  451. }
  452. /// Searches for best match of pattern by shifting it
  453. /// Returns rate of match and the best location
  454. /// max match = 132, min match = 0
  455. uint8_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr){
  456. if (!pixels || !pattern || !pc || !pr)
  457. return -1;
  458. uint8_t max_c = 0;
  459. uint8_t max_r = 0;
  460. uint8_t max_match = 0;
  461. // DBG(_n("Matching:\n"));
  462. /// pixel precision
  463. for (uint8_t r = 0; r < (32 - 12); ++r){
  464. for (uint8_t c = 0; c < (32 - 12); ++c){
  465. const uint8_t match = xyzcal_match_pattern_12x12_in_32x32(pattern, pixels, c, r);
  466. if (max_match < match){
  467. max_c = c;
  468. max_r = r;
  469. max_match = match;
  470. }
  471. // DBG(_n("%d "), match);
  472. }
  473. // DBG(_n("\n"));
  474. }
  475. DBG(_n("max_c=%f max_r=%f max_match=%d pixel\n"), max_c, max_r, max_match);
  476. *pc = max_c;
  477. *pr = max_r;
  478. return max_match;
  479. }
  480. uint8_t xyzcal_xycoords2point(int16_t x, int16_t y)
  481. {
  482. uint8_t ix = (x > 10000)?1:0;
  483. uint8_t iy = (y > 10000)?1:0;
  484. return iy?(3-ix):ix;
  485. }
  486. //MK3
  487. #if ((MOTHERBOARD == BOARD_EINSY_1_0a))
  488. const int16_t xyzcal_point_xcoords[4] PROGMEM = {1200, 22000, 22000, 1200};
  489. const int16_t xyzcal_point_ycoords[4] PROGMEM = {600, 600, 19800, 19800};
  490. #endif //((MOTHERBOARD == BOARD_EINSY_1_0a))
  491. //MK2.5
  492. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  493. const int16_t xyzcal_point_xcoords[4] PROGMEM = {1200, 22000, 22000, 1200};
  494. const int16_t xyzcal_point_ycoords[4] PROGMEM = {700, 700, 19800, 19800};
  495. #endif //((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  496. const uint16_t xyzcal_point_pattern[12] PROGMEM = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000};
  497. bool xyzcal_searchZ(void)
  498. {
  499. DBG(_n("xyzcal_searchZ x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
  500. int16_t x0 = _X;
  501. int16_t y0 = _Y;
  502. int16_t z0 = _Z;
  503. // int16_t min_z = -6000;
  504. // int16_t dz = 100;
  505. int16_t z = z0;
  506. while (z > -2300) //-6mm + 0.25mm
  507. {
  508. uint16_t ad = 0;
  509. if (xyzcal_spiral8(x0, y0, z, 100, 900, 320, 1, &ad)) //dz=100 radius=900 delay=400
  510. {
  511. int16_t x_on = _X;
  512. int16_t y_on = _Y;
  513. int16_t z_on = _Z;
  514. DBG(_n(" ON-SIGNAL at x=%d y=%d z=%d ad=%d\n"), x_on, y_on, z_on, ad);
  515. return true;
  516. }
  517. z -= 400;
  518. }
  519. DBG(_n("xyzcal_searchZ no signal\n x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
  520. return false;
  521. }
  522. /// returns value of any location within data
  523. /// uses bilinear interpolation
  524. float get_value(uint8_t * matrix_32x32, float c, float r){
  525. if (c <= 0 || r <= 0 || c >= 31 || r >= 31)
  526. return 0;
  527. /// calculate weights of nearby points
  528. const float wc1 = c - floor(c);
  529. const float wr1 = r - floor(r);
  530. const float wc0 = 1 - wc1;
  531. const float wr0 = 1 - wr1;
  532. const float w00 = wc0 * wr0;
  533. const float w01 = wc0 * wr1;
  534. const float w10 = wc1 * wr0;
  535. const float w11 = wc1 * wr1;
  536. const uint16_t c0 = c;
  537. const uint16_t c1 = c0 + 1;
  538. const uint16_t r0 = r;
  539. const uint16_t r1 = r0 + 1;
  540. const uint16_t idx00 = c0 + 32 * r0;
  541. const uint16_t idx01 = c0 + 32 * r1;
  542. const uint16_t idx10 = c1 + 32 * r0;
  543. const uint16_t idx11 = c1 + 32 * r1;
  544. /// bilinear resampling
  545. return w00 * matrix_32x32[idx00] + w01 * matrix_32x32[idx01] + w10 * matrix_32x32[idx10] + w11 * matrix_32x32[idx11];
  546. }
  547. const constexpr float m_infinity = -1000.f;
  548. /// replaces the highest number by m_infinity
  549. void remove_highest(float *points, const uint8_t num_points){
  550. if (num_points <= 0)
  551. return;
  552. float max = points[0];
  553. uint8_t max_i = 0;
  554. for (uint8_t i = 0; i < num_points; ++i){
  555. if (max < points[i]){
  556. max = points[i];
  557. max_i = i;
  558. }
  559. }
  560. points[max_i] = m_infinity;
  561. }
  562. /// return the highest number in the list
  563. float highest(float *points, const uint8_t num_points){
  564. if (num_points <= 0)
  565. return 0;
  566. float max = points[0];
  567. for (uint8_t i = 0; i < num_points; ++i){
  568. if (max < points[i]){
  569. max = points[i];
  570. }
  571. }
  572. return max;
  573. }
  574. /// Searches for circle iteratively
  575. /// Uses points on the perimeter. If point is high it pushes circle out of the center (shift or change of radius),
  576. /// otherwise to the center.
  577. /// Algorithm is stopped after fixed number of iterations. Move is limited to 0.5 px per iteration.
  578. void dynamic_circle(uint8_t *matrix_32x32, float &x, float &y, float &r, uint8_t iterations){
  579. /// circle of 10.5 diameter has 33 in circumference, don't go much above
  580. const constexpr uint8_t num_points = 33;
  581. float points[num_points];
  582. float pi_2_div_num_points = 2 * M_PI / num_points;
  583. const constexpr uint8_t target_z = 32; ///< target z height of the circle
  584. float norm;
  585. float angle;
  586. float max_val = 0.5f;
  587. const uint8_t blocks = 7;
  588. float shifts_x[blocks];
  589. float shifts_y[blocks];
  590. float shifts_r[blocks];
  591. for (int8_t i = iterations; i > 0; --i){
  592. // DBG(_n(" [%f, %f][%f] circle\n"), x, y, r);
  593. /// read points on the circle
  594. for (uint8_t p = 0; p < num_points; ++p){
  595. angle = p * pi_2_div_num_points;
  596. points[p] = get_value(matrix_32x32, r * cos(angle) + x, r * sin(angle) + y) - target_z;
  597. // DBG(_n("%f "), points[p]);
  598. }
  599. // DBG(_n(" points\n"));
  600. /// sum blocks
  601. for (uint8_t j = 0; j < blocks; ++j){
  602. shifts_x[j] = shifts_y[j] = shifts_r[j] = 0;
  603. /// first part
  604. for (uint8_t p = 0; p < num_points * 3 / 4; ++p){
  605. uint8_t idx = (p + j * num_points / blocks) % num_points;
  606. angle = idx * pi_2_div_num_points;
  607. shifts_x[j] += cos(angle) * points[idx];
  608. shifts_y[j] += sin(angle) * points[idx];
  609. shifts_r[j] += points[idx];
  610. }
  611. }
  612. /// remove extreme values (slow but simple)
  613. for (uint8_t j = 0; j < blocks / 2; ++j){
  614. remove_highest(shifts_x, blocks);
  615. remove_highest(shifts_y, blocks);
  616. remove_highest(shifts_r, blocks);
  617. }
  618. /// median is the highest now
  619. norm = 1.f / (32.f * (num_points * 3 / 4));
  620. x += CLAMP(highest(shifts_x, blocks) * norm, -max_val, max_val);
  621. y += CLAMP(highest(shifts_y, blocks) * norm, -max_val, max_val);
  622. r += CLAMP(highest(shifts_r, blocks) * norm, -max_val, max_val);
  623. r = MAX(2, r);
  624. }
  625. DBG(_n(" [%f, %f][%f] final circle\n"), x, y, r);
  626. }
  627. /// Prints matrix in hex to debug output (serial line)
  628. void print_image(uint8_t *matrix_32x32){
  629. for (uint8_t y = 0; y < 32; ++y){
  630. const uint16_t idx_y = y * 32;
  631. for (uint8_t x = 0; x < 32; ++x){
  632. DBG(_n("%02x"), matrix_32x32[idx_y + x]);
  633. }
  634. DBG(_n("\n"));
  635. }
  636. DBG(_n("\n"));
  637. }
  638. /// scans area around the current head location and
  639. /// searches for the center of the calibration pin
  640. bool xyzcal_scan_and_process(void){
  641. DBG(_n("sizeof(block_buffer)=%d\n"), sizeof(block_t)*BLOCK_BUFFER_SIZE);
  642. bool ret = false;
  643. int16_t x = _X;
  644. int16_t y = _Y;
  645. int16_t z = _Z;
  646. uint8_t *matrix32 = (uint8_t *)block_buffer;
  647. uint16_t *pattern = (uint16_t *)(matrix32 + 32 * 32);
  648. xyzcal_scan_pixels_32x32_Zhop(x, y, z - 72, 2400, 300, matrix32);
  649. print_image(matrix32);
  650. for (uint8_t i = 0; i < 12; i++){
  651. pattern[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern + i));
  652. // DBG(_n(" pattern[%d]=%d\n"), i, pattern[i]);
  653. }
  654. /// SEARCH FOR BINARY CIRCLE
  655. uint8_t uc = 0;
  656. uint8_t ur = 0;
  657. /// max match = 132, 1/2 good = 66, 2/3 good = 88
  658. if (xyzcal_find_pattern_12x12_in_32x32(matrix32, pattern, &uc, &ur) >= 88){
  659. /// find precise circle
  660. /// move to the center of the pattern (+5.5)
  661. float xf = uc + 5.5f;
  662. float yf = ur + 5.5f;
  663. float radius = 5; ///< default radius
  664. const uint8_t iterations = 20;
  665. dynamic_circle(matrix32, xf, yf, radius, iterations);
  666. if (ABS(xf - uc + 5.5f) > 3 || ABS(yf - ur + 5.5f) > 3 || ABS(radius - 5) > 3){
  667. /// dynamic algorithm diverged, use original position instead
  668. xf = uc + 5.5f;
  669. yf = ur + 5.5f;
  670. }
  671. /// move to the center of area and convert to position
  672. xf = (float)x + (xf - 15.5f) * 64;
  673. yf = (float)y + (yf - 15.5f) * 64;
  674. DBG(_n(" [%f %f] mm pattern center\n"), pos_2_mm(xf), pos_2_mm(yf));
  675. x = round_to_i16(xf);
  676. y = round_to_i16(yf);
  677. xyzcal_lineXYZ_to(x, y, z, 200, 0);
  678. ret = true;
  679. }
  680. /// wipe buffer
  681. for (uint16_t i = 0; i < sizeof(block_t)*BLOCK_BUFFER_SIZE; i++)
  682. matrix32[i] = 0;
  683. return ret;
  684. }
  685. bool xyzcal_find_bed_induction_sensor_point_xy(void){
  686. bool ret = false;
  687. DBG(_n("xyzcal_find_bed_induction_sensor_point_xy x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
  688. st_synchronize();
  689. pos_i16_t x = _X;
  690. pos_i16_t y = _Y;
  691. pos_i16_t z = _Z;
  692. uint8_t point = xyzcal_xycoords2point(x, y);
  693. x = pgm_read_word((uint16_t *)(xyzcal_point_xcoords + point));
  694. y = pgm_read_word((uint16_t *)(xyzcal_point_ycoords + point));
  695. DBG(_n("point=%d x=%d y=%d z=%d\n"), point, x, y, z);
  696. xyzcal_meassure_enter();
  697. xyzcal_lineXYZ_to(x, y, z, 200, 0);
  698. if (xyzcal_searchZ()){
  699. int16_t z = _Z;
  700. xyzcal_lineXYZ_to(x, y, z, 200, 0);
  701. ret = xyzcal_scan_and_process();
  702. }
  703. xyzcal_meassure_leave();
  704. return ret;
  705. }
  706. #endif //NEW_XYZCAL