xyzcal.cpp 30 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016
  1. //xyzcal.cpp - xyz calibration with image processing
  2. #include "Configuration_var.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_measure_enter(void)
  112. {
  113. DBG(_n("xyzcal_measure_enter\n"));
  114. lcd_puts_at_P(4,3,PSTR("Measure center ")); ////MSG_MEASURE_CENTER c=16
  115. // disable heaters and stop motion before we initialize sm4
  116. disable_heater();
  117. st_synchronize();
  118. // disable incompatible interrupts
  119. DISABLE_STEPPER_DRIVER_INTERRUPT();
  120. #ifdef WATCHDOG
  121. wdt_disable();
  122. #endif //WATCHDOG
  123. // setup internal callbacks
  124. sm4_stop_cb = 0;
  125. sm4_update_pos_cb = xyzcal_update_pos;
  126. sm4_calc_delay_cb = xyzcal_calc_delay;
  127. }
  128. void xyzcal_measure_leave(void)
  129. {
  130. DBG(_n("xyzcal_measure_leave\n"));
  131. lcd_set_cursor(4,3);
  132. lcd_space(16);
  133. // resync planner position from counters (changed by xyzcal_update_pos)
  134. planner_reset_position();
  135. // re-enable interrupts
  136. #ifdef WATCHDOG
  137. wdt_enable(WDTO_4S);
  138. #ifdef EMERGENCY_HANDLERS
  139. WDTCSR |= (1 << WDIE);
  140. #endif //EMERGENCY_HANDLERS
  141. #endif //WATCHDOG
  142. ENABLE_STEPPER_DRIVER_INTERRUPT();
  143. }
  144. uint8_t check_pinda_0()
  145. {
  146. return _PINDA?0:1;
  147. }
  148. uint8_t check_pinda_1()
  149. {
  150. return _PINDA?1:0;
  151. }
  152. uint8_t xyzcal_dm = 0;
  153. void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t)
  154. {
  155. // DBG(_n("xyzcal_update_pos dx=%d dy=%d dz=%d dir=%02x\n"), dx, dy, dz, xyzcal_dm);
  156. if (xyzcal_dm&1) count_position[0] -= dx; else count_position[0] += dx;
  157. if (xyzcal_dm&2) count_position[1] -= dy; else count_position[1] += dy;
  158. if (xyzcal_dm&4) count_position[2] -= dz; else count_position[2] += dz;
  159. // DBG(_n(" after xyzcal_update_pos x=%ld y=%ld z=%ld\n"), count_position[0], count_position[1], count_position[2]);
  160. }
  161. uint16_t xyzcal_sm4_delay = 0;
  162. //#define SM4_ACCEL_TEST
  163. #ifdef SM4_ACCEL_TEST
  164. uint16_t xyzcal_sm4_v0 = 2000;
  165. uint16_t xyzcal_sm4_vm = 45000;
  166. uint16_t xyzcal_sm4_v = xyzcal_sm4_v0;
  167. uint16_t xyzcal_sm4_ac = 2000;
  168. uint16_t xyzcal_sm4_ac2 = (uint32_t)xyzcal_sm4_ac * 1024 / 10000;
  169. //float xyzcal_sm4_vm = 10000;
  170. #endif //SM4_ACCEL_TEST
  171. #ifdef SM4_ACCEL_TEST
  172. uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd)
  173. {
  174. uint16_t del_us = 0;
  175. if (xyzcal_sm4_v & 0xf000) //>=4096
  176. {
  177. del_us = (uint16_t)62500 / (uint16_t)(xyzcal_sm4_v >> 4);
  178. xyzcal_sm4_v += (xyzcal_sm4_ac2 * del_us + 512) >> 10;
  179. if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
  180. if (del_us > 25) return del_us - 25;
  181. }
  182. else
  183. {
  184. del_us = (uint32_t)1000000 / xyzcal_sm4_v;
  185. xyzcal_sm4_v += ((uint32_t)xyzcal_sm4_ac2 * del_us + 512) >> 10;
  186. if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
  187. if (del_us > 50) return del_us - 50;
  188. }
  189. // uint16_t del_us = (uint16_t)(((float)1000000 / xyzcal_sm4_v) + 0.5);
  190. // uint16_t del_us = (uint32_t)1000000 / xyzcal_sm4_v;
  191. // uint16_t del_us = 100;
  192. // uint16_t del_us = (uint16_t)10000 / xyzcal_sm4_v;
  193. // v += (ac * del_us + 500) / 1000;
  194. // xyzcal_sm4_v += (xyzcal_sm4_ac * del_us) / 1000;
  195. // return xyzcal_sm4_delay;
  196. // DBG(_n("xyzcal_calc_delay nd=%d dd=%d v=%d del_us=%d\n"), nd, dd, xyzcal_sm4_v, del_us);
  197. return 0;
  198. }
  199. #else //SM4_ACCEL_TEST
  200. uint16_t xyzcal_calc_delay(uint16_t, uint16_t)
  201. {
  202. return xyzcal_sm4_delay;
  203. }
  204. #endif //SM4_ACCEL_TEST
  205. /// Moves printer to absolute position [x,y,z] defined in integer position system
  206. /// check_pinda == 0: ordinary move
  207. /// check_pinda == 1: stop when PINDA triggered
  208. /// check_pinda == -1: stop when PINDA untriggered
  209. bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda)
  210. {
  211. // DBG(_n("xyzcal_lineXYZ_to x=%d y=%d z=%d check=%d\n"), x, y, z, check_pinda);
  212. x -= (int16_t)count_position[0];
  213. y -= (int16_t)count_position[1];
  214. z -= (int16_t)count_position[2];
  215. xyzcal_dm = ((x<0)?1:0) | ((y<0)?2:0) | ((z<0)?4:0);
  216. sm4_set_dir_bits(xyzcal_dm);
  217. sm4_stop_cb = check_pinda?((check_pinda<0)?check_pinda_0:check_pinda_1):0;
  218. xyzcal_sm4_delay = delay_us;
  219. // uint32_t u = _micros();
  220. bool ret = sm4_line_xyz_ui(abs(x), abs(y), abs(z)) ? true : false;
  221. // u = _micros() - u;
  222. return ret;
  223. }
  224. /// Moves printer to absolute position [x,y,z] defined in millimeters
  225. 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){
  226. return xyzcal_lineXYZ_to(mm_2_pos(x), mm_2_pos(y), mm_2_pos(z), delay_us, check_pinda);
  227. }
  228. 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)
  229. {
  230. bool ret = false;
  231. float r = 0; //radius
  232. uint16_t ad = 0; //angle [deg]
  233. float ar; //angle [rad]
  234. uint8_t dad = 0; //delta angle [deg]
  235. uint8_t dad_min = 4; //delta angle min [deg]
  236. uint8_t dad_max = 16; //delta angle max [deg]
  237. uint8_t k = 720 / (dad_max - dad_min); //delta calculation constant
  238. ad = 0;
  239. if (pad) ad = *pad % 720;
  240. //@size=214
  241. DBG(_n("xyzcal_spiral2 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
  242. // lcd_set_cursor(0, 4);
  243. // char text[10];
  244. // snprintf(text, 10, "%4d", z0);
  245. // lcd_print(text);
  246. for (; ad < 720; ad++)
  247. {
  248. if (radius > 0)
  249. {
  250. dad = dad_max - (ad / k);
  251. r = (float)(((uint32_t)ad) * radius) / 720;
  252. }
  253. else
  254. {
  255. dad = dad_max - ((719 - ad) / k);
  256. r = (float)(((uint32_t)(719 - ad)) * (-radius)) / 720;
  257. }
  258. ar = radians(ad + rotation);
  259. int x = (int)(cx + (cos(ar) * r));
  260. int y = (int)(cy + (sin(ar) * r));
  261. int z = (int)(z0 - ((float)((int32_t)dz * ad) / 720));
  262. if (xyzcal_lineXYZ_to(x, y, z, delay_us, check_pinda))
  263. {
  264. ad += dad + 1;
  265. ret = true;
  266. break;
  267. }
  268. ad += dad;
  269. }
  270. if (pad) *pad = ad;
  271. // if(ret){
  272. // lcd_set_cursor(0, 4);
  273. // lcd_print(" ");
  274. // }
  275. return ret;
  276. }
  277. 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)
  278. {
  279. bool ret = false;
  280. uint16_t ad = 0;
  281. if (pad) ad = *pad;
  282. //@size=274
  283. DBG(_n("xyzcal_spiral8 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
  284. if (!ret && (ad < 720))
  285. if ((ret = xyzcal_spiral2(cx, cy, z0 - 0*dz, dz, radius, 0, delay_us, check_pinda, &ad)) != 0)
  286. ad += 0;
  287. if (!ret && (ad < 1440))
  288. if ((ret = xyzcal_spiral2(cx, cy, z0 - 1*dz, dz, -radius, 0, delay_us, check_pinda, &ad)) != 0)
  289. ad += 720;
  290. if (!ret && (ad < 2160))
  291. if ((ret = xyzcal_spiral2(cx, cy, z0 - 2*dz, dz, radius, 180, delay_us, check_pinda, &ad)) != 0)
  292. ad += 1440;
  293. if (!ret && (ad < 2880))
  294. if ((ret = xyzcal_spiral2(cx, cy, z0 - 3*dz, dz, -radius, 180, delay_us, check_pinda, &ad)) != 0)
  295. ad += 2160;
  296. if (pad) *pad = ad;
  297. return ret;
  298. }
  299. #ifdef XYZCAL_MEASURE_PINDA_HYSTERESIS
  300. int8_t xyzcal_measure_pinda_hysteresis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples)
  301. {
  302. DBG(_n("xyzcal_measure_pinda_hysteresis\n"));
  303. int8_t ret = -1; // PINDA signal error
  304. int16_t z = _Z;
  305. int16_t sum_up = 0;
  306. int16_t sum_dn = 0;
  307. int16_t up;
  308. int16_t dn;
  309. uint8_t sample;
  310. xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
  311. xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1);
  312. if (!_PINDA)
  313. {
  314. for (sample = 0; sample < samples; sample++)
  315. {
  316. dn = _Z;
  317. if (!xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1)) break;
  318. dn = dn - _Z;
  319. up = _Z;
  320. if (!xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1)) break;
  321. up = _Z - up;
  322. DBG(_n("%d. up=%d dn=%d\n"), sample, up, dn);
  323. sum_up += up;
  324. sum_dn += dn;
  325. if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
  326. {
  327. ret = -2; // difference between up-dn to high
  328. break;
  329. }
  330. }
  331. if (sample == samples)
  332. {
  333. up = sum_up / samples;
  334. dn = sum_dn / samples;
  335. uint16_t hyst = (up + dn) / 2;
  336. if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
  337. ret = -2; // difference between up-dn to high
  338. else if ((hyst < XYZCAL_PINDA_HYST_MIN) || (hyst > XYZCAL_PINDA_HYST_MAX))
  339. ret = -3; // hysteresis out of range
  340. else
  341. ret = hyst;
  342. }
  343. }
  344. xyzcal_lineXYZ_to(_X, _Y, z, delay_us, 0);
  345. return ret;
  346. }
  347. #endif //XYZCAL_MEASURE_PINDA_HYSTERESIS
  348. void print_hysteresis(int16_t min_z, int16_t max_z, int16_t step){
  349. int16_t delay_us = 600;
  350. int16_t trigger = 0;
  351. int16_t untrigger = 0;
  352. DBG(_n("Hysteresis\n"));
  353. xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 0);
  354. for (int16_t z = min_z; z <= max_z; z += step){
  355. xyzcal_lineXYZ_to(_X, _Y, z, delay_us, -1);
  356. untrigger = _Z;
  357. xyzcal_lineXYZ_to(_X, _Y, z, delay_us, 0);
  358. xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
  359. trigger = _Z;
  360. //xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 0);
  361. //@size=114
  362. DBG(_n("min, trigger, untrigger, max: [%d %d %d %d]\n"), _Z, trigger, untrigger, z);
  363. }
  364. }
  365. void update_position_1_step(uint8_t axis, uint8_t dir){
  366. if (axis & X_AXIS_MASK)
  367. _X_ += dir & X_AXIS_MASK ? -1 : 1;
  368. if (axis & Y_AXIS_MASK)
  369. _Y_ += dir & Y_AXIS_MASK ? -1 : 1;
  370. if (axis & Z_AXIS_MASK)
  371. _Z_ += dir & Z_AXIS_MASK ? -1 : 1;
  372. }
  373. void set_axes_dir(uint8_t axes, uint8_t dir){
  374. if (axes & X_AXIS_MASK)
  375. sm4_set_dir(X_AXIS, dir & X_AXIS_MASK);
  376. if (axes & Y_AXIS_MASK)
  377. sm4_set_dir(Y_AXIS, dir & Y_AXIS_MASK);
  378. if (axes & Z_AXIS_MASK)
  379. sm4_set_dir(Z_AXIS, dir & Z_AXIS_MASK);
  380. }
  381. /// Accelerate up to max.speed (defined by @min_delay_us)
  382. /// does not update global positions
  383. void accelerate_1_step(uint8_t axes, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){
  384. sm4_do_step(axes);
  385. /// keep max speed (avoid extra computation)
  386. if (acc > 0 && delay_us == min_delay_us){
  387. delayMicroseconds(delay_us);
  388. return;
  389. }
  390. // v1 = v0 + a * t
  391. // 0.01 = length of a step
  392. const float t0 = delay_us * 0.000001f;
  393. const float v1 = (0.01f / t0 + acc * t0);
  394. uint16_t t1;
  395. if (v1 <= 0.16f){ ///< slowest speed convertible to uint16_t delay
  396. t1 = MAX_DELAY; ///< already too slow so it wants to move back
  397. } else {
  398. /// don't exceed max.speed
  399. t1 = MAX(min_delay_us, round_to_u16(0.01f / v1 * 1000000.f));
  400. }
  401. /// make sure delay has changed a bit at least
  402. if (t1 == delay_us && acc != 0){
  403. if (acc > 0)
  404. t1--;
  405. else
  406. t1++;
  407. }
  408. //DBG(_n("%d "), t1);
  409. delayMicroseconds(t1);
  410. delay_us = t1;
  411. }
  412. /// Goes defined number of steps while accelerating
  413. /// updates global positions
  414. void accelerate(uint8_t axes, uint8_t dir, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us, uint16_t steps){
  415. set_axes_dir(axes, dir);
  416. while (steps--){
  417. accelerate_1_step(axes, acc, delay_us, min_delay_us);
  418. update_position_1_step(axes, dir);
  419. }
  420. }
  421. /// keeps speed and then it decelerates to a complete stop (if possible)
  422. /// it goes defined number of steps
  423. /// returns after each step
  424. /// \returns true if step was done
  425. /// does not update global positions
  426. bool go_and_stop_1_step(uint8_t axes, int16_t dec, uint16_t &delay_us, uint16_t &steps){
  427. if (steps <= 0 || dec <= 0)
  428. return false;
  429. /// deceleration distance in steps, s = 1/2 v^2 / a
  430. uint16_t s = round_to_u16(100 * 0.5f * SQR(0.01f) / (SQR((float)delay_us) * dec));
  431. if (steps > s){
  432. /// go steady
  433. sm4_do_step(axes);
  434. delayMicroseconds(delay_us);
  435. } else {
  436. /// decelerate
  437. accelerate_1_step(axes, -dec, delay_us, delay_us);
  438. }
  439. --steps;
  440. return true;
  441. }
  442. /// \param dir sets direction of movement
  443. /// updates global positions
  444. void go_and_stop(uint8_t axes, uint8_t dir, int16_t dec, uint16_t &delay_us, uint16_t steps){
  445. set_axes_dir(axes, dir);
  446. while (go_and_stop_1_step(axes, dec, delay_us, steps)){
  447. update_position_1_step(axes, dir);
  448. }
  449. }
  450. /// goes all the way to stop
  451. /// \returns steps done
  452. /// updates global positions
  453. void stop_smoothly(uint8_t axes, uint8_t dir, int16_t dec, uint16_t &delay_us){
  454. if (dec <= 0)
  455. return;
  456. set_axes_dir(axes, dir);
  457. while (delay_us < MAX_DELAY){
  458. accelerate_1_step(axes, -dec, delay_us, delay_us);
  459. update_position_1_step(axes, dir);
  460. }
  461. }
  462. void go_start_stop(uint8_t axes, uint8_t dir, int16_t acc, uint16_t min_delay_us, uint16_t steps){
  463. if (steps == 0)
  464. return;
  465. uint16_t current_delay_us = MAX_DELAY;
  466. const uint16_t half = steps / 2;
  467. accelerate(axes, dir, acc, current_delay_us, min_delay_us, half);
  468. go_and_stop(axes, dir, -acc, current_delay_us, steps - half);
  469. }
  470. /// moves X, Y, Z one after each other
  471. /// starts and ends at 0 speed
  472. void go_manhattan(int16_t x, int16_t y, int16_t z, int16_t acc, uint16_t min_delay_us){
  473. int32_t length;
  474. // DBG(_n("x %d -> %d, "), x, _X);
  475. length = x - _X;
  476. go_start_stop(X_AXIS_MASK, length < 0 ? X_MINUS_MASK : X_PLUS_MASK, acc, min_delay_us, ABS(length));
  477. // DBG(_n("y %d -> %d, "), y, _Y);
  478. length = y - _Y;
  479. go_start_stop(Y_AXIS_MASK, length < 0 ? Y_MINUS_MASK : Y_PLUS_MASK, acc, min_delay_us, ABS(length));
  480. // DBG(_n("z %d -> %d\n"), z, _Z);
  481. length = z - _Z;
  482. go_start_stop(Z_AXIS_MASK, length < 0 ? Z_MINUS_MASK : Z_PLUS_MASK, acc, min_delay_us, ABS(length));
  483. // DBG(_n("\n"));
  484. }
  485. void __attribute__((noinline)) 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){
  486. if (!pixels)
  487. return;
  488. int16_t z_trig;
  489. uint16_t line_buffer[32];
  490. uint16_t current_delay_us = MAX_DELAY; ///< defines current speed
  491. int16_t start_z;
  492. uint16_t steps_to_go;
  493. DBG(_n("Scan countdown: "));
  494. for (uint8_t r = 0; r < 32; r++){ ///< Y axis
  495. for (uint8_t d = 0; d < 2; ++d){
  496. go_manhattan((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, Z_ACCEL, Z_MIN_DELAY);
  497. xyzcal_lineXYZ_to((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, delay_us, 0);
  498. sm4_set_dir(X_AXIS, d);
  499. //@size=242
  500. DBG(_n("%d\n"), 64 - (r * 2 + d)); ///< to keep OctoPrint connection alive
  501. lcd_set_cursor(4,3);
  502. lcd_printf_P(PSTR("Countdown: %d "),64 - (r * 2 + d)); ////MSG_COUNTDOWN c=12
  503. for (uint8_t c = 0; c < 32; c++){ ///< X axis
  504. /// move to the next point and move Z up diagonally (if needed)
  505. current_delay_us = MAX_DELAY;
  506. const int16_t end_x = ((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx;
  507. const int16_t length_x = ABS(end_x - _X);
  508. const int16_t half_x = length_x / 2;
  509. /// don't go up if PINDA not triggered (optimization)
  510. const bool up = _PINDA;
  511. const uint8_t axes = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK;
  512. const uint8_t dir = Z_PLUS_MASK | (d & 1 ? X_MINUS_MASK : X_PLUS_MASK);
  513. accelerate(axes, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x);
  514. go_and_stop(axes, dir, Z_ACCEL, current_delay_us, length_x - half_x);
  515. z_trig = min_z;
  516. /// move up to un-trigger (surpress hysteresis)
  517. sm4_set_dir(Z_AXIS, Z_PLUS);
  518. /// speed up from stop, go half the way
  519. current_delay_us = MAX_DELAY;
  520. for (start_z = _Z; _Z < (max_z + start_z) / 2; ++_Z_){
  521. if (!_PINDA){
  522. break;
  523. }
  524. accelerate_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY);
  525. }
  526. if (_PINDA){
  527. steps_to_go = MAX(0, max_z - _Z);
  528. while (_PINDA && _Z < max_z){
  529. go_and_stop_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go);
  530. ++_Z_;
  531. }
  532. }
  533. stop_smoothly(Z_AXIS_MASK, Z_PLUS_MASK, Z_ACCEL, current_delay_us);
  534. /// move down to trigger
  535. sm4_set_dir(Z_AXIS, Z_MINUS);
  536. /// speed up
  537. current_delay_us = MAX_DELAY;
  538. for (start_z = _Z; _Z > (min_z + start_z) / 2; --_Z_){
  539. if (_PINDA){
  540. z_trig = _Z;
  541. break;
  542. }
  543. accelerate_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY);
  544. }
  545. /// slow down
  546. if (!_PINDA){
  547. steps_to_go = MAX(0, _Z - min_z);
  548. while (!_PINDA && _Z > min_z){
  549. go_and_stop_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go);
  550. --_Z_;
  551. }
  552. z_trig = _Z;
  553. }
  554. /// slow down to stop but not lower than min_z
  555. while (_Z > min_z && current_delay_us < MAX_DELAY){
  556. accelerate_1_step(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY);
  557. --_Z_;
  558. }
  559. if (d == 0){
  560. line_buffer[c] = (uint16_t)(z_trig - min_z);
  561. } else {
  562. /// !!! data reversed in X
  563. // DBG(_n("%04x"), ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2);
  564. /// save average of both directions (filters effect of hysteresis)
  565. pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2);
  566. }
  567. }
  568. }
  569. }
  570. DBG(endl);
  571. }
  572. /// Returns rate of match
  573. /// max match = 132, min match = 0
  574. uint8_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t c, uint8_t r){
  575. uint8_t thr = 16;
  576. uint8_t match = 0;
  577. for (uint8_t i = 0; i < 12; ++i){
  578. for (uint8_t j = 0; j < 12; ++j){
  579. /// skip corners (3 pixels in each)
  580. if (((i == 0) || (i == 11)) && ((j < 2) || (j >= 10))) continue;
  581. if (((j == 0) || (j == 11)) && ((i < 2) || (i >= 10))) continue;
  582. const uint16_t idx = (c + j) + 32 * ((uint16_t)r + i);
  583. const bool high_pix = pixels[idx] > thr;
  584. const bool high_pat = pattern[i] & (1 << j);
  585. if (high_pix == high_pat)
  586. match++;
  587. }
  588. }
  589. return match;
  590. }
  591. /// Searches for best match of pattern by shifting it
  592. /// Returns rate of match and the best location
  593. /// max match = 132, min match = 0
  594. uint8_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr){
  595. if (!pixels || !pattern || !pc || !pr)
  596. return -1;
  597. uint8_t max_c = 0;
  598. uint8_t max_r = 0;
  599. uint8_t max_match = 0;
  600. // DBG(_n("Matching:\n"));
  601. /// pixel precision
  602. for (uint8_t r = 0; r < (32 - 12); ++r){
  603. for (uint8_t c = 0; c < (32 - 12); ++c){
  604. const uint8_t match = xyzcal_match_pattern_12x12_in_32x32(pattern, pixels, c, r);
  605. if (max_match < match){
  606. max_c = c;
  607. max_r = r;
  608. max_match = match;
  609. }
  610. // DBG(_n("%d "), match);
  611. }
  612. // DBG(_n("\n"));
  613. }
  614. //@size=278
  615. DBG(_n("Pattern center [%f %f], match %f%%\n"), max_c + 5.5f, max_r + 5.5f, max_match / 1.32f);
  616. *pc = max_c;
  617. *pr = max_r;
  618. return max_match;
  619. }
  620. const uint16_t xyzcal_point_pattern_10[12] PROGMEM = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000};
  621. const uint16_t xyzcal_point_pattern_08[12] PROGMEM = {0x000, 0x000, 0x0f0, 0x1f8, 0x3fc, 0x3fc, 0x3fc, 0x3fc, 0x1f8, 0x0f0, 0x000, 0x000};
  622. bool xyzcal_searchZ(void) {
  623. //@size=118
  624. DBG(_n("xyzcal_searchZ x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
  625. int16_t x0 = _X;
  626. int16_t y0 = _Y;
  627. int16_t z = _Z;
  628. // int16_t min_z = -6000;
  629. // int16_t dz = 100;
  630. while (z > -2300) { //-6mm + 0.25mm
  631. uint16_t ad = 0;
  632. if (xyzcal_spiral8(x0, y0, z, 100, 900, 320, 1, &ad)) { //dz=100 radius=900 delay=400
  633. //@size=82
  634. DBG(_n(" ON-SIGNAL at x=%d y=%d z=%d ad=%d\n"), _X, _Y, _Z, ad);
  635. /// return to starting XY position
  636. /// magic constant, lowers min_z after searchZ to obtain more dense data in scan
  637. const pos_i16_t lower_z = 72;
  638. xyzcal_lineXYZ_to(x0, y0, _Z - lower_z, 200, 0);
  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. /// Scan should include normal data.
  796. /// If it's too extreme (00, FF) it could be caused by biased sensor.
  797. /// \return true if data looks normal
  798. bool check_scan(uint8_t *matrix32){
  799. /// magic constants that define normality
  800. const int16_t threshold_total = 900;
  801. const int threshold_extreme = 50;
  802. int16_t mins = 0;
  803. int16_t maxs = 0;
  804. for (int16_t i = 0; i < 32*32;++i){
  805. if (matrix32[i] == 0) {
  806. ++mins;
  807. } else if (matrix32[i] == 0xFF){
  808. ++maxs;
  809. }
  810. }
  811. const int16_t rest = 1024 - mins - maxs;
  812. if (mins + maxs > threshold_total
  813. && mins > threshold_extreme
  814. && maxs > threshold_extreme
  815. && mins > rest
  816. && maxs > rest)
  817. return false;
  818. return true;
  819. }
  820. /// scans area around the current head location and
  821. /// searches for the center of the calibration pin
  822. BedSkewOffsetDetectionResultType xyzcal_scan_and_process(){
  823. //@size=44
  824. // DBG(_n("sizeof(block_buffer)=%d\n"), sizeof(block_t)*BLOCK_BUFFER_SIZE);
  825. BedSkewOffsetDetectionResultType ret = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  826. int16_t x = _X;
  827. int16_t y = _Y;
  828. const int16_t z = _Z;
  829. uint8_t *matrix32 = (uint8_t *)block_buffer;
  830. uint16_t *pattern08 = (uint16_t *)(matrix32 + 32 * 32);
  831. uint16_t *pattern10 = (uint16_t *)(pattern08 + 12);
  832. for (uint8_t i = 0; i < 12; i++){
  833. pattern08[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern_08 + i));
  834. pattern10[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern_10 + i));
  835. }
  836. xyzcal_scan_pixels_32x32_Zhop(x, y, z, 2400, 200, matrix32);
  837. print_image(matrix32);
  838. if (!check_scan(matrix32))
  839. return BED_SKEW_OFFSET_DETECTION_POINT_SCAN_FAILED;
  840. /// SEARCH FOR BINARY CIRCLE
  841. uint8_t uc = 0;
  842. uint8_t ur = 0;
  843. /// max match = 132, 1/2 good = 66, 2/3 good = 88
  844. if (find_patterns(matrix32, pattern08, pattern10, uc, ur) >= 88){
  845. /// find precise circle
  846. /// move to the center of the pattern (+5.5)
  847. float xf = uc + 5.5f;
  848. float yf = ur + 5.5f;
  849. float radius = 4.5f; ///< default radius
  850. constexpr const uint8_t iterations = 20;
  851. dynamic_circle(matrix32, xf, yf, radius, iterations);
  852. if (fabs(xf - (uc + 5.5f)) > 3 || fabs(yf - (ur + 5.5f)) > 3 || fabs(radius - 5) > 3){
  853. //@size=88
  854. DBG(_n(" [%f %f][%f] mm divergence\n"), xf - (uc + 5.5f), yf - (ur + 5.5f), radius - 5);
  855. /// dynamic algorithm diverged, use original position instead
  856. xf = uc + 5.5f;
  857. yf = ur + 5.5f;
  858. }
  859. /// move to the center of area and convert to position
  860. xf = (float)x + (xf - 15.5f) * 64;
  861. yf = (float)y + (yf - 15.5f) * 64;
  862. //@size=114
  863. DBG(_n(" [%f %f] mm pattern center\n"), pos_2_mm(xf), pos_2_mm(yf));
  864. x = round_to_i16(xf);
  865. y = round_to_i16(yf);
  866. xyzcal_lineXYZ_to(x, y, z, 200, 0);
  867. ret = BED_SKEW_OFFSET_DETECTION_POINT_FOUND;
  868. }
  869. /// wipe buffer
  870. for (uint16_t i = 0; i < sizeof(block_t)*BLOCK_BUFFER_SIZE; i++)
  871. matrix32[i] = 0;
  872. return ret;
  873. }
  874. BedSkewOffsetDetectionResultType xyzcal_find_bed_induction_sensor_point_xy(void) {
  875. // 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]);
  876. BedSkewOffsetDetectionResultType ret = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  877. xyzcal_measure_enter();
  878. if (xyzcal_searchZ())
  879. ret = xyzcal_scan_and_process();
  880. xyzcal_measure_leave();
  881. return ret;
  882. }
  883. #endif //NEW_XYZCAL