xyzcal.cpp 29 KB

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