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