From ccf26e4e7a7c3ca1d476f6d5f40c8da523fe0fe2 Mon Sep 17 00:00:00 2001
From: espr14 <espr14@gmail.com>
Date: Fri, 18 Dec 2020 20:04:00 +0100
Subject: [PATCH] Basic acceleration

---
 Firmware/xyzcal.cpp | 54 ++++++++++++++++++++++++++++++++++++---------
 1 file changed, 44 insertions(+), 10 deletions(-)

diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp
index 2130b223..3a794f9b 100644
--- a/Firmware/xyzcal.cpp
+++ b/Firmware/xyzcal.cpp
@@ -360,20 +360,36 @@ int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t d
 }
 #endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS
 
+uint8_t slow_down_z(uint16_t delay_us){
+	sm4_do_step(Z_AXIS_MASK);
+	delayMicroseconds(delay_us / 3 * 4);
+	sm4_do_step(Z_AXIS_MASK);
+	delayMicroseconds(delay_us * 2);
+	sm4_do_step(Z_AXIS_MASK);
+	delayMicroseconds(delay_us * 4);
+	return 3;
+}
+
+uint8_t speed_up_z(int16_t &z, uint8_t direction, uint16_t delay_us){
+	sm4_do_step(Z_AXIS_MASK);
+	delayMicroseconds(delay_us * 4);
+	sm4_do_step(Z_AXIS_MASK);
+	delayMicroseconds(delay_us * 2);
+	sm4_do_step(Z_AXIS_MASK);
+	delayMicroseconds(delay_us / 3 * 4);
+	return 3;
+}
+
 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];
-	xyzcal_lineXYZ_to(cx, cy, z, delay_us, 0);
+	xyzcal_lineXYZ_to(cx - 1024, cy - 1024, min_z, delay_us, 0);
 	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, z, 2 * delay_us, 0);
-			// xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
-			// xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1);
-
+		for (int8_t d = 0; d < 2; ++d){ ///< direction			
 			xyzcal_lineXYZ_to((d & 1) ? (cx + 1024) : (cx - 1024), _Y, min_z, delay_us, 0);
 
 			z = _Z;
@@ -382,29 +398,47 @@ void xyzcal_scan_pixels_32x32_Zhop(int16_t cx, int16_t cy, int16_t min_z, int16_
 				
 				/// move up to un-trigger (surpress hysteresis)
 				sm4_set_dir(Z_AXIS, Z_PLUS);
+				z += speed_up_z(delay_us);
 				while (z < max_z && _PINDA){
 					sm4_do_step(Z_AXIS_MASK);
 					delayMicroseconds(delay_us);
 					z++;
 				}
 				int16_t last_top_z = z;
+				z += slow_down_z(delay_us);
 
 				/// move down to trigger
 				sm4_set_dir(Z_AXIS, Z_MINUS);
+
+				/// speed up
+				do (){
+					if (z <= min_z || _PINDA) break;
+					sm4_do_step(Z_AXIS_MASK);
+					delayMicroseconds(delay_us * 4);
+					if (z <= min_z || _PINDA) break;
+					sm4_do_step(Z_AXIS_MASK);
+					delayMicroseconds(delay_us * 2);
+					if (z <= min_z || _PINDA) break;
+					sm4_do_step(Z_AXIS_MASK);
+					delayMicroseconds(delay_us / 3 * 4);
+				} while (0);
+
 				while (z > min_z && !_PINDA){
 					sm4_do_step(Z_AXIS_MASK);
 					delayMicroseconds(delay_us);
 					z--;
 				}
-				
+				z_trig = z;
+				z -= slow_down_z(delay_us);
+
 				count_position[2] = z;
 				if (d == 0){
-					line_buffer[c] = (uint16_t)(z - min_z);
+					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
-					pixels[(uint16_t)r * 32 + (31 - c)] = (uint8_t)MIN((uint32_t)255, ((uint32_t)line_buffer[31 - c] + (z - min_z)) / 2);
+					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