|
@@ -29,6 +29,11 @@
|
|
|
#define _Z ((int16_t)count_position[Z_AXIS])
|
|
|
#define _E ((int16_t)count_position[E_AXIS])
|
|
|
|
|
|
+#define _X_ (count_position[X_AXIS])
|
|
|
+#define _Y_ (count_position[Y_AXIS])
|
|
|
+#define _Z_ (count_position[Z_AXIS])
|
|
|
+#define _E_ (count_position[E_AXIS])
|
|
|
+
|
|
|
#ifndef M_PI
|
|
|
const constexpr float M_PI = 3.1415926535897932384626433832795f;
|
|
|
#endif
|
|
@@ -40,6 +45,13 @@ const constexpr uint8_t Y_MINUS = 1;
|
|
|
const constexpr uint8_t Z_PLUS = 0;
|
|
|
const constexpr uint8_t Z_MINUS = 1;
|
|
|
|
|
|
+const constexpr uint8_t X_PLUS_MASK = 0;
|
|
|
+const constexpr uint8_t X_MINUS_MASK = X_AXIS_MASK;
|
|
|
+const constexpr uint8_t Y_PLUS_MASK = 0;
|
|
|
+const constexpr uint8_t Y_MINUS_MASK = Y_AXIS_MASK;
|
|
|
+const constexpr uint8_t Z_PLUS_MASK = 0;
|
|
|
+const constexpr uint8_t Z_MINUS_MASK = Z_AXIS_MASK;
|
|
|
+
|
|
|
/// Max. jerk in PrusaSlicer, 10000 = 1 mm/s
|
|
|
const constexpr uint16_t MAX_DELAY = 10000;
|
|
|
const constexpr float MIN_SPEED = 0.01f / (MAX_DELAY * 0.000001f);
|
|
@@ -226,6 +238,9 @@ uint16_t xyzcal_calc_delay(uint16_t, uint16_t)
|
|
|
#endif //SM4_ACCEL_TEST
|
|
|
|
|
|
/// Moves printer to absolute position [x,y,z] defined in integer position system
|
|
|
+/// check_pinda == 0: ordinary move
|
|
|
+/// check_pinda == 1: stop when PINDA triggered
|
|
|
+/// check_pinda == -1: stop when PINDA untriggered
|
|
|
bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda)
|
|
|
{
|
|
|
// DBG(_n("xyzcal_lineXYZ_to x=%d y=%d z=%d check=%d\n"), x, y, z, check_pinda);
|
|
@@ -370,9 +385,48 @@ int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t d
|
|
|
}
|
|
|
#endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS
|
|
|
|
|
|
+void print_hysteresis(int16_t min_z, int16_t max_z, int16_t step){
|
|
|
+ int16_t delay_us = 600;
|
|
|
+ int16_t trigger = 0;
|
|
|
+ int16_t untrigger = 0;
|
|
|
+ DBG(_n("Hysteresis\n"));
|
|
|
+
|
|
|
+ xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 0);
|
|
|
+
|
|
|
+ for (int16_t z = min_z; z <= max_z; z += step){
|
|
|
+ xyzcal_lineXYZ_to(_X, _Y, z, delay_us, -1);
|
|
|
+ untrigger = _Z;
|
|
|
+ xyzcal_lineXYZ_to(_X, _Y, z, delay_us, 0);
|
|
|
+ xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
|
|
|
+ trigger = _Z;
|
|
|
+ //xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 0);
|
|
|
+
|
|
|
+ DBG(_n("min, trigger, untrigger, max: [%d %d %d %d]\n"), _Z, trigger, untrigger, z);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void update_position_1_step(uint8_t axis, uint8_t dir){
|
|
|
+ if (axis & X_AXIS_MASK)
|
|
|
+ _X_ += dir & X_AXIS_MASK ? -1 : 1;
|
|
|
+ if (axis & Y_AXIS_MASK)
|
|
|
+ _Y_ += dir & Y_AXIS_MASK ? -1 : 1;
|
|
|
+ if (axis & Z_AXIS_MASK)
|
|
|
+ _Z_ += dir & Z_AXIS_MASK ? -1 : 1;
|
|
|
+}
|
|
|
+
|
|
|
+void set_axes_dir(uint8_t axes, uint8_t dir){
|
|
|
+ if (axes & X_AXIS_MASK)
|
|
|
+ sm4_set_dir(X_AXIS, dir & X_AXIS_MASK);
|
|
|
+ if (axes & Y_AXIS_MASK)
|
|
|
+ sm4_set_dir(Y_AXIS, dir & Y_AXIS_MASK);
|
|
|
+ if (axes & Z_AXIS_MASK)
|
|
|
+ sm4_set_dir(Z_AXIS, dir & Z_AXIS_MASK);
|
|
|
+}
|
|
|
+
|
|
|
/// Accelerate up to max.speed (defined by @min_delay_us)
|
|
|
-void accelerate(uint8_t axis, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){
|
|
|
- sm4_do_step(axis);
|
|
|
+/// does not update global positions
|
|
|
+void accelerate_1_step(uint8_t axes, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us){
|
|
|
+ sm4_do_step(axes);
|
|
|
|
|
|
/// keep max speed (avoid extra computation)
|
|
|
if (acc > 0 && delay_us == min_delay_us){
|
|
@@ -406,136 +460,182 @@ void accelerate(uint8_t axis, int16_t acc, uint16_t &delay_us, uint16_t min_dela
|
|
|
delay_us = t1;
|
|
|
}
|
|
|
|
|
|
-void go_and_stop(uint8_t axis, int16_t dec, uint16_t &delay_us, uint16_t &steps){
|
|
|
+/// Goes defined number of steps while accelerating
|
|
|
+/// updates global positions
|
|
|
+void accelerate(uint8_t axes, uint8_t dir, int16_t acc, uint16_t &delay_us, uint16_t min_delay_us, uint16_t steps){
|
|
|
+ set_axes_dir(axes, dir);
|
|
|
+ while (steps--){
|
|
|
+ accelerate_1_step(axes, acc, delay_us, min_delay_us);
|
|
|
+ update_position_1_step(axes, dir);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+/// keeps speed and then it decelerates to a complete stop (if possible)
|
|
|
+/// it goes defined number of steps
|
|
|
+/// returns after each step
|
|
|
+/// \returns true if step was done
|
|
|
+/// does not update global positions
|
|
|
+bool go_and_stop_1_step(uint8_t axes, int16_t dec, uint16_t &delay_us, uint16_t &steps){
|
|
|
if (steps <= 0 || dec <= 0)
|
|
|
- return;
|
|
|
+ return false;
|
|
|
|
|
|
/// deceleration distance in steps, s = 1/2 v^2 / a
|
|
|
uint16_t s = round_to_u16(100 * 0.5f * SQR(0.01f) / (SQR((float)delay_us) * dec));
|
|
|
if (steps > s){
|
|
|
/// go steady
|
|
|
- sm4_do_step(axis);
|
|
|
+ sm4_do_step(axes);
|
|
|
delayMicroseconds(delay_us);
|
|
|
} else {
|
|
|
/// decelerate
|
|
|
- accelerate(axis, -dec, delay_us, delay_us);
|
|
|
+ accelerate_1_step(axes, -dec, delay_us, delay_us);
|
|
|
}
|
|
|
--steps;
|
|
|
+ return true;
|
|
|
}
|
|
|
|
|
|
-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){
|
|
|
+/// \param dir sets direction of movement
|
|
|
+/// updates global positions
|
|
|
+void go_and_stop(uint8_t axes, uint8_t dir, int16_t dec, uint16_t &delay_us, uint16_t steps){
|
|
|
+ set_axes_dir(axes, dir);
|
|
|
+ while (go_and_stop_1_step(axes, dec, delay_us, steps)){
|
|
|
+ update_position_1_step(axes, dir);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+/// goes all the way to stop
|
|
|
+/// \returns steps done
|
|
|
+/// updates global positions
|
|
|
+void stop_smoothly(uint8_t axes, uint8_t dir, int16_t dec, uint16_t &delay_us){
|
|
|
+ if (dec <= 0)
|
|
|
+ return;
|
|
|
+ set_axes_dir(axes, dir);
|
|
|
+ while (delay_us < MAX_DELAY){
|
|
|
+ accelerate_1_step(axes, -dec, delay_us, delay_us);
|
|
|
+ update_position_1_step(axes, dir);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void go_start_stop(uint8_t axes, uint8_t dir, int16_t acc, uint16_t min_delay_us, uint16_t steps){
|
|
|
+ if (steps == 0)
|
|
|
+ return;
|
|
|
+ uint16_t current_delay_us = MAX_DELAY;
|
|
|
+ const uint16_t half = steps / 2;
|
|
|
+ accelerate(axes, dir, acc, current_delay_us, min_delay_us, half);
|
|
|
+ go_and_stop(axes, dir, -acc, current_delay_us, steps - half);
|
|
|
+}
|
|
|
+
|
|
|
+/// moves X, Y, Z one after each other
|
|
|
+/// starts and ends at 0 speed
|
|
|
+void go_manhattan(int16_t x, int16_t y, int16_t z, int16_t acc, uint16_t min_delay_us){
|
|
|
+ int32_t length;
|
|
|
+
|
|
|
+ // DBG(_n("x %d -> %d, "), x, _X);
|
|
|
+ length = x - _X;
|
|
|
+ go_start_stop(X_AXIS_MASK, length < 0 ? X_MINUS_MASK : X_PLUS_MASK, acc, min_delay_us, ABS(length));
|
|
|
+
|
|
|
+ // DBG(_n("y %d -> %d, "), y, _Y);
|
|
|
+ length = y - _Y;
|
|
|
+ go_start_stop(Y_AXIS_MASK, length < 0 ? Y_MINUS_MASK : Y_PLUS_MASK, acc, min_delay_us, ABS(length));
|
|
|
+
|
|
|
+ // DBG(_n("z %d -> %d\n"), z, _Z);
|
|
|
+ length = z - _Z;
|
|
|
+ go_start_stop(Z_AXIS_MASK, length < 0 ? Z_MINUS_MASK : Z_PLUS_MASK, acc, min_delay_us, ABS(length));
|
|
|
+ // DBG(_n("\n"));
|
|
|
+}
|
|
|
+
|
|
|
+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){
|
|
|
if (!pixels)
|
|
|
return;
|
|
|
- int16_t z = _Z;
|
|
|
int16_t z_trig;
|
|
|
uint16_t line_buffer[32];
|
|
|
uint16_t current_delay_us = MAX_DELAY; ///< defines current speed
|
|
|
- xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0);
|
|
|
int16_t start_z;
|
|
|
uint16_t steps_to_go;
|
|
|
|
|
|
- for (uint8_t r = 0; r < 32; r++){ ///< Y axis
|
|
|
- xyzcal_lineXYZ_to(_X, cy - 1024 + r * 64, z, delay_us, 0);
|
|
|
- for (int8_t d = 0; d < 2; ++d){ ///< direction
|
|
|
- xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, min_z, delay_us, 0);
|
|
|
+ DBG(_n("Scan countdown: "));
|
|
|
|
|
|
- z = _Z;
|
|
|
+ for (uint8_t r = 0; r < 32; r++){ ///< Y axis
|
|
|
+ for (uint8_t d = 0; d < 2; ++d){
|
|
|
+ go_manhattan((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, Z_ACCEL, Z_MIN_DELAY);
|
|
|
+ xyzcal_lineXYZ_to((d & 1) ? (cx + 992) : (cx - 992), cy - 992 + r * 64, _Z, delay_us, 0);
|
|
|
sm4_set_dir(X_AXIS, d);
|
|
|
+ DBG(_n("%d\n"), 64 - (r * 2 + d)); ///< to keep OctoPrint connection alive
|
|
|
+
|
|
|
for (uint8_t c = 0; c < 32; c++){ ///< X axis
|
|
|
+ /// move to the next point and move Z up diagonally (if needed)
|
|
|
+ current_delay_us = MAX_DELAY;
|
|
|
+ const int16_t end_x = ((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx;
|
|
|
+ const int16_t length_x = ABS(end_x - _X);
|
|
|
+ const int16_t half_x = length_x / 2;
|
|
|
+ /// don't go up if PINDA not triggered (optimization)
|
|
|
+ const bool up = _PINDA;
|
|
|
+ const uint8_t axes = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK;
|
|
|
+ const uint8_t dir = Z_PLUS_MASK | (d & 1 ? X_MINUS_MASK : X_PLUS_MASK);
|
|
|
|
|
|
+ accelerate(axes, dir, Z_ACCEL, current_delay_us, Z_MIN_DELAY, half_x);
|
|
|
+ go_and_stop(axes, dir, Z_ACCEL, current_delay_us, length_x - half_x);
|
|
|
+
|
|
|
+
|
|
|
z_trig = min_z;
|
|
|
|
|
|
/// move up to un-trigger (surpress hysteresis)
|
|
|
sm4_set_dir(Z_AXIS, Z_PLUS);
|
|
|
/// speed up from stop, go half the way
|
|
|
current_delay_us = MAX_DELAY;
|
|
|
- for (start_z = z; z < (max_z + start_z) / 2; ++z){
|
|
|
+ for (start_z = _Z; _Z < (max_z + start_z) / 2; ++_Z_){
|
|
|
if (!_PINDA){
|
|
|
break;
|
|
|
}
|
|
|
- accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY);
|
|
|
+ accelerate_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY);
|
|
|
}
|
|
|
|
|
|
- if(_PINDA){
|
|
|
- uint16_t steps_to_go = MAX(0, max_z - z);
|
|
|
- while (_PINDA && z < max_z){
|
|
|
- go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go);
|
|
|
- ++z;
|
|
|
+ if (_PINDA){
|
|
|
+ steps_to_go = MAX(0, max_z - _Z);
|
|
|
+ while (_PINDA && _Z < max_z){
|
|
|
+ go_and_stop_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go);
|
|
|
+ ++_Z_;
|
|
|
}
|
|
|
}
|
|
|
- /// slow down to stop
|
|
|
- while (current_delay_us < MAX_DELAY){
|
|
|
- accelerate(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY);
|
|
|
- ++z;
|
|
|
- }
|
|
|
+ stop_smoothly(Z_AXIS_MASK, Z_PLUS_MASK, Z_ACCEL, current_delay_us);
|
|
|
|
|
|
/// move down to trigger
|
|
|
sm4_set_dir(Z_AXIS, Z_MINUS);
|
|
|
/// speed up
|
|
|
current_delay_us = MAX_DELAY;
|
|
|
- for (start_z = z; z > (min_z + start_z) / 2; --z){
|
|
|
+ for (start_z = _Z; _Z > (min_z + start_z) / 2; --_Z_){
|
|
|
if (_PINDA){
|
|
|
- z_trig = z;
|
|
|
+ z_trig = _Z;
|
|
|
break;
|
|
|
}
|
|
|
- accelerate(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY);
|
|
|
+ accelerate_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, Z_MIN_DELAY);
|
|
|
}
|
|
|
/// slow down
|
|
|
- if(!_PINDA){
|
|
|
- steps_to_go = MAX(0, z - min_z);
|
|
|
- while (!_PINDA && z > min_z){
|
|
|
- go_and_stop(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go);
|
|
|
- --z;
|
|
|
+ if (!_PINDA){
|
|
|
+ steps_to_go = MAX(0, _Z - min_z);
|
|
|
+ while (!_PINDA && _Z > min_z){
|
|
|
+ go_and_stop_1_step(Z_AXIS_MASK, Z_ACCEL, current_delay_us, steps_to_go);
|
|
|
+ --_Z_;
|
|
|
}
|
|
|
- z_trig = z;
|
|
|
+ z_trig = _Z;
|
|
|
}
|
|
|
- /// slow down to stop
|
|
|
- while (z > min_z && current_delay_us < MAX_DELAY){
|
|
|
- accelerate(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY);
|
|
|
- --z;
|
|
|
+ /// slow down to stop but not lower than min_z
|
|
|
+ while (_Z > min_z && current_delay_us < MAX_DELAY){
|
|
|
+ accelerate_1_step(Z_AXIS_MASK, -Z_ACCEL, current_delay_us, Z_MIN_DELAY);
|
|
|
+ --_Z_;
|
|
|
}
|
|
|
|
|
|
- count_position[2] = z;
|
|
|
if (d == 0){
|
|
|
line_buffer[c] = (uint16_t)(z_trig - min_z);
|
|
|
} else {
|
|
|
- /// data reversed in X
|
|
|
- // DBG(_n("%04x"), (line_buffer[31 - c] + (z - min_z)) / 2);
|
|
|
- /// save average of both directions
|
|
|
+ /// !!! data reversed in X
|
|
|
+ // DBG(_n("%04x"), ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2);
|
|
|
+ /// save average of both directions (filters effect of hysteresis)
|
|
|
pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z_trig - min_z)) / 2);
|
|
|
}
|
|
|
-
|
|
|
- /// move to the next point and move Z up diagonally (if needed)
|
|
|
- current_delay_us = MAX_DELAY;
|
|
|
- // const int8_t dir = (d & 1) ? -1 : 1;
|
|
|
- const int16_t end_x = ((d & 1) ? 1 : -1) * (64 * (16 - c) - 32) + cx;
|
|
|
- const int16_t length_x = ABS(end_x - _X);
|
|
|
- const int16_t half_x = length_x / 2;
|
|
|
- int16_t x = 0;
|
|
|
- /// don't go up if PINDA not triggered
|
|
|
- const bool up = _PINDA;
|
|
|
- int8_t axis = up ? X_AXIS_MASK | Z_AXIS_MASK : X_AXIS_MASK;
|
|
|
-
|
|
|
- sm4_set_dir(Z_AXIS, Z_PLUS);
|
|
|
- /// speed up
|
|
|
- for (x = 0; x <= half_x; ++x){
|
|
|
- accelerate(axis, Z_ACCEL, current_delay_us, Z_MIN_DELAY);
|
|
|
- if (up)
|
|
|
- ++z;
|
|
|
- }
|
|
|
- /// slow down
|
|
|
- steps_to_go = length_x - x;
|
|
|
- for (; x < length_x; ++x){
|
|
|
- go_and_stop(axis, Z_ACCEL, current_delay_us, steps_to_go);
|
|
|
- if (up)
|
|
|
- ++z;
|
|
|
- }
|
|
|
- count_position[0] = end_x;
|
|
|
- count_position[2] = z;
|
|
|
}
|
|
|
}
|
|
|
- // DBG(_n("\n\n"));
|
|
|
}
|
|
|
+ DBG(_n("\n"));
|
|
|
}
|
|
|
|
|
|
/// Returns rate of match
|
|
@@ -608,7 +708,8 @@ const int16_t xyzcal_point_xcoords[4] PROGMEM = {1200, 22000, 22000, 1200};
|
|
|
const int16_t xyzcal_point_ycoords[4] PROGMEM = {700, 700, 19800, 19800};
|
|
|
#endif //((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
|
|
|
|
|
|
-const uint16_t xyzcal_point_pattern[12] PROGMEM = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000};
|
|
|
+const uint16_t xyzcal_point_pattern_10[12] PROGMEM = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000};
|
|
|
+const uint16_t xyzcal_point_pattern_08[12] PROGMEM = {0x000, 0x000, 0x0f0, 0x1f8, 0x3fc, 0x3fc, 0x3fc, 0x3fc, 0x1f8, 0x0f0, 0x000, 0x000};
|
|
|
|
|
|
bool xyzcal_searchZ(void)
|
|
|
{
|
|
@@ -699,6 +800,30 @@ float highest(float *points, const uint8_t num_points){
|
|
|
return max;
|
|
|
}
|
|
|
|
|
|
+/// slow bubble sort but short
|
|
|
+void sort(float *points, const uint8_t num_points){
|
|
|
+ /// one direction bubble sort
|
|
|
+ for (uint8_t i = 0; i < num_points; ++i){
|
|
|
+ for (uint8_t j = 0; j < num_points - i - 1; ++j){
|
|
|
+ if (points[j] > points[j + 1])
|
|
|
+ SWAP(points[j], points[j + 1]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // DBG(_n("Sorted: "));
|
|
|
+ // for (uint8_t i = 0; i < num_points; ++i)
|
|
|
+ // DBG(_n("%f "), points[i]);
|
|
|
+ // DBG(_n("\n"));
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+/// sort array and returns median value
|
|
|
+/// don't send empty array or nullptr
|
|
|
+float median(float *points, const uint8_t num_points){
|
|
|
+ sort(points, num_points);
|
|
|
+ return points[num_points / 2];
|
|
|
+}
|
|
|
+
|
|
|
/// Searches for circle iteratively
|
|
|
/// Uses points on the perimeter. If point is high it pushes circle out of the center (shift or change of radius),
|
|
|
/// otherwise to the center.
|
|
@@ -706,55 +831,37 @@ float highest(float *points, const uint8_t num_points){
|
|
|
void dynamic_circle(uint8_t *matrix_32x32, float &x, float &y, float &r, uint8_t iterations){
|
|
|
/// circle of 10.5 diameter has 33 in circumference, don't go much above
|
|
|
const constexpr uint8_t num_points = 33;
|
|
|
- float points[num_points];
|
|
|
float pi_2_div_num_points = 2 * M_PI / num_points;
|
|
|
const constexpr uint8_t target_z = 32; ///< target z height of the circle
|
|
|
- float norm;
|
|
|
float angle;
|
|
|
- float max_val = 0.5f;
|
|
|
- const uint8_t blocks = 7;
|
|
|
+ float max_change = 0.5f; ///< avoids too fast changes (avoid oscillation)
|
|
|
+ const uint8_t blocks = num_points;
|
|
|
float shifts_x[blocks];
|
|
|
float shifts_y[blocks];
|
|
|
float shifts_r[blocks];
|
|
|
|
|
|
+ DBG(_n(" [%f, %f][%f] start circle\n"), x, y, r);
|
|
|
+
|
|
|
for (int8_t i = iterations; i > 0; --i){
|
|
|
|
|
|
- // DBG(_n(" [%f, %f][%f] circle\n"), x, y, r);
|
|
|
+ DBG(_n(" [%f, %f][%f] circle\n"), x, y, r);
|
|
|
|
|
|
/// read points on the circle
|
|
|
for (uint8_t p = 0; p < num_points; ++p){
|
|
|
angle = p * pi_2_div_num_points;
|
|
|
- points[p] = get_value(matrix_32x32, r * cos(angle) + x, r * sin(angle) + y) - target_z;
|
|
|
- // DBG(_n("%f "), points[p]);
|
|
|
- }
|
|
|
- // DBG(_n(" points\n"));
|
|
|
-
|
|
|
- /// sum blocks
|
|
|
- for (uint8_t j = 0; j < blocks; ++j){
|
|
|
- shifts_x[j] = shifts_y[j] = shifts_r[j] = 0;
|
|
|
- /// first part
|
|
|
- for (uint8_t p = 0; p < num_points * 3 / 4; ++p){
|
|
|
- uint8_t idx = (p + j * num_points / blocks) % num_points;
|
|
|
-
|
|
|
- angle = idx * pi_2_div_num_points;
|
|
|
- shifts_x[j] += cos(angle) * points[idx];
|
|
|
- shifts_y[j] += sin(angle) * points[idx];
|
|
|
- shifts_r[j] += points[idx];
|
|
|
- }
|
|
|
- }
|
|
|
+ const float height = get_value(matrix_32x32, r * cos(angle) + x, r * sin(angle) + y) - target_z;
|
|
|
+ // DBG(_n("%f "), point);
|
|
|
|
|
|
- /// remove extreme values (slow but simple)
|
|
|
- for (uint8_t j = 0; j < blocks / 2; ++j){
|
|
|
- remove_highest(shifts_x, blocks);
|
|
|
- remove_highest(shifts_y, blocks);
|
|
|
- remove_highest(shifts_r, blocks);
|
|
|
+ shifts_x[p] = cos(angle) * height;
|
|
|
+ shifts_y[p] = sin(angle) * height;
|
|
|
+ shifts_r[p] = height;
|
|
|
}
|
|
|
+ // DBG(_n(" points\n"));
|
|
|
|
|
|
- /// median is the highest now
|
|
|
- norm = 1.f / (32.f * (num_points * 3 / 4));
|
|
|
- x += CLAMP(highest(shifts_x, blocks) * norm, -max_val, max_val);
|
|
|
- y += CLAMP(highest(shifts_y, blocks) * norm, -max_val, max_val);
|
|
|
- r += CLAMP(highest(shifts_r, blocks) * norm, -max_val, max_val);
|
|
|
+ const float norm = 1.f / 32.f;
|
|
|
+ x += CLAMP(median(shifts_x, blocks) * norm, -max_change, max_change);
|
|
|
+ y += CLAMP(median(shifts_y, blocks) * norm, -max_change, max_change);
|
|
|
+ r += CLAMP(median(shifts_r, blocks) * norm * .5f, -max_change, max_change);
|
|
|
|
|
|
r = MAX(2, r);
|
|
|
|
|
@@ -774,6 +881,30 @@ void print_image(uint8_t *matrix_32x32){
|
|
|
DBG(_n("\n"));
|
|
|
}
|
|
|
|
|
|
+/// Takes two patterns and searches them in matrix32
|
|
|
+/// \returns best match
|
|
|
+uint8_t find_patterns(uint8_t *matrix32, uint16_t *pattern08, uint16_t *pattern10, uint8_t &col, uint8_t &row){
|
|
|
+ uint8_t c08 = 0;
|
|
|
+ uint8_t r08 = 0;
|
|
|
+ uint8_t match08 = 0;
|
|
|
+ uint8_t c10 = 0;
|
|
|
+ uint8_t r10 = 0;
|
|
|
+ uint8_t match10 = 0;
|
|
|
+
|
|
|
+ match08 = xyzcal_find_pattern_12x12_in_32x32(matrix32, pattern08, &c08, &r08);
|
|
|
+ match10 = xyzcal_find_pattern_12x12_in_32x32(matrix32, pattern10, &c10, &r10);
|
|
|
+
|
|
|
+ if (match08 > match10){
|
|
|
+ col = c08;
|
|
|
+ row = r08;
|
|
|
+ return match08;
|
|
|
+ }
|
|
|
+
|
|
|
+ col = c10;
|
|
|
+ row = r10;
|
|
|
+ return match10;
|
|
|
+}
|
|
|
+
|
|
|
/// scans area around the current head location and
|
|
|
/// searches for the center of the calibration pin
|
|
|
bool xyzcal_scan_and_process(void){
|
|
@@ -784,21 +915,24 @@ bool xyzcal_scan_and_process(void){
|
|
|
int16_t z = _Z;
|
|
|
|
|
|
uint8_t *matrix32 = (uint8_t *)block_buffer;
|
|
|
- uint16_t *pattern = (uint16_t *)(matrix32 + 32 * 32);
|
|
|
+ uint16_t *pattern08 = (uint16_t *)(matrix32 + 32 * 32);
|
|
|
+ uint16_t *pattern10 = (uint16_t *)(pattern08 + 12);
|
|
|
|
|
|
xyzcal_scan_pixels_32x32_Zhop(x, y, z - 72, 2400, 200, matrix32);
|
|
|
print_image(matrix32);
|
|
|
|
|
|
for (uint8_t i = 0; i < 12; i++){
|
|
|
- pattern[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern + i));
|
|
|
-// DBG(_n(" pattern[%d]=%d\n"), i, pattern[i]);
|
|
|
+ pattern08[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern_08 + i));
|
|
|
+ pattern10[i] = pgm_read_word((uint16_t*)(xyzcal_point_pattern_10 + i));
|
|
|
}
|
|
|
|
|
|
/// SEARCH FOR BINARY CIRCLE
|
|
|
uint8_t uc = 0;
|
|
|
uint8_t ur = 0;
|
|
|
+
|
|
|
+
|
|
|
/// max match = 132, 1/2 good = 66, 2/3 good = 88
|
|
|
- if (xyzcal_find_pattern_12x12_in_32x32(matrix32, pattern, &uc, &ur) >= 88){
|
|
|
+ if (find_patterns(matrix32, pattern08, pattern10, uc, ur) >= 88){
|
|
|
/// find precise circle
|
|
|
/// move to the center of the pattern (+5.5)
|
|
|
float xf = uc + 5.5f;
|
|
@@ -846,8 +980,7 @@ bool xyzcal_find_bed_induction_sensor_point_xy(void){
|
|
|
xyzcal_lineXYZ_to(x, y, z, 200, 0);
|
|
|
|
|
|
if (xyzcal_searchZ()){
|
|
|
- int16_t z = _Z;
|
|
|
- xyzcal_lineXYZ_to(x, y, z, 200, 0);
|
|
|
+ xyzcal_lineXYZ_to(x, y, _Z, 200, 0);
|
|
|
ret = xyzcal_scan_and_process();
|
|
|
}
|
|
|
xyzcal_meassure_leave();
|