xyzcal.cpp 30 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012
  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. static const char endl[2] PROGMEM = "\n";
  16. #define DBG(args...) printf_P(args)
  17. //#define DBG(args...)
  18. #ifndef _n
  19. #define _n PSTR
  20. #endif //_n
  21. #define _X ((int16_t)count_position[X_AXIS])
  22. #define _Y ((int16_t)count_position[Y_AXIS])
  23. #define _Z ((int16_t)count_position[Z_AXIS])
  24. #define _E ((int16_t)count_position[E_AXIS])
  25. #define _X_ (count_position[X_AXIS])
  26. #define _Y_ (count_position[Y_AXIS])
  27. #define _Z_ (count_position[Z_AXIS])
  28. #define _E_ (count_position[E_AXIS])
  29. #ifndef M_PI
  30. const constexpr float M_PI = 3.1415926535897932384626433832795f;
  31. #endif
  32. const constexpr uint8_t X_PLUS = 0;
  33. const constexpr uint8_t X_MINUS = 1;
  34. const constexpr uint8_t Y_PLUS = 0;
  35. const constexpr uint8_t Y_MINUS = 1;
  36. const constexpr uint8_t Z_PLUS = 0;
  37. const constexpr uint8_t Z_MINUS = 1;
  38. const constexpr uint8_t X_PLUS_MASK = 0;
  39. const constexpr uint8_t X_MINUS_MASK = X_AXIS_MASK;
  40. const constexpr uint8_t Y_PLUS_MASK = 0;
  41. const constexpr uint8_t Y_MINUS_MASK = Y_AXIS_MASK;
  42. const constexpr uint8_t Z_PLUS_MASK = 0;
  43. const constexpr uint8_t Z_MINUS_MASK = Z_AXIS_MASK;
  44. /// Max. jerk in PrusaSlicer, 10000 = 1 mm/s
  45. const constexpr uint16_t MAX_DELAY = 10000;
  46. const constexpr float MIN_SPEED = 0.01f / (MAX_DELAY * 0.000001f);
  47. /// 200 = 50 mm/s
  48. const constexpr uint16_t Z_MIN_DELAY = 200;
  49. const constexpr uint16_t Z_ACCEL = 1000;
  50. /// \returns positive value always
  51. #define ABS(a) \
  52. ({ __typeof__ (a) _a = (a); \
  53. _a >= 0 ? _a : (-_a); })
  54. /// \returns maximum of the two
  55. #define MAX(a, b) \
  56. ({ __typeof__ (a) _a = (a); \
  57. __typeof__ (b) _b = (b); \
  58. _a >= _b ? _a : _b; })
  59. /// \returns minimum of the two
  60. #define MIN(a, b) \
  61. ({ __typeof__ (a) _a = (a); \
  62. __typeof__ (b) _b = (b); \
  63. _a <= _b ? _a : _b; })
  64. /// swap values
  65. #define SWAP(a, b) \
  66. ({ __typeof__ (a) c = (a); \
  67. a = (b); \
  68. b = c; })
  69. /// Saturates value
  70. /// \returns min if value is less than min
  71. /// \returns max if value is more than min
  72. /// \returns value otherwise
  73. #define CLAMP(value, min, max) \
  74. ({ __typeof__ (value) a_ = (value); \
  75. __typeof__ (min) min_ = (min); \
  76. __typeof__ (max) max_ = (max); \
  77. ( a_ < min_ ? min_ : (a_ <= max_ ? a_ : max_)); })
  78. /// \returns square of the value
  79. #define SQR(a) \
  80. ({ __typeof__ (a) a_ = (a); \
  81. (a_ * a_); })
  82. /// position types
  83. typedef int16_t pos_i16_t;
  84. typedef long pos_i32_t;
  85. typedef float pos_mm_t;
  86. typedef int16_t usteps_t;
  87. uint8_t check_pinda_0();
  88. uint8_t check_pinda_1();
  89. void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
  90. uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd);
  91. uint8_t round_to_u8(float f){
  92. return (uint8_t)(f + .5f);
  93. }
  94. uint16_t round_to_u16(float f){
  95. return (uint16_t)(f + .5f);
  96. }
  97. int16_t round_to_i16(float f){
  98. return (int16_t)(f + .5f);
  99. }
  100. /// converts millimeters to integer position
  101. pos_i16_t mm_2_pos(pos_mm_t mm){
  102. return (pos_i16_t)(0.5f + mm * 100);
  103. }
  104. /// converts integer position to millimeters
  105. pos_mm_t pos_2_mm(pos_i16_t pos){
  106. return pos * 0.01f;
  107. }
  108. pos_mm_t pos_2_mm(float pos){
  109. return pos * 0.01f;
  110. }
  111. void xyzcal_meassure_enter(void)
  112. {
  113. DBG(_n("xyzcal_meassure_enter\n"));
  114. // disable heaters and stop motion before we initialize sm4
  115. disable_heater();
  116. st_synchronize();
  117. // disable incompatible interrupts
  118. DISABLE_STEPPER_DRIVER_INTERRUPT();
  119. #ifdef WATCHDOG
  120. wdt_disable();
  121. #endif //WATCHDOG
  122. // setup internal callbacks
  123. sm4_stop_cb = 0;
  124. sm4_update_pos_cb = xyzcal_update_pos;
  125. sm4_calc_delay_cb = xyzcal_calc_delay;
  126. }
  127. void xyzcal_meassure_leave(void)
  128. {
  129. DBG(_n("xyzcal_meassure_leave\n"));
  130. // resync planner position from counters (changed by xyzcal_update_pos)
  131. planner_reset_position();
  132. // re-enable interrupts
  133. #ifdef WATCHDOG
  134. wdt_enable(WDTO_4S);
  135. #ifdef EMERGENCY_HANDLERS
  136. WDTCSR |= (1 << WDIE);
  137. #endif //EMERGENCY_HANDLERS
  138. #endif //WATCHDOG
  139. ENABLE_STEPPER_DRIVER_INTERRUPT();
  140. }
  141. uint8_t check_pinda_0()
  142. {
  143. return _PINDA?0:1;
  144. }
  145. uint8_t check_pinda_1()
  146. {
  147. return _PINDA?1:0;
  148. }
  149. uint8_t xyzcal_dm = 0;
  150. void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t)
  151. {
  152. // DBG(_n("xyzcal_update_pos dx=%d dy=%d dz=%d dir=%02x\n"), dx, dy, dz, xyzcal_dm);
  153. if (xyzcal_dm&1) count_position[0] -= dx; else count_position[0] += dx;
  154. if (xyzcal_dm&2) count_position[1] -= dy; else count_position[1] += dy;
  155. if (xyzcal_dm&4) count_position[2] -= dz; else count_position[2] += dz;
  156. // DBG(_n(" after xyzcal_update_pos x=%ld y=%ld z=%ld\n"), count_position[0], count_position[1], count_position[2]);
  157. }
  158. uint16_t xyzcal_sm4_delay = 0;
  159. //#define SM4_ACCEL_TEST
  160. #ifdef SM4_ACCEL_TEST
  161. uint16_t xyzcal_sm4_v0 = 2000;
  162. uint16_t xyzcal_sm4_vm = 45000;
  163. uint16_t xyzcal_sm4_v = xyzcal_sm4_v0;
  164. uint16_t xyzcal_sm4_ac = 2000;
  165. uint16_t xyzcal_sm4_ac2 = (uint32_t)xyzcal_sm4_ac * 1024 / 10000;
  166. //float xyzcal_sm4_vm = 10000;
  167. #endif //SM4_ACCEL_TEST
  168. #ifdef SM4_ACCEL_TEST
  169. uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd)
  170. {
  171. uint16_t del_us = 0;
  172. if (xyzcal_sm4_v & 0xf000) //>=4096
  173. {
  174. del_us = (uint16_t)62500 / (uint16_t)(xyzcal_sm4_v >> 4);
  175. xyzcal_sm4_v += (xyzcal_sm4_ac2 * del_us + 512) >> 10;
  176. if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
  177. if (del_us > 25) return del_us - 25;
  178. }
  179. else
  180. {
  181. del_us = (uint32_t)1000000 / xyzcal_sm4_v;
  182. xyzcal_sm4_v += ((uint32_t)xyzcal_sm4_ac2 * del_us + 512) >> 10;
  183. if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
  184. if (del_us > 50) return del_us - 50;
  185. }
  186. // uint16_t del_us = (uint16_t)(((float)1000000 / xyzcal_sm4_v) + 0.5);
  187. // uint16_t del_us = (uint32_t)1000000 / xyzcal_sm4_v;
  188. // uint16_t del_us = 100;
  189. // uint16_t del_us = (uint16_t)10000 / xyzcal_sm4_v;
  190. // v += (ac * del_us + 500) / 1000;
  191. // xyzcal_sm4_v += (xyzcal_sm4_ac * del_us) / 1000;
  192. // return xyzcal_sm4_delay;
  193. // DBG(_n("xyzcal_calc_delay nd=%d dd=%d v=%d del_us=%d\n"), nd, dd, xyzcal_sm4_v, del_us);
  194. return 0;
  195. }
  196. #else //SM4_ACCEL_TEST
  197. uint16_t xyzcal_calc_delay(uint16_t, uint16_t)
  198. {
  199. return xyzcal_sm4_delay;
  200. }
  201. #endif //SM4_ACCEL_TEST
  202. /// Moves printer to absolute position [x,y,z] defined in integer position system
  203. /// check_pinda == 0: ordinary move
  204. /// check_pinda == 1: stop when PINDA triggered
  205. /// check_pinda == -1: stop when PINDA untriggered
  206. bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda)
  207. {
  208. // DBG(_n("xyzcal_lineXYZ_to x=%d y=%d z=%d check=%d\n"), x, y, z, check_pinda);
  209. x -= (int16_t)count_position[0];
  210. y -= (int16_t)count_position[1];
  211. z -= (int16_t)count_position[2];
  212. xyzcal_dm = ((x<0)?1:0) | ((y<0)?2:0) | ((z<0)?4:0);
  213. sm4_set_dir_bits(xyzcal_dm);
  214. sm4_stop_cb = check_pinda?((check_pinda<0)?check_pinda_0:check_pinda_1):0;
  215. xyzcal_sm4_delay = delay_us;
  216. // uint32_t u = _micros();
  217. bool ret = sm4_line_xyz_ui(abs(x), abs(y), abs(z)) ? true : false;
  218. // u = _micros() - u;
  219. return ret;
  220. }
  221. /// Moves printer to absolute position [x,y,z] defined in millimeters
  222. 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){
  223. return xyzcal_lineXYZ_to(mm_2_pos(x), mm_2_pos(y), mm_2_pos(z), delay_us, check_pinda);
  224. }
  225. 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)
  226. {
  227. bool ret = false;
  228. float r = 0; //radius
  229. uint16_t ad = 0; //angle [deg]
  230. float ar; //angle [rad]
  231. uint8_t dad = 0; //delta angle [deg]
  232. uint8_t dad_min = 4; //delta angle min [deg]
  233. uint8_t dad_max = 16; //delta angle max [deg]
  234. uint8_t k = 720 / (dad_max - dad_min); //delta calculation constant
  235. ad = 0;
  236. if (pad) ad = *pad % 720;
  237. //@size=214
  238. DBG(_n("xyzcal_spiral2 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
  239. // lcd_set_cursor(0, 4);
  240. // char text[10];
  241. // snprintf(text, 10, "%4d", z0);
  242. // lcd_print(text);
  243. for (; ad < 720; ad++)
  244. {
  245. if (radius > 0)
  246. {
  247. dad = dad_max - (ad / k);
  248. r = (float)(((uint32_t)ad) * radius) / 720;
  249. }
  250. else
  251. {
  252. dad = dad_max - ((719 - ad) / k);
  253. r = (float)(((uint32_t)(719 - ad)) * (-radius)) / 720;
  254. }
  255. ar = radians(ad + rotation);
  256. int x = (int)(cx + (cos(ar) * r));
  257. int y = (int)(cy + (sin(ar) * r));
  258. int z = (int)(z0 - ((float)((int32_t)dz * ad) / 720));
  259. if (xyzcal_lineXYZ_to(x, y, z, delay_us, check_pinda))
  260. {
  261. ad += dad + 1;
  262. ret = true;
  263. break;
  264. }
  265. ad += dad;
  266. }
  267. if (pad) *pad = ad;
  268. // if(ret){
  269. // lcd_set_cursor(0, 4);
  270. // lcd_print(" ");
  271. // }
  272. return ret;
  273. }
  274. 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)
  275. {
  276. bool ret = false;
  277. uint16_t ad = 0;
  278. if (pad) ad = *pad;
  279. //@size=274
  280. DBG(_n("xyzcal_spiral8 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
  281. if (!ret && (ad < 720))
  282. if ((ret = xyzcal_spiral2(cx, cy, z0 - 0*dz, dz, radius, 0, delay_us, check_pinda, &ad)) != 0)
  283. ad += 0;
  284. if (!ret && (ad < 1440))
  285. if ((ret = xyzcal_spiral2(cx, cy, z0 - 1*dz, dz, -radius, 0, delay_us, check_pinda, &ad)) != 0)
  286. ad += 720;
  287. if (!ret && (ad < 2160))
  288. if ((ret = xyzcal_spiral2(cx, cy, z0 - 2*dz, dz, radius, 180, delay_us, check_pinda, &ad)) != 0)
  289. ad += 1440;
  290. if (!ret && (ad < 2880))
  291. if ((ret = xyzcal_spiral2(cx, cy, z0 - 3*dz, dz, -radius, 180, delay_us, check_pinda, &ad)) != 0)
  292. ad += 2160;
  293. if (pad) *pad = ad;
  294. return ret;
  295. }
  296. #ifdef XYZCAL_MEASSURE_PINDA_HYSTEREZIS
  297. int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples)
  298. {
  299. DBG(_n("xyzcal_meassure_pinda_hysterezis\n"));
  300. int8_t ret = -1; // PINDA signal error
  301. int16_t z = _Z;
  302. int16_t sum_up = 0;
  303. int16_t sum_dn = 0;
  304. int16_t up;
  305. int16_t dn;
  306. uint8_t sample;
  307. xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
  308. xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1);
  309. if (!_PINDA)
  310. {
  311. for (sample = 0; sample < samples; sample++)
  312. {
  313. dn = _Z;
  314. if (!xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1)) break;
  315. dn = dn - _Z;
  316. up = _Z;
  317. if (!xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1)) break;
  318. up = _Z - up;
  319. DBG(_n("%d. up=%d dn=%d\n"), sample, up, dn);
  320. sum_up += up;
  321. sum_dn += dn;
  322. if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
  323. {
  324. ret = -2; // difference between up-dn to high
  325. break;
  326. }
  327. }
  328. if (sample == samples)
  329. {
  330. up = sum_up / samples;
  331. dn = sum_dn / samples;
  332. uint16_t hyst = (up + dn) / 2;
  333. if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
  334. ret = -2; // difference between up-dn to high
  335. else if ((hyst < XYZCAL_PINDA_HYST_MIN) || (hyst > XYZCAL_PINDA_HYST_MAX))
  336. ret = -3; // hysterezis out of range
  337. else
  338. ret = hyst;
  339. }
  340. }
  341. xyzcal_lineXYZ_to(_X, _Y, z, delay_us, 0);
  342. return ret;
  343. }
  344. #endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS
  345. void print_hysteresis(int16_t min_z, int16_t max_z, int16_t step){
  346. int16_t delay_us = 600;
  347. int16_t trigger = 0;
  348. int16_t untrigger = 0;
  349. DBG(_n("Hysteresis\n"));
  350. xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 0);
  351. for (int16_t z = min_z; z <= max_z; z += step){
  352. xyzcal_lineXYZ_to(_X, _Y, z, delay_us, -1);
  353. untrigger = _Z;
  354. xyzcal_lineXYZ_to(_X, _Y, z, delay_us, 0);
  355. xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
  356. trigger = _Z;
  357. //xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 0);
  358. //@size=114
  359. DBG(_n("min, trigger, untrigger, max: [%d %d %d %d]\n"), _Z, trigger, untrigger, z);
  360. }
  361. }
  362. void update_position_1_step(uint8_t axis, uint8_t dir){
  363. if (axis & X_AXIS_MASK)
  364. _X_ += dir & X_AXIS_MASK ? -1 : 1;
  365. if (axis & Y_AXIS_MASK)
  366. _Y_ += dir & Y_AXIS_MASK ? -1 : 1;
  367. if (axis & Z_AXIS_MASK)
  368. _Z_ += dir & Z_AXIS_MASK ? -1 : 1;
  369. }
  370. void set_axes_dir(uint8_t axes, uint8_t dir){
  371. if (axes & X_AXIS_MASK)
  372. sm4_set_dir(X_AXIS, dir & X_AXIS_MASK);
  373. if (axes & Y_AXIS_MASK)
  374. sm4_set_dir(Y_AXIS, dir & Y_AXIS_MASK);
  375. if (axes & Z_AXIS_MASK)
  376. sm4_set_dir(Z_AXIS, dir & Z_AXIS_MASK);
  377. }
  378. /// Accelerate up to max.speed (defined by @min_delay_us)
  379. /// does not update global positions
  380. void accelerate_1_step(uint8_t axes, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){
  381. sm4_do_step(axes);
  382. /// keep max speed (avoid extra computation)
  383. if (acc > 0 && delay_us == min_delay_us){
  384. delayMicroseconds(delay_us);
  385. return;
  386. }
  387. // v1 = v0 + a * t
  388. // 0.01 = length of a step
  389. const float t0 = delay_us * 0.000001f;
  390. const float v1 = (0.01f / t0 + acc * t0);
  391. uint16_t t1;
  392. if (v1 <= 0.16f){ ///< slowest speed convertible to uint16_t delay
  393. t1 = MAX_DELAY; ///< already too slow so it wants to move back
  394. } else {
  395. /// don't exceed max.speed
  396. t1 = MAX(min_delay_us, round_to_u16(0.01f / v1 * 1000000.f));
  397. }
  398. /// make sure delay has changed a bit at least
  399. if (t1 == delay_us && acc != 0){
  400. if (acc > 0)
  401. t1--;
  402. else
  403. t1++;
  404. }
  405. //DBG(_n("%d "), t1);
  406. delayMicroseconds(t1);
  407. delay_us = t1;
  408. }
  409. /// Goes defined number of steps while accelerating
  410. /// updates global positions
  411. void accelerate(uint8_t axes, uint8_t dir, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us, uint16_t steps){
  412. set_axes_dir(axes, dir);
  413. while (steps--){
  414. accelerate_1_step(axes, acc, delay_us, min_delay_us);
  415. update_position_1_step(axes, dir);
  416. }
  417. }
  418. /// keeps speed and then it decelerates to a complete stop (if possible)
  419. /// it goes defined number of steps
  420. /// returns after each step
  421. /// \returns true if step was done
  422. /// does not update global positions
  423. bool go_and_stop_1_step(uint8_t axes, int16_t dec, uint16_t &delay_us, uint16_t &steps){
  424. if (steps <= 0 || dec <= 0)
  425. return false;
  426. /// deceleration distance in steps, s = 1/2 v^2 / a
  427. uint16_t s = round_to_u16(100 * 0.5f * SQR(0.01f) / (SQR((float)delay_us) * dec));
  428. if (steps > s){
  429. /// go steady
  430. sm4_do_step(axes);
  431. delayMicroseconds(delay_us);
  432. } else {
  433. /// decelerate
  434. accelerate_1_step(axes, -dec, delay_us, delay_us);
  435. }
  436. --steps;
  437. return true;
  438. }
  439. /// \param dir sets direction of movement
  440. /// updates global positions
  441. void go_and_stop(uint8_t axes, uint8_t dir, int16_t dec, uint16_t &delay_us, uint16_t steps){
  442. set_axes_dir(axes, dir);
  443. while (go_and_stop_1_step(axes, dec, delay_us, steps)){
  444. update_position_1_step(axes, dir);
  445. }
  446. }
  447. /// goes all the way to stop
  448. /// \returns steps done
  449. /// updates global positions
  450. void stop_smoothly(uint8_t axes, uint8_t dir, int16_t dec, uint16_t &delay_us){
  451. if (dec <= 0)
  452. return;
  453. set_axes_dir(axes, dir);
  454. while (delay_us < MAX_DELAY){
  455. accelerate_1_step(axes, -dec, delay_us, delay_us);
  456. update_position_1_step(axes, dir);
  457. }
  458. }
  459. void go_start_stop(uint8_t axes, uint8_t dir, int16_t acc, uint16_t min_delay_us, uint16_t steps){
  460. if (steps == 0)
  461. return;
  462. uint16_t current_delay_us = MAX_DELAY;
  463. const uint16_t half = steps / 2;
  464. accelerate(axes, dir, acc, current_delay_us, min_delay_us, half);
  465. go_and_stop(axes, dir, -acc, current_delay_us, steps - half);
  466. }
  467. /// moves X, Y, Z one after each other
  468. /// starts and ends at 0 speed
  469. void go_manhattan(int16_t x, int16_t y, int16_t z, int16_t acc, uint16_t min_delay_us){
  470. int32_t length;
  471. // DBG(_n("x %d -> %d, "), x, _X);
  472. length = x - _X;
  473. go_start_stop(X_AXIS_MASK, length < 0 ? X_MINUS_MASK : X_PLUS_MASK, acc, min_delay_us, ABS(length));
  474. // DBG(_n("y %d -> %d, "), y, _Y);
  475. length = y - _Y;
  476. go_start_stop(Y_AXIS_MASK, length < 0 ? Y_MINUS_MASK : Y_PLUS_MASK, acc, min_delay_us, ABS(length));
  477. // DBG(_n("z %d -> %d\n"), z, _Z);
  478. length = z - _Z;
  479. go_start_stop(Z_AXIS_MASK, length < 0 ? Z_MINUS_MASK : Z_PLUS_MASK, acc, min_delay_us, ABS(length));
  480. // DBG(_n("\n"));
  481. }
  482. 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){
  483. if (!pixels)
  484. return;
  485. int16_t z_trig;
  486. uint16_t line_buffer[32];
  487. uint16_t current_delay_us = MAX_DELAY; ///< defines current speed
  488. int16_t start_z;
  489. uint16_t steps_to_go;
  490. DBG(_n("Scan countdown: "));
  491. for (uint8_t r = 0; r < 32; r++){ ///< Y axis
  492. for (uint8_t d = 0; d < 2; ++d){
  493. go_manhattan((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, Z_ACCEL, Z_MIN_DELAY);
  494. xyzcal_lineXYZ_to((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, delay_us, 0);
  495. sm4_set_dir(X_AXIS, d);
  496. //@size=242
  497. DBG(_n("%d\n"), 64 - (r * 2 + d)); ///< to keep OctoPrint connection alive
  498. for (uint8_t c = 0; c < 32; c++){ ///< X axis
  499. /// move to the next point and move Z up diagonally (if needed)
  500. current_delay_us = MAX_DELAY;
  501. const int16_t end_x = ((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx;
  502. const int16_t length_x = ABS(end_x - _X);
  503. const int16_t half_x = length_x / 2;
  504. /// don't go up if PINDA not triggered (optimization)
  505. const bool up = _PINDA;
  506. const uint8_t axes = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK;
  507. const uint8_t dir = Z_PLUS_MASK | (d & 1 ? X_MINUS_MASK : X_PLUS_MASK);
  508. accelerate(axes, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x);
  509. go_and_stop(axes, dir, Z_ACCEL, current_delay_us, length_x - half_x);
  510. z_trig = min_z;
  511. /// move up to un-trigger (surpress hysteresis)
  512. sm4_set_dir(Z_AXIS, Z_PLUS);
  513. /// speed up from stop, go half the way
  514. current_delay_us = MAX_DELAY;
  515. for (start_z = _Z; _Z < (max_z + start_z) / 2; ++_Z_){
  516. if (!_PINDA){
  517. break;
  518. }
  519. accelerate_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY);
  520. }
  521. if (_PINDA){
  522. steps_to_go = MAX(0, max_z - _Z);
  523. while (_PINDA && _Z < max_z){
  524. go_and_stop_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go);
  525. ++_Z_;
  526. }
  527. }
  528. stop_smoothly(Z_AXIS_MASK, Z_PLUS_MASK, Z_ACCEL, current_delay_us);
  529. /// move down to trigger
  530. sm4_set_dir(Z_AXIS, Z_MINUS);
  531. /// speed up
  532. current_delay_us = MAX_DELAY;
  533. for (start_z = _Z; _Z > (min_z + start_z) / 2; --_Z_){
  534. if (_PINDA){
  535. z_trig = _Z;
  536. break;
  537. }
  538. accelerate_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY);
  539. }
  540. /// slow down
  541. if (!_PINDA){
  542. steps_to_go = MAX(0, _Z - min_z);
  543. while (!_PINDA && _Z > min_z){
  544. go_and_stop_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go);
  545. --_Z_;
  546. }
  547. z_trig = _Z;
  548. }
  549. /// slow down to stop but not lower than min_z
  550. while (_Z > min_z && current_delay_us < MAX_DELAY){
  551. accelerate_1_step(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY);
  552. --_Z_;
  553. }
  554. if (d == 0){
  555. line_buffer[c] = (uint16_t)(z_trig - min_z);
  556. } else {
  557. /// !!! data reversed in X
  558. // DBG(_n("%04x"), ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2);
  559. /// save average of both directions (filters effect of hysteresis)
  560. pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2);
  561. }
  562. }
  563. }
  564. }
  565. DBG(endl);
  566. }
  567. /// Returns rate of match
  568. /// max match = 132, min match = 0
  569. uint8_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t c, uint8_t r){
  570. uint8_t thr = 16;
  571. uint8_t match = 0;
  572. for (uint8_t i = 0; i < 12; ++i){
  573. for (uint8_t j = 0; j < 12; ++j){
  574. /// skip corners (3 pixels in each)
  575. if (((i == 0) || (i == 11)) && ((j < 2) || (j >= 10))) continue;
  576. if (((j == 0) || (j == 11)) && ((i < 2) || (i >= 10))) continue;
  577. const uint16_t idx = (c + j) + 32 * ((uint16_t)r + i);
  578. const bool high_pix = pixels[idx] > thr;
  579. const bool high_pat = pattern[i] & (1 << j);
  580. if (high_pix == high_pat)
  581. match++;
  582. }
  583. }
  584. return match;
  585. }
  586. /// Searches for best match of pattern by shifting it
  587. /// Returns rate of match and the best location
  588. /// max match = 132, min match = 0
  589. uint8_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr){
  590. if (!pixels || !pattern || !pc || !pr)
  591. return -1;
  592. uint8_t max_c = 0;
  593. uint8_t max_r = 0;
  594. uint8_t max_match = 0;
  595. // DBG(_n("Matching:\n"));
  596. /// pixel precision
  597. for (uint8_t r = 0; r < (32 - 12); ++r){
  598. for (uint8_t c = 0; c < (32 - 12); ++c){
  599. const uint8_t match = xyzcal_match_pattern_12x12_in_32x32(pattern, pixels, c, r);
  600. if (max_match < match){
  601. max_c = c;
  602. max_r = r;
  603. max_match = match;
  604. }
  605. // DBG(_n("%d "), match);
  606. }
  607. // DBG(_n("\n"));
  608. }
  609. //@size=278
  610. DBG(_n("Pattern center [%f %f], match %f%%\n"), max_c + 5.5f, max_r + 5.5f, max_match / 1.32f);
  611. *pc = max_c;
  612. *pr = max_r;
  613. return max_match;
  614. }
  615. const uint16_t xyzcal_point_pattern_10[12] PROGMEM = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000};
  616. const uint16_t xyzcal_point_pattern_08[12] PROGMEM = {0x000, 0x000, 0x0f0, 0x1f8, 0x3fc, 0x3fc, 0x3fc, 0x3fc, 0x1f8, 0x0f0, 0x000, 0x000};
  617. bool xyzcal_searchZ(void) {
  618. //@size=118
  619. DBG(_n("xyzcal_searchZ x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
  620. int16_t x0 = _X;
  621. int16_t y0 = _Y;
  622. int16_t z = _Z;
  623. // int16_t min_z = -6000;
  624. // int16_t dz = 100;
  625. while (z > -2300) { //-6mm + 0.25mm
  626. uint16_t ad = 0;
  627. if (xyzcal_spiral8(x0, y0, z, 100, 900, 320, 1, &ad)) { //dz=100 radius=900 delay=400
  628. //@size=82
  629. DBG(_n(" ON-SIGNAL at x=%d y=%d z=%d ad=%d\n"), _X, _Y, _Z, ad);
  630. /// return to starting XY position
  631. /// magic constant, lowers min_z after searchZ to obtain more dense data in scan
  632. const pos_i16_t lower_z = 72;
  633. xyzcal_lineXYZ_to(x0, y0, _Z - lower_z, 200, 0);
  634. return true;
  635. }
  636. z -= 400;
  637. }
  638. //@size=138
  639. 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]);
  640. return false;
  641. }
  642. /// returns value of any location within data
  643. /// uses bilinear interpolation
  644. float get_value(uint8_t * matrix_32x32, float c, float r){
  645. if (c <= 0 || r <= 0 || c >= 31 || r >= 31)
  646. return 0;
  647. /// calculate weights of nearby points
  648. const float wc1 = c - floor(c);
  649. const float wr1 = r - floor(r);
  650. const float wc0 = 1 - wc1;
  651. const float wr0 = 1 - wr1;
  652. const float w00 = wc0 * wr0;
  653. const float w01 = wc0 * wr1;
  654. const float w10 = wc1 * wr0;
  655. const float w11 = wc1 * wr1;
  656. const uint16_t c0 = c;
  657. const uint16_t c1 = c0 + 1;
  658. const uint16_t r0 = r;
  659. const uint16_t r1 = r0 + 1;
  660. const uint16_t idx00 = c0 + 32 * r0;
  661. const uint16_t idx01 = c0 + 32 * r1;
  662. const uint16_t idx10 = c1 + 32 * r0;
  663. const uint16_t idx11 = c1 + 32 * r1;
  664. /// bilinear resampling
  665. return w00 * matrix_32x32[idx00] + w01 * matrix_32x32[idx01] + w10 * matrix_32x32[idx10] + w11 * matrix_32x32[idx11];
  666. }
  667. const constexpr float m_infinity = -1000.f;
  668. /// replaces the highest number by m_infinity
  669. void remove_highest(float *points, const uint8_t num_points){
  670. if (num_points <= 0)
  671. return;
  672. float max = points[0];
  673. uint8_t max_i = 0;
  674. for (uint8_t i = 0; i < num_points; ++i){
  675. if (max < points[i]){
  676. max = points[i];
  677. max_i = i;
  678. }
  679. }
  680. points[max_i] = m_infinity;
  681. }
  682. /// return the highest number in the list
  683. float highest(float *points, const uint8_t num_points){
  684. if (num_points <= 0)
  685. return 0;
  686. float max = points[0];
  687. for (uint8_t i = 0; i < num_points; ++i){
  688. if (max < points[i]){
  689. max = points[i];
  690. }
  691. }
  692. return max;
  693. }
  694. /// slow bubble sort but short
  695. void sort(float *points, const uint8_t num_points){
  696. /// one direction bubble sort
  697. for (uint8_t i = 0; i < num_points; ++i){
  698. for (uint8_t j = 0; j < num_points - i - 1; ++j){
  699. if (points[j] > points[j + 1])
  700. SWAP(points[j], points[j + 1]);
  701. }
  702. }
  703. // DBG(_n("Sorted: "));
  704. // for (uint8_t i = 0; i < num_points; ++i)
  705. // DBG(_n("%f "), points[i]);
  706. // DBG(_n("\n"));
  707. }
  708. /// sort array and returns median value
  709. /// don't send empty array or nullptr
  710. float median(float *points, const uint8_t num_points){
  711. sort(points, num_points);
  712. return points[num_points / 2];
  713. }
  714. float __attribute__ ((noinline)) CLAMP_median(float *shifts, uint8_t blocks, float norm){
  715. const constexpr float max_change = 0.5f; ///< avoids too fast changes (avoid oscillation)
  716. return CLAMP( median(shifts, blocks) * norm, -max_change, max_change);
  717. }
  718. /// Searches for circle iteratively
  719. /// Uses points on the perimeter. If point is high it pushes circle out of the center (shift or change of radius),
  720. /// otherwise to the center.
  721. /// Algorithm is stopped after fixed number of iterations. Move is limited to 0.5 px per iteration.
  722. void dynamic_circle(uint8_t *matrix_32x32, float &x, float &y, float &r, uint8_t iterations){
  723. /// circle of 10.5 diameter has 33 in circumference, don't go much above
  724. const constexpr uint8_t num_points = 33;
  725. const float pi_2_div_num_points = 2 * M_PI / num_points;
  726. const constexpr uint8_t target_z = 32; ///< target z height of the circle
  727. const uint8_t blocks = num_points;
  728. float shifts_x[blocks];
  729. float shifts_y[blocks];
  730. float shifts_r[blocks];
  731. // DBG(_n(" [%f, %f][%f] start circle\n"), x, y, r);
  732. for (int8_t i = iterations; i > 0; --i){
  733. //@size=128B
  734. // DBG(_n(" [%f, %f][%f] circle\n"), x, y, r);
  735. /// read points on the circle
  736. for (uint8_t p = 0; p < num_points; ++p){
  737. const float angle = p * pi_2_div_num_points;
  738. const float height = get_value(matrix_32x32, r * cos(angle) + x, r * sin(angle) + y) - target_z;
  739. // DBG(_n("%f "), point);
  740. shifts_x[p] = cos(angle) * height;
  741. shifts_y[p] = sin(angle) * height;
  742. shifts_r[p] = height;
  743. }
  744. // DBG(_n(" points\n"));
  745. const float reducer = 32.f; ///< reduces speed of convergency to avoid oscillation
  746. const float norm = 1.f / reducer;
  747. // x += CLAMP(median(shifts_x, blocks) * norm, -max_change, max_change);
  748. // y += CLAMP(median(shifts_y, blocks) * norm, -max_change, max_change);
  749. // r += CLAMP(median(shifts_r, blocks) * norm * .5f, -max_change, max_change);
  750. //104B down
  751. x += CLAMP_median(shifts_x, blocks, norm);
  752. y += CLAMP_median(shifts_y, blocks, norm);
  753. r += CLAMP_median(shifts_r, blocks, norm * .5f);
  754. r = MAX(2, r);
  755. }
  756. //@size=118
  757. DBG(_n(" [%f, %f][%f] final circle\n"), x, y, r);
  758. }
  759. /// Prints matrix in hex to debug output (serial line)
  760. void print_image(const uint8_t *matrix_32x32){
  761. for (uint8_t y = 0; y < 32; ++y){
  762. const uint16_t idx_y = y * 32;
  763. for (uint8_t x = 0; x < 32; ++x){
  764. DBG(_n("%02x"), matrix_32x32[idx_y + x]);
  765. }
  766. DBG(endl);
  767. }
  768. DBG(endl);
  769. }
  770. /// Takes two patterns and searches them in matrix32
  771. /// \returns best match
  772. uint8_t find_patterns(uint8_t *matrix32, uint16_t *pattern08, uint16_t *pattern10, uint8_t &col, uint8_t &row){
  773. uint8_t c08 = 0;
  774. uint8_t r08 = 0;
  775. uint8_t match08 = 0;
  776. uint8_t c10 = 0;
  777. uint8_t r10 = 0;
  778. uint8_t match10 = 0;
  779. match08 = xyzcal_find_pattern_12x12_in_32x32(matrix32, pattern08, &c08, &r08);
  780. match10 = xyzcal_find_pattern_12x12_in_32x32(matrix32, pattern10, &c10, &r10);
  781. if (match08 > match10){
  782. col = c08;
  783. row = r08;
  784. return match08;
  785. }
  786. col = c10;
  787. row = r10;
  788. return match10;
  789. }
  790. /// Scan should include normal data.
  791. /// If it's too extreme (00, FF) it could be caused by biased sensor.
  792. /// \return true if data looks normal
  793. bool check_scan(uint8_t *matrix32){
  794. /// magic constants that define normality
  795. const int16_t threshold_total = 900;
  796. const int threshold_extreme = 50;
  797. int16_t mins = 0;
  798. int16_t maxs = 0;
  799. for (int16_t i = 0; i < 32*32;++i){
  800. if (matrix32[i] == 0) {
  801. ++mins;
  802. } else if (matrix32[i] == 0xFF){
  803. ++maxs;
  804. }
  805. }
  806. const int16_t rest = 1024 - mins - maxs;
  807. if (mins + maxs > threshold_total
  808. && mins > threshold_extreme
  809. && maxs > threshold_extreme
  810. && mins > rest
  811. && maxs > rest)
  812. return false;
  813. return true;
  814. }
  815. /// scans area around the current head location and
  816. /// searches for the center of the calibration pin
  817. BedSkewOffsetDetectionResultType xyzcal_scan_and_process(){
  818. //@size=44
  819. // DBG(_n("sizeof(block_buffer)=%d\n"), sizeof(block_t)*BLOCK_BUFFER_SIZE);
  820. BedSkewOffsetDetectionResultType ret = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  821. int16_t x = _X;
  822. int16_t y = _Y;
  823. const int16_t z = _Z;
  824. uint8_t *matrix32 = (uint8_t *)block_buffer;
  825. uint16_t *pattern08 = (uint16_t *)(matrix32 + 32 * 32);
  826. uint16_t *pattern10 = (uint16_t *)(pattern08 + 12);
  827. for (uint8_t i = 0; i < 12; i++){
  828. pattern08[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern_08 + i));
  829. pattern10[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern_10 + i));
  830. }
  831. xyzcal_scan_pixels_32x32_Zhop(x, y, z, 2400, 200, matrix32);
  832. print_image(matrix32);
  833. if (!check_scan(matrix32))
  834. return BED_SKEW_OFFSET_DETECTION_POINT_SCAN_FAILED;
  835. /// SEARCH FOR BINARY CIRCLE
  836. uint8_t uc = 0;
  837. uint8_t ur = 0;
  838. /// max match = 132, 1/2 good = 66, 2/3 good = 88
  839. if (find_patterns(matrix32, pattern08, pattern10, uc, ur) >= 88){
  840. /// find precise circle
  841. /// move to the center of the pattern (+5.5)
  842. float xf = uc + 5.5f;
  843. float yf = ur + 5.5f;
  844. float radius = 4.5f; ///< default radius
  845. constexpr const uint8_t iterations = 20;
  846. dynamic_circle(matrix32, xf, yf, radius, iterations);
  847. if (fabs(xf - (uc + 5.5f)) > 3 || fabs(yf - (ur + 5.5f)) > 3 || fabs(radius - 5) > 3){
  848. //@size=88
  849. DBG(_n(" [%f %f][%f] mm divergence\n"), xf - (uc + 5.5f), yf - (ur + 5.5f), radius - 5);
  850. /// dynamic algorithm diverged, use original position instead
  851. xf = uc + 5.5f;
  852. yf = ur + 5.5f;
  853. }
  854. /// move to the center of area and convert to position
  855. xf = (float)x + (xf - 15.5f) * 64;
  856. yf = (float)y + (yf - 15.5f) * 64;
  857. //@size=114
  858. DBG(_n(" [%f %f] mm pattern center\n"), pos_2_mm(xf), pos_2_mm(yf));
  859. x = round_to_i16(xf);
  860. y = round_to_i16(yf);
  861. xyzcal_lineXYZ_to(x, y, z, 200, 0);
  862. ret = BED_SKEW_OFFSET_DETECTION_POINT_FOUND;
  863. }
  864. /// wipe buffer
  865. for (uint16_t i = 0; i < sizeof(block_t)*BLOCK_BUFFER_SIZE; i++)
  866. matrix32[i] = 0;
  867. return ret;
  868. }
  869. BedSkewOffsetDetectionResultType xyzcal_find_bed_induction_sensor_point_xy(void) {
  870. // 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]);
  871. BedSkewOffsetDetectionResultType ret = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  872. xyzcal_meassure_enter();
  873. if (xyzcal_searchZ())
  874. ret = xyzcal_scan_and_process();
  875. xyzcal_meassure_leave();
  876. return ret;
  877. }
  878. #endif //NEW_XYZCAL