xyzcal.cpp 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814
  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 _PI 3.14159265F
  25. extern long count_position[NUM_AXIS];
  26. uint8_t check_pinda_0();
  27. uint8_t check_pinda_1();
  28. void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
  29. uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd);
  30. void xyzcal_meassure_enter(void)
  31. {
  32. DBG(_n("xyzcal_meassure_enter\n"));
  33. disable_heater();
  34. DISABLE_TEMPERATURE_INTERRUPT();
  35. #if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
  36. DISABLE_FANCHECK_INTERRUPT();
  37. #endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
  38. DISABLE_STEPPER_DRIVER_INTERRUPT();
  39. #ifdef WATCHDOG
  40. wdt_disable();
  41. #endif //WATCHDOG
  42. sm4_stop_cb = 0;
  43. sm4_update_pos_cb = xyzcal_update_pos;
  44. sm4_calc_delay_cb = xyzcal_calc_delay;
  45. }
  46. void xyzcal_meassure_leave(void)
  47. {
  48. DBG(_n("xyzcal_meassure_leave\n"));
  49. planner_abort_hard();
  50. ENABLE_TEMPERATURE_INTERRUPT();
  51. #if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
  52. ENABLE_FANCHECK_INTERRUPT();
  53. #endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
  54. ENABLE_STEPPER_DRIVER_INTERRUPT();
  55. #ifdef WATCHDOG
  56. wdt_enable(WDTO_4S);
  57. #endif //WATCHDOG
  58. sm4_stop_cb = 0;
  59. sm4_update_pos_cb = 0;
  60. sm4_calc_delay_cb = 0;
  61. }
  62. uint8_t check_pinda_0()
  63. {
  64. return _PINDA?0:1;
  65. }
  66. uint8_t check_pinda_1()
  67. {
  68. return _PINDA?1:0;
  69. }
  70. uint8_t xyzcal_dm = 0;
  71. void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de)
  72. {
  73. // DBG(_n("xyzcal_update_pos dx=%d dy=%d dz=%d dir=%02x\n"), dx, dy, dz, xyzcal_dm);
  74. if (xyzcal_dm&1) count_position[0] -= dx; else count_position[0] += dx;
  75. if (xyzcal_dm&2) count_position[1] -= dy; else count_position[1] += dy;
  76. if (xyzcal_dm&4) count_position[2] -= dz; else count_position[2] += dz;
  77. // DBG(_n(" after xyzcal_update_pos x=%ld y=%ld z=%ld\n"), count_position[0], count_position[1], count_position[2]);
  78. }
  79. uint16_t xyzcal_sm4_delay = 0;
  80. //#define SM4_ACCEL_TEST
  81. #ifdef SM4_ACCEL_TEST
  82. uint16_t xyzcal_sm4_v0 = 2000;
  83. uint16_t xyzcal_sm4_vm = 45000;
  84. uint16_t xyzcal_sm4_v = xyzcal_sm4_v0;
  85. uint16_t xyzcal_sm4_ac = 2000;
  86. uint16_t xyzcal_sm4_ac2 = (uint32_t)xyzcal_sm4_ac * 1024 / 10000;
  87. //float xyzcal_sm4_vm = 10000;
  88. #endif //SM4_ACCEL_TEST
  89. uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd)
  90. {
  91. return xyzcal_sm4_delay;
  92. #ifdef SM4_ACCEL_TEST
  93. uint16_t del_us = 0;
  94. if (xyzcal_sm4_v & 0xf000) //>=4096
  95. {
  96. del_us = (uint16_t)62500 / (uint16_t)(xyzcal_sm4_v >> 4);
  97. xyzcal_sm4_v += (xyzcal_sm4_ac2 * del_us + 512) >> 10;
  98. if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
  99. if (del_us > 25) return del_us - 25;
  100. }
  101. else
  102. {
  103. del_us = (uint32_t)1000000 / xyzcal_sm4_v;
  104. xyzcal_sm4_v += ((uint32_t)xyzcal_sm4_ac2 * del_us + 512) >> 10;
  105. if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
  106. if (del_us > 50) return del_us - 50;
  107. }
  108. // uint16_t del_us = (uint16_t)(((float)1000000 / xyzcal_sm4_v) + 0.5);
  109. // uint16_t del_us = (uint32_t)1000000 / xyzcal_sm4_v;
  110. // uint16_t del_us = 100;
  111. // uint16_t del_us = (uint16_t)10000 / xyzcal_sm4_v;
  112. // v += (ac * del_us + 500) / 1000;
  113. // xyzcal_sm4_v += (xyzcal_sm4_ac * del_us) / 1000;
  114. // return xyzcal_sm4_delay;
  115. // DBG(_n("xyzcal_calc_delay nd=%d dd=%d v=%d del_us=%d\n"), nd, dd, xyzcal_sm4_v, del_us);
  116. return 0;
  117. #endif //SM4_ACCEL_TEST
  118. }
  119. bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda)
  120. {
  121. // DBG(_n("xyzcal_lineXYZ_to x=%d y=%d z=%d check=%d\n"), x, y, z, check_pinda);
  122. x -= (int16_t)count_position[0];
  123. y -= (int16_t)count_position[1];
  124. z -= (int16_t)count_position[2];
  125. xyzcal_dm = ((x<0)?1:0) | ((y<0)?2:0) | ((z<0)?4:0);
  126. sm4_set_dir_bits(xyzcal_dm);
  127. sm4_stop_cb = check_pinda?((check_pinda<0)?check_pinda_0:check_pinda_1):0;
  128. xyzcal_sm4_delay = delay_us;
  129. // uint32_t u = micros();
  130. bool ret = sm4_line_xyze_ui(abs(x), abs(y), abs(z), 0)?true:false;
  131. // u = micros() - u;
  132. return ret;
  133. }
  134. 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)
  135. {
  136. bool ret = false;
  137. float r = 0; //radius
  138. uint8_t n = 0; //point number
  139. uint16_t ad = 0; //angle [deg]
  140. float ar; //angle [rad]
  141. uint8_t dad = 0; //delta angle [deg]
  142. uint8_t dad_min = 4; //delta angle min [deg]
  143. uint8_t dad_max = 16; //delta angle max [deg]
  144. uint8_t k = 720 / (dad_max - dad_min); //delta calculation constant
  145. ad = 0;
  146. if (pad) ad = *pad % 720;
  147. DBG(_n("xyzcal_spiral2 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
  148. for (; ad < 720; ad++)
  149. {
  150. if (radius > 0)
  151. {
  152. dad = dad_max - (ad / k);
  153. r = (float)(((uint32_t)ad) * radius) / 720;
  154. }
  155. else
  156. {
  157. dad = dad_max - ((719 - ad) / k);
  158. r = (float)(((uint32_t)(719 - ad)) * (-radius)) / 720;
  159. }
  160. ar = (ad + rotation)* (float)_PI / 180;
  161. float _cos = cos(ar);
  162. float _sin = sin(ar);
  163. int x = (int)(cx + (_cos * r));
  164. int y = (int)(cy + (_sin * r));
  165. int z = (int)(z0 - ((float)((int32_t)dz * ad) / 720));
  166. if (xyzcal_lineXYZ_to(x, y, z, delay_us, check_pinda))
  167. {
  168. ad += dad + 1;
  169. ret = true;
  170. break;
  171. }
  172. n++;
  173. ad += dad;
  174. }
  175. if (pad) *pad = ad;
  176. return ret;
  177. }
  178. 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)
  179. {
  180. bool ret = false;
  181. uint16_t ad = 0;
  182. if (pad) ad = *pad;
  183. DBG(_n("xyzcal_spiral8 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
  184. if (!ret && (ad < 720))
  185. if ((ret = xyzcal_spiral2(cx, cy, z0 - 0*dz, dz, radius, 0, delay_us, check_pinda, &ad)) != 0)
  186. ad += 0;
  187. if (!ret && (ad < 1440))
  188. if ((ret = xyzcal_spiral2(cx, cy, z0 - 1*dz, dz, -radius, 0, delay_us, check_pinda, &ad)) != 0)
  189. ad += 720;
  190. if (!ret && (ad < 2160))
  191. if ((ret = xyzcal_spiral2(cx, cy, z0 - 2*dz, dz, radius, 180, delay_us, check_pinda, &ad)) != 0)
  192. ad += 1440;
  193. if (!ret && (ad < 2880))
  194. if ((ret = xyzcal_spiral2(cx, cy, z0 - 3*dz, dz, -radius, 180, delay_us, check_pinda, &ad)) != 0)
  195. ad += 2160;
  196. if (pad) *pad = ad;
  197. return ret;
  198. }
  199. #ifdef XYZCAL_MEASSURE_PINDA_HYSTEREZIS
  200. int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples)
  201. {
  202. DBG(_n("xyzcal_meassure_pinda_hysterezis\n"));
  203. int8_t ret = -1; // PINDA signal error
  204. int16_t z = _Z;
  205. int16_t sum_up = 0;
  206. int16_t sum_dn = 0;
  207. int16_t up;
  208. int16_t dn;
  209. uint8_t sample;
  210. xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
  211. xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1);
  212. if (!_PINDA)
  213. {
  214. for (sample = 0; sample < samples; sample++)
  215. {
  216. dn = _Z;
  217. if (!xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1)) break;
  218. dn = dn - _Z;
  219. up = _Z;
  220. if (!xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1)) break;
  221. up = _Z - up;
  222. DBG(_n("%d. up=%d dn=%d\n"), sample, up, dn);
  223. sum_up += up;
  224. sum_dn += dn;
  225. if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
  226. {
  227. ret = -2; // difference between up-dn to high
  228. break;
  229. }
  230. }
  231. if (sample == samples)
  232. {
  233. up = sum_up / samples;
  234. dn = sum_dn / samples;
  235. uint16_t hyst = (up + dn) / 2;
  236. if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
  237. ret = -2; // difference between up-dn to high
  238. else if ((hyst < XYZCAL_PINDA_HYST_MIN) || (hyst > XYZCAL_PINDA_HYST_MAX))
  239. ret = -3; // hysterezis out of range
  240. else
  241. ret = hyst;
  242. }
  243. }
  244. xyzcal_lineXYZ_to(_X, _Y, z, delay_us, 0);
  245. return ret;
  246. }
  247. #endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS
  248. void xyzcal_scan_pixels_32x32(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels)
  249. {
  250. DBG(_n("xyzcal_scan_pixels_32x32 cx=%d cy=%d min_z=%d max_z=%d\n"), cx, cy, min_z, max_z);
  251. // xyzcal_lineXYZ_to(cx - 1024, cy - 1024, max_z, 2*delay_us, 0);
  252. // xyzcal_lineXYZ_to(cx, cy, max_z, delay_us, 0);
  253. int16_t z = (int16_t)count_position[2];
  254. xyzcal_lineXYZ_to(cx, cy, z, 2*delay_us, 0);
  255. for (uint8_t r = 0; r < 32; r++)
  256. {
  257. int8_t _pinda = _PINDA;
  258. xyzcal_lineXYZ_to((r&1)?(cx+1024):(cx-1024), cy - 1024 + r*64, z, 2*delay_us, 0);
  259. xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
  260. xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1);
  261. z = (int16_t)count_position[2];
  262. sm4_set_dir(X_AXIS, (r&1)?1:0);
  263. for (uint8_t c = 0; c < 32; c++)
  264. {
  265. uint16_t sum = 0;
  266. int16_t z_sum = 0;
  267. for (uint8_t i = 0; i < 64; i++)
  268. {
  269. int8_t pinda = _PINDA;
  270. int16_t pix = z - min_z;
  271. pix += (pinda)?23:-24;
  272. if (pix < 0) pix = 0;
  273. if (pix > 255) pix = 255;
  274. sum += pix;
  275. z_sum += z;
  276. // if (_pinda != pinda)
  277. // {
  278. // if (pinda)
  279. // DBG(_n("!1 x=%d z=%d\n"), c*64+i, z+23);
  280. // else
  281. // DBG(_n("!0 x=%d z=%d\n"), c*64+i, z-24);
  282. // }
  283. sm4_set_dir(Z_AXIS, !pinda);
  284. if (!pinda)
  285. {
  286. if (z > min_z)
  287. {
  288. sm4_do_step(Z_AXIS_MASK);
  289. z--;
  290. }
  291. }
  292. else
  293. {
  294. if (z < max_z)
  295. {
  296. sm4_do_step(Z_AXIS_MASK);
  297. z++;
  298. }
  299. }
  300. sm4_do_step(X_AXIS_MASK);
  301. delayMicroseconds(600);
  302. _pinda = pinda;
  303. }
  304. sum >>= 6; //div 64
  305. if (z_sum < 0)
  306. {
  307. z_sum = -z_sum;
  308. z_sum >>= 6; //div 64
  309. z_sum = -z_sum;
  310. }
  311. else
  312. z_sum >>= 6; //div 64
  313. if (pixels) pixels[((uint16_t)r<<5) + ((r&1)?(31-c):c)] = sum;
  314. // DBG(_n("c=%d r=%d l=%d z=%d\n"), c, r, sum, z_sum);
  315. count_position[0] += (r&1)?-64:64;
  316. count_position[2] = z;
  317. }
  318. if (pixels)
  319. for (uint8_t c = 0; c < 32; c++)
  320. DBG(_n("%02x"), pixels[((uint16_t)r<<5) + c]);
  321. DBG(_n("\n"));
  322. }
  323. // xyzcal_lineXYZ_to(cx, cy, z, 2*delay_us, 0);
  324. }
  325. void xyzcal_histo_pixels_32x32(uint8_t* pixels, uint16_t* histo)
  326. {
  327. for (uint8_t l = 0; l < 16; l++)
  328. histo[l] = 0;
  329. for (uint8_t r = 0; r < 32; r++)
  330. for (uint8_t c = 0; c < 32; c++)
  331. {
  332. uint8_t pix = pixels[((uint16_t)r<<5) + c];
  333. histo[pix >> 4]++;
  334. }
  335. for (uint8_t l = 0; l < 16; l++)
  336. DBG(_n(" %2d %d\n"), l, histo[l]);
  337. }
  338. void xyzcal_adjust_pixels(uint8_t* pixels, uint16_t* histo)
  339. {
  340. uint8_t l;
  341. uint16_t max_c = histo[1];
  342. uint8_t max_l = 1;
  343. for (l = 1; l < 16; l++)
  344. {
  345. uint16_t c = histo[l];
  346. if (c > max_c)
  347. {
  348. max_c = c;
  349. max_l = l;
  350. }
  351. }
  352. DBG(_n("max_c=%2d max_l=%d\n"), max_c, max_l);
  353. for (l = 14; l > 8; l--)
  354. if (histo[l] >= 10)
  355. break;
  356. uint8_t pix_min = 0;
  357. uint8_t pix_max = l << 4;
  358. if (histo[0] < (32*32 - 144))
  359. {
  360. pix_min = (max_l << 4) / 2;
  361. }
  362. uint8_t pix_dif = pix_max - pix_min;
  363. DBG(_n(" min=%d max=%d dif=%d\n"), pix_min, pix_max, pix_dif);
  364. for (int16_t i = 0; i < 32*32; i++)
  365. {
  366. uint16_t pix = pixels[i];
  367. if (pix > pix_min) pix -= pix_min;
  368. else pix = 0;
  369. pix <<= 8;
  370. pix /= pix_dif;
  371. // if (pix < 0) pix = 0;
  372. if (pix > 255) pix = 255;
  373. pixels[i] = (uint8_t)pix;
  374. }
  375. for (uint8_t r = 0; r < 32; r++)
  376. {
  377. for (uint8_t c = 0; c < 32; c++)
  378. DBG(_n("%02x"), pixels[((uint16_t)r<<5) + c]);
  379. DBG(_n("\n"));
  380. }
  381. }
  382. /*
  383. void xyzcal_draw_pattern_12x12_in_32x32(uint8_t* pattern, uint32_t* pixels, int w, int h, uint8_t x, uint8_t y, uint32_t and, uint32_t or)
  384. {
  385. for (int i = 0; i < 8; i++)
  386. for (int j = 0; j < 8; j++)
  387. {
  388. int idx = (x + j) + w * (y + i);
  389. if (pattern[i] & (1 << j))
  390. {
  391. pixels[idx] &= and;
  392. pixels[idx] |= or;
  393. }
  394. }
  395. }
  396. */
  397. int16_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t c, uint8_t r)
  398. {
  399. uint8_t thr = 16;
  400. int16_t match = 0;
  401. for (uint8_t i = 0; i < 12; i++)
  402. for (uint8_t j = 0; j < 12; j++)
  403. {
  404. if (((i == 0) || (i == 11)) && ((j < 2) || (j >= 10))) continue; //skip corners
  405. if (((j == 0) || (j == 11)) && ((i < 2) || (i >= 10))) continue;
  406. uint16_t idx = (c + j) + 32 * (r + i);
  407. uint8_t val = pixels[idx];
  408. if (pattern[i] & (1 << j))
  409. {
  410. if (val > thr) match ++;
  411. else match --;
  412. }
  413. else
  414. {
  415. if (val <= thr) match ++;
  416. else match --;
  417. }
  418. }
  419. return match;
  420. }
  421. int16_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr)
  422. {
  423. uint8_t max_c = 0;
  424. uint8_t max_r = 0;
  425. int16_t max_match = 0;
  426. for (uint8_t r = 0; r < (32 - 12); r++)
  427. for (uint8_t c = 0; c < (32 - 12); c++)
  428. {
  429. int16_t match = xyzcal_match_pattern_12x12_in_32x32(pattern, pixels, c, r);
  430. if (max_match < match)
  431. {
  432. max_c = c;
  433. max_r = r;
  434. max_match = match;
  435. }
  436. }
  437. DBG(_n("max_c=%d max_r=%d max_match=%d\n"), max_c, max_r, max_match);
  438. if (pc) *pc = max_c;
  439. if (pr) *pr = max_r;
  440. return max_match;
  441. }
  442. #define MAX_DIAMETR 600
  443. #define XYZCAL_FIND_CENTER_DIAGONAL
  444. int8_t xyzcal_find_point_center2(uint16_t delay_us)
  445. {
  446. printf_P(PSTR("xyzcal_find_point_center2\n"));
  447. int16_t x0 = _X;
  448. int16_t y0 = _Y;
  449. int16_t z0 = _Z;
  450. printf_P(PSTR(" x0=%d\n"), x0);
  451. printf_P(PSTR(" y0=%d\n"), y0);
  452. printf_P(PSTR(" z0=%d\n"), z0);
  453. xyzcal_lineXYZ_to(_X, _Y, z0 + 400, 500, -1);
  454. xyzcal_lineXYZ_to(_X, _Y, z0 - 400, 500, 1);
  455. xyzcal_lineXYZ_to(_X, _Y, z0 + 400, 500, -1);
  456. xyzcal_lineXYZ_to(_X, _Y, z0 - 400, 500, 1);
  457. z0 = _Z - 20;
  458. xyzcal_lineXYZ_to(_X, _Y, z0, 500, 0);
  459. // xyzcal_lineXYZ_to(x0, y0, z0 - 100, 500, 1);
  460. // z0 = _Z;
  461. // printf_P(PSTR(" z0=%d\n"), z0);
  462. // xyzcal_lineXYZ_to(x0, y0, z0 + 100, 500, -1);
  463. // z0 += _Z;
  464. // z0 /= 2;
  465. printf_P(PSTR(" z0=%d\n"), z0);
  466. // xyzcal_lineXYZ_to(x0, y0, z0 - 100, 500, 1);
  467. // z0 = _Z - 10;
  468. int8_t ret = 1;
  469. #ifdef XYZCAL_FIND_CENTER_DIAGONAL
  470. int32_t xc = 0;
  471. int32_t yc = 0;
  472. int16_t ad = 45;
  473. for (; ad < 360; ad += 90)
  474. {
  475. float ar = (float)ad * _PI / 180;
  476. int16_t x = x0 + MAX_DIAMETR * cos(ar);
  477. int16_t y = y0 + MAX_DIAMETR * sin(ar);
  478. if (!xyzcal_lineXYZ_to(x, y, z0, delay_us, -1))
  479. {
  480. printf_P(PSTR("ERROR ad=%d\n"), ad);
  481. ret = 0;
  482. break;
  483. }
  484. xc += _X;
  485. yc += _Y;
  486. xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
  487. }
  488. if (ret)
  489. {
  490. printf_P(PSTR("OK\n"), ad);
  491. x0 = xc / 4;
  492. y0 = yc / 4;
  493. printf_P(PSTR(" x0=%d\n"), x0);
  494. printf_P(PSTR(" y0=%d\n"), y0);
  495. }
  496. #else //XYZCAL_FIND_CENTER_DIAGONAL
  497. xyzcal_lineXYZ_to(x0 - MAX_DIAMETR, y0, z0, delay_us, -1);
  498. int16_t dx1 = x0 - _X;
  499. if (dx1 >= MAX_DIAMETR)
  500. {
  501. printf_P(PSTR("!!! dx1 = %d\n"), dx1);
  502. return 0;
  503. }
  504. xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
  505. xyzcal_lineXYZ_to(x0 + MAX_DIAMETR, y0, z0, delay_us, -1);
  506. int16_t dx2 = _X - x0;
  507. if (dx2 >= MAX_DIAMETR)
  508. {
  509. printf_P(PSTR("!!! dx2 = %d\n"), dx2);
  510. return 0;
  511. }
  512. xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
  513. xyzcal_lineXYZ_to(x0 , y0 - MAX_DIAMETR, z0, delay_us, -1);
  514. int16_t dy1 = y0 - _Y;
  515. if (dy1 >= MAX_DIAMETR)
  516. {
  517. printf_P(PSTR("!!! dy1 = %d\n"), dy1);
  518. return 0;
  519. }
  520. xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
  521. xyzcal_lineXYZ_to(x0, y0 + MAX_DIAMETR, z0, delay_us, -1);
  522. int16_t dy2 = _Y - y0;
  523. if (dy2 >= MAX_DIAMETR)
  524. {
  525. printf_P(PSTR("!!! dy2 = %d\n"), dy2);
  526. return 0;
  527. }
  528. printf_P(PSTR("dx1=%d\n"), dx1);
  529. printf_P(PSTR("dx2=%d\n"), dx2);
  530. printf_P(PSTR("dy1=%d\n"), dy1);
  531. printf_P(PSTR("dy2=%d\n"), dy2);
  532. x0 += (dx2 - dx1) / 2;
  533. y0 += (dy2 - dy1) / 2;
  534. printf_P(PSTR(" x0=%d\n"), x0);
  535. printf_P(PSTR(" y0=%d\n"), y0);
  536. #endif //XYZCAL_FIND_CENTER_DIAGONAL
  537. xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0);
  538. return ret;
  539. }
  540. #ifdef XYZCAL_FIND_POINT_CENTER
  541. int8_t xyzcal_find_point_center(int16_t x0, int16_t y0, int16_t z0, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t turns)
  542. {
  543. uint8_t n;
  544. uint16_t ad;
  545. float ar;
  546. float _cos;
  547. float _sin;
  548. int16_t r_min = 0;
  549. int16_t r_max = 0;
  550. int16_t x_min = 0;
  551. int16_t x_max = 0;
  552. int16_t y_min = 0;
  553. int16_t y_max = 0;
  554. int16_t r = 10;
  555. int16_t x = x0;
  556. int16_t y = y0;
  557. int16_t z = z0;
  558. int8_t _pinda = _PINDA;
  559. for (n = 0; n < turns; n++)
  560. {
  561. uint32_t r_sum = 0;
  562. for (ad = 0; ad < 720; ad++)
  563. {
  564. ar = ad * _PI / 360;
  565. _cos = cos(ar);
  566. _sin = sin(ar);
  567. x = x0 + (int)(_cos * r);
  568. y = y0 + (int)(_sin * r);
  569. xyzcal_lineXYZ_to(x, y, z, 1000, 0);
  570. int8_t pinda = _PINDA;
  571. if (pinda)
  572. r += 1;
  573. else
  574. {
  575. r -= 1;
  576. ad--;
  577. r_sum -= r;
  578. }
  579. if (ad == 0)
  580. {
  581. x_min = x0;
  582. x_max = x0;
  583. y_min = y0;
  584. y_max = y0;
  585. r_min = r;
  586. r_max = r;
  587. }
  588. else if (pinda)
  589. {
  590. if (x_min > x) x_min = (2*x + x_min) / 3;
  591. if (x_max < x) x_max = (2*x + x_max) / 3;
  592. if (y_min > y) y_min = (2*y + y_min) / 3;
  593. if (y_max < y) y_max = (2*y + y_max) / 3;
  594. /* if (x_min > x) x_min = x;
  595. if (x_max < x) x_max = x;
  596. if (y_min > y) y_min = y;
  597. if (y_max < y) y_max = y;*/
  598. if (r_min > r) r_min = r;
  599. if (r_max < r) r_max = r;
  600. }
  601. r_sum += r;
  602. /* if (_pinda != pinda)
  603. {
  604. if (pinda)
  605. DBG(_n("!1 x=%d y=%d\n"), x, y);
  606. else
  607. DBG(_n("!0 x=%d y=%d\n"), x, y);
  608. }*/
  609. _pinda = pinda;
  610. // DBG(_n("x=%d y=%d rx=%d ry=%d\n"), x, y, rx, ry);
  611. }
  612. DBG(_n("x_min=%d x_max=%d y_min=%d y_max=%d r_min=%d r_max=%d r_avg=%d\n"), x_min, x_max, y_min, y_max, r_min, r_max, r_sum / 720);
  613. if ((n > 2) && (n & 1))
  614. {
  615. x0 += (x_min + x_max);
  616. y0 += (y_min + y_max);
  617. x0 /= 3;
  618. y0 /= 3;
  619. int rx = (x_max - x_min) / 2;
  620. int ry = (y_max - y_min) / 2;
  621. r = (rx + ry) / 3;//(rx < ry)?rx:ry;
  622. DBG(_n("x0=%d y0=%d r=%d\n"), x0, y0, r);
  623. }
  624. }
  625. xyzcal_lineXYZ_to(x0, y0, z, 200, 0);
  626. }
  627. #endif //XYZCAL_FIND_POINT_CENTER
  628. uint8_t xyzcal_xycoords2point(int16_t x, int16_t y)
  629. {
  630. uint8_t ix = (x > 10000)?1:0;
  631. uint8_t iy = (y > 10000)?1:0;
  632. return iy?(3-ix):ix;
  633. }
  634. //MK3
  635. #if ((MOTHERBOARD == BOARD_EINSY_1_0a))
  636. const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200};
  637. const int16_t PROGMEM xyzcal_point_ycoords[4] = {600, 600, 19800, 19800};
  638. #endif //((MOTHERBOARD == BOARD_EINSY_1_0a))
  639. //MK2.5
  640. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  641. const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200};
  642. const int16_t PROGMEM xyzcal_point_ycoords[4] = {700, 700, 19800, 19800};
  643. #endif //((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  644. const uint16_t PROGMEM xyzcal_point_pattern[12] = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000};
  645. bool xyzcal_searchZ(void)
  646. {
  647. DBG(_n("xyzcal_searchZ x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
  648. int16_t x0 = _X;
  649. int16_t y0 = _Y;
  650. int16_t z0 = _Z;
  651. // int16_t min_z = -6000;
  652. // int16_t dz = 100;
  653. int16_t z = z0;
  654. while (z > -2300) //-6mm + 0.25mm
  655. {
  656. uint16_t ad = 0;
  657. if (xyzcal_spiral8(x0, y0, z, 100, 900, 320, 1, &ad)) //dz=100 radius=900 delay=400
  658. {
  659. int16_t x_on = _X;
  660. int16_t y_on = _Y;
  661. int16_t z_on = _Z;
  662. DBG(_n(" ON-SIGNAL at x=%d y=%d z=%d ad=%d\n"), x_on, y_on, z_on, ad);
  663. return true;
  664. }
  665. z -= 400;
  666. }
  667. 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]);
  668. return false;
  669. }
  670. bool xyzcal_scan_and_process(void)
  671. {
  672. DBG(_n("sizeof(block_buffer)=%d\n"), sizeof(block_t)*BLOCK_BUFFER_SIZE);
  673. // DBG(_n("sizeof(pixels)=%d\n"), 32*32);
  674. // DBG(_n("sizeof(histo)=%d\n"), 2*16);
  675. // DBG(_n("sizeof(pattern)=%d\n"), 2*12);
  676. DBG(_n("sizeof(total)=%d\n"), 32*32+2*16+2*12);
  677. bool ret = false;
  678. int16_t x = _X;
  679. int16_t y = _Y;
  680. int16_t z = _Z;
  681. uint8_t* pixels = (uint8_t*)block_buffer;
  682. xyzcal_scan_pixels_32x32(x, y, z - 72, 2400, 200, pixels);
  683. uint16_t* histo = (uint16_t*)(pixels + 32*32);
  684. xyzcal_histo_pixels_32x32(pixels, histo);
  685. xyzcal_adjust_pixels(pixels, histo);
  686. uint16_t* pattern = (uint16_t*)(histo + 2*16);
  687. for (uint8_t i = 0; i < 12; i++)
  688. {
  689. pattern[i] = pgm_read_word_far((uint16_t*)(xyzcal_point_pattern + i));
  690. // DBG(_n(" pattern[%d]=%d\n"), i, pattern[i]);
  691. }
  692. uint8_t c = 0;
  693. uint8_t r = 0;
  694. if (xyzcal_find_pattern_12x12_in_32x32(pixels, pattern, &c, &r) > 66) //total pixels=144, corner=12 (1/2 = 66)
  695. {
  696. DBG(_n(" pattern found at %d %d\n"), c, r);
  697. c += 6;
  698. r += 6;
  699. x += ((int16_t)c - 16) << 6;
  700. y += ((int16_t)r - 16) << 6;
  701. DBG(_n(" x=%d y=%d z=%d\n"), x, y, z);
  702. xyzcal_lineXYZ_to(x, y, z, 200, 0);
  703. ret = true;
  704. }
  705. for (uint16_t i = 0; i < sizeof(block_t)*BLOCK_BUFFER_SIZE; i++)
  706. pixels[i] = 0;
  707. return ret;
  708. }
  709. bool xyzcal_find_bed_induction_sensor_point_xy(void)
  710. {
  711. 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]);
  712. bool ret = false;
  713. st_synchronize();
  714. int16_t x = _X;
  715. int16_t y = _Y;
  716. int16_t z = _Z;
  717. uint8_t point = xyzcal_xycoords2point(x, y);
  718. x = pgm_read_word_far((uint16_t*)(xyzcal_point_xcoords + point));
  719. y = pgm_read_word_far((uint16_t*)(xyzcal_point_ycoords + point));
  720. DBG(_n("point=%d x=%d y=%d z=%d\n"), point, x, y, z);
  721. xyzcal_meassure_enter();
  722. xyzcal_lineXYZ_to(x, y, z, 200, 0);
  723. if (xyzcal_searchZ())
  724. {
  725. int16_t z = _Z;
  726. xyzcal_lineXYZ_to(x, y, z, 200, 0);
  727. if (xyzcal_scan_and_process())
  728. {
  729. if (xyzcal_find_point_center2(500))
  730. {
  731. uint32_t x_avg = 0;
  732. uint32_t y_avg = 0;
  733. uint8_t n; for (n = 0; n < 4; n++)
  734. {
  735. if (!xyzcal_find_point_center2(1000)) break;
  736. x_avg += _X;
  737. y_avg += _Y;
  738. }
  739. if (n == 4)
  740. {
  741. xyzcal_lineXYZ_to(x_avg >> 2, y_avg >> 2, _Z, 200, 0);
  742. ret = true;
  743. }
  744. }
  745. }
  746. }
  747. xyzcal_meassure_leave();
  748. return ret;
  749. }
  750. #endif //NEW_XYZCAL