xyzcal.cpp 24 KB

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