From 57499dc6a0f30f94b0e3230e3800260023772fa7 Mon Sep 17 00:00:00 2001
From: Robert Pelnar <robert.pelnar@seznam.cz>
Date: Thu, 15 Feb 2018 15:40:49 +0100
Subject: [PATCH] M350 for E axis (8, 16, 32, 64, 128 microsteps)

---
 Firmware/Dcodes.cpp      | 23 ++++++++++-
 Firmware/Marlin_main.cpp | 26 +++++++++++++
 Firmware/planner.h       |  2 +
 Firmware/stepper.cpp     |  4 ++
 Firmware/tmc2130.cpp     | 83 ++++++++++++++++++++++------------------
 Firmware/tmc2130.h       |  5 ++-
 6 files changed, 103 insertions(+), 40 deletions(-)

diff --git a/Firmware/Dcodes.cpp b/Firmware/Dcodes.cpp
index 3ac40909..eb0d09fa 100644
--- a/Firmware/Dcodes.cpp
+++ b/Firmware/Dcodes.cpp
@@ -446,6 +446,11 @@ void dcode_10()
 	calibration_status_store(CALIBRATION_STATUS_LIVE_ADJUST); 
 }
 
+void dcode_12()
+{//Time
+	LOG("D12 - Time\n");
+}
+
 #include "tmc2130.h"
 #include "Marlin.h"
 #include "planner.h"
@@ -495,7 +500,8 @@ void dcode_2130()
 		}
 		else if (strchr_pointer[1+5] == '?')
 		{
-			if (strcmp(strchr_pointer + 7, "step") == 0) printf_P(PSTR("%c step=%d\n"), ch_axis, tmc2130_rd_MSCNT(axis) >> tmc2130_mres[axis]);
+			if (strcmp(strchr_pointer + 7, "mres") == 0) printf_P(PSTR("%c mres=%d\n"), ch_axis, tmc2130_mres[axis]);
+			else if (strcmp(strchr_pointer + 7, "step") == 0) printf_P(PSTR("%c step=%d\n"), ch_axis, tmc2130_rd_MSCNT(axis) >> tmc2130_mres[axis]);
 			else if (strcmp(strchr_pointer + 7, "mscnt") == 0) printf_P(PSTR("%c MSCNT=%d\n"), ch_axis, tmc2130_rd_MSCNT(axis));
 			else if (strcmp(strchr_pointer + 7, "mscuract") == 0)
 			{
@@ -519,6 +525,21 @@ void dcode_2130()
 				uint16_t res = tmc2130_get_res(axis);
 				tmc2130_goto_step(axis, step & (4*res - 1), 2, 1000, res);
 			}
+			else if (strncmp(strchr_pointer + 7, "mres", 4) == 0)
+			{
+				uint8_t mres = strchr_pointer[11] - '0';
+				if ((mres >= 0) && (mres <= 8))
+				{
+					st_synchronize();
+					uint16_t res = tmc2130_get_res(axis);
+					uint16_t res_new = tmc2130_mres2usteps(mres);
+					tmc2130_set_res(axis, res_new);
+					if (res_new > res)
+						axis_steps_per_unit[axis] *= (res_new / res);
+					else
+						axis_steps_per_unit[axis] /= (res / res_new);
+				}
+			}
 			else if (strncmp(strchr_pointer + 7, "wave", 4) == 0)
 			{
 				uint8_t fac200 = atoi(strchr_pointer + 11) & 0xfe;
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 3f2322ee..e16e8869 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -6195,12 +6195,38 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 
     case 350: // M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
     {
+	#ifdef TMC2130
+		if(code_seen('E'))
+		{
+			uint16_t res_new = code_value();
+			if ((res_new == 8) || (res_new == 16) || (res_new == 32) || (res_new == 64) || (res_new == 128))
+			{
+				st_synchronize();
+				uint8_t axis = E_AXIS;
+				uint16_t res = tmc2130_get_res(axis);
+				tmc2130_set_res(axis, res_new);
+				if (res_new > res)
+				{
+					uint16_t fac = (res_new / res);
+					axis_steps_per_unit[axis] *= fac;
+					position[E_AXIS] *= fac;
+				}
+				else
+				{
+					uint16_t fac = (res / res_new);
+					axis_steps_per_unit[axis] /= fac;
+					position[E_AXIS] /= fac;
+				}
+			}
+		}
+	#else //TMC2130
       #if defined(X_MS1_PIN) && X_MS1_PIN > -1
         if(code_seen('S')) for(int i=0;i<=4;i++) microstep_mode(i,code_value());
         for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_mode(i,(uint8_t)code_value());
         if(code_seen('B')) microstep_mode(4,code_value());
         microstep_readings();
       #endif
+	#endif //TMC2130
     }
     break;
     case 351: // M351 Toggle MS1 MS2 pins directly, S# determines MS1 or MS2, X# sets the pin high/low.
diff --git a/Firmware/planner.h b/Firmware/planner.h
index 3b9e65a1..42307d22 100644
--- a/Firmware/planner.h
+++ b/Firmware/planner.h
@@ -148,6 +148,8 @@ extern float max_jerk[NUM_AXIS];
 extern float mintravelfeedrate;
 extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];
 
+extern long position[NUM_AXIS];
+
 #ifdef AUTOTEMP
     extern bool autotemp_enabled;
     extern float autotemp_max;
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index 91f9b015..a70e342e 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -1433,6 +1433,9 @@ void microstep_init()
   #endif
 }
 
+
+#ifndef TMC2130
+
 void microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2)
 {
   if(ms1 > -1) switch(driver)
@@ -1490,3 +1493,4 @@ void microstep_readings()
       SERIAL_PROTOCOLLN( digitalRead(E1_MS2_PIN));
       #endif
 }
+#endif //TMC2130
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index 0b2c3331..8c836f04 100644
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -565,11 +565,11 @@ void tmc2130_wr_THIGH(uint8_t axis, uint32_t val32)
 	tmc2130_wr(axis, TMC2130_REG_THIGH, val32);
 }
 
-uint8_t tmc2130_usteps2mres(uint16_t usteps)
-{
-	uint8_t mres = 8; while (mres && (usteps >>= 1)) mres--;
-	return mres;
-}
+uint8_t tmc2130_usteps2mres(uint16_t usteps)
+{
+	uint8_t mres = 8; while (mres && (usteps >>= 1)) mres--;
+	return mres;
+}
 
 uint8_t tmc2130_wr(uint8_t axis, uint8_t addr, uint32_t wval)
 {
@@ -698,6 +698,15 @@ uint16_t tmc2130_get_res(uint8_t axis)
 	return tmc2130_mres2usteps(tmc2130_mres[axis]);
 }
 
+void tmc2130_set_res(uint8_t axis, uint16_t res)
+{
+	tmc2130_mres[axis] = tmc2130_usteps2mres(res);
+//	uint32_t u = micros();
+	tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
+//	u = micros() - u;
+//	printf_P(PSTR("tmc2130_setup_chopper %c %lu us"), "XYZE"[axis], u);
+}
+
 uint8_t tmc2130_get_pwr(uint8_t axis)
 {
 	switch (axis)
@@ -872,26 +881,26 @@ void tmc2130_set_wave(uint8_t axis, uint8_t fac200)
 		break;*/
 	case 216: //calculated wave 247/1.080
 		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0x9494911e);
-		tmc2130_wr_MSLUT(axis, 1, 0x4a94a94a);
-		tmc2130_wr_MSLUT(axis, 2, 0x92492929);
-		tmc2130_wr_MSLUT(axis, 3, 0x41044444);
-		tmc2130_wr_MSLUT(axis, 4, 0x00000040);
-		tmc2130_wr_MSLUT(axis, 5, 0xaedddf7f);
-		tmc2130_wr_MSLUT(axis, 6, 0x94a956ad);
-		tmc2130_wr_MSLUT(axis, 7, 0x00808448);
+		tmc2130_wr_MSLUT(axis, 0, 0x9494911e);
+		tmc2130_wr_MSLUT(axis, 1, 0x4a94a94a);
+		tmc2130_wr_MSLUT(axis, 2, 0x92492929);
+		tmc2130_wr_MSLUT(axis, 3, 0x41044444);
+		tmc2130_wr_MSLUT(axis, 4, 0x00000040);
+		tmc2130_wr_MSLUT(axis, 5, 0xaedddf7f);
+		tmc2130_wr_MSLUT(axis, 6, 0x94a956ad);
+		tmc2130_wr_MSLUT(axis, 7, 0x00808448);
 		tmc2130_wr_MSLUTSEL(axis, 4, 159, 255, 1, 2, 1, 1);
 		break;
 	case 218: //calculated wave 247/1.090
 		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0x4a49223e);
-		tmc2130_wr_MSLUT(axis, 1, 0x4a52a529);
-		tmc2130_wr_MSLUT(axis, 2, 0x49252529);
-		tmc2130_wr_MSLUT(axis, 3, 0x08422224);
-		tmc2130_wr_MSLUT(axis, 4, 0xfc008004);
-		tmc2130_wr_MSLUT(axis, 5, 0xb6eef7df);
-		tmc2130_wr_MSLUT(axis, 6, 0xa4aaaab5);
-		tmc2130_wr_MSLUT(axis, 7, 0x00808448);
+		tmc2130_wr_MSLUT(axis, 0, 0x4a49223e);
+		tmc2130_wr_MSLUT(axis, 1, 0x4a52a529);
+		tmc2130_wr_MSLUT(axis, 2, 0x49252529);
+		tmc2130_wr_MSLUT(axis, 3, 0x08422224);
+		tmc2130_wr_MSLUT(axis, 4, 0xfc008004);
+		tmc2130_wr_MSLUT(axis, 5, 0xb6eef7df);
+		tmc2130_wr_MSLUT(axis, 6, 0xa4aaaab5);
+		tmc2130_wr_MSLUT(axis, 7, 0x00808448);
 		tmc2130_wr_MSLUTSEL(axis, 5, 153, 255, 1, 2, 1, 1);
 		break;
 	case 220: //calculated wave 247/1.100
@@ -908,26 +917,26 @@ void tmc2130_set_wave(uint8_t axis, uint8_t fac200)
 		break;
 	case 222: //calculated wave 247/1.110
 		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0x524910fe);
-		tmc2130_wr_MSLUT(axis, 1, 0xa5294a52);
-		tmc2130_wr_MSLUT(axis, 2, 0x24929294);
-		tmc2130_wr_MSLUT(axis, 3, 0x20844489);
-		tmc2130_wr_MSLUT(axis, 4, 0xc0004008);
-		tmc2130_wr_MSLUT(axis, 5, 0xdbbbdf7f);
-		tmc2130_wr_MSLUT(axis, 6, 0x252aab5a);
-		tmc2130_wr_MSLUT(axis, 7, 0x00808449);
+		tmc2130_wr_MSLUT(axis, 0, 0x524910fe);
+		tmc2130_wr_MSLUT(axis, 1, 0xa5294a52);
+		tmc2130_wr_MSLUT(axis, 2, 0x24929294);
+		tmc2130_wr_MSLUT(axis, 3, 0x20844489);
+		tmc2130_wr_MSLUT(axis, 4, 0xc0004008);
+		tmc2130_wr_MSLUT(axis, 5, 0xdbbbdf7f);
+		tmc2130_wr_MSLUT(axis, 6, 0x252aab5a);
+		tmc2130_wr_MSLUT(axis, 7, 0x00808449);
 		tmc2130_wr_MSLUTSEL(axis, 7, 157, 255, 1, 2, 1, 1);
 		break;
 	case 224: //calculated wave 247/1.120
 		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0x292223fe);
-		tmc2130_wr_MSLUT(axis, 1, 0x94a52949);
-		tmc2130_wr_MSLUT(axis, 2, 0x92524a52);
-		tmc2130_wr_MSLUT(axis, 3, 0x04222244);
-		tmc2130_wr_MSLUT(axis, 4, 0x00000101);
-		tmc2130_wr_MSLUT(axis, 5, 0x6dddefe0);
-		tmc2130_wr_MSLUT(axis, 6, 0x254aad5b);
-		tmc2130_wr_MSLUT(axis, 7, 0x00810889);
+		tmc2130_wr_MSLUT(axis, 0, 0x292223fe);
+		tmc2130_wr_MSLUT(axis, 1, 0x94a52949);
+		tmc2130_wr_MSLUT(axis, 2, 0x92524a52);
+		tmc2130_wr_MSLUT(axis, 3, 0x04222244);
+		tmc2130_wr_MSLUT(axis, 4, 0x00000101);
+		tmc2130_wr_MSLUT(axis, 5, 0x6dddefe0);
+		tmc2130_wr_MSLUT(axis, 6, 0x254aad5b);
+		tmc2130_wr_MSLUT(axis, 7, 0x00810889);
 		tmc2130_wr_MSLUTSEL(axis, 9, 164, 255, 1, 2, 1, 1);
 		break;
 /*	case 230: //calculated wave 247/1.150
diff --git a/Firmware/tmc2130.h b/Firmware/tmc2130.h
index 0ac0c03b..0ec12c5f 100644
--- a/Firmware/tmc2130.h
+++ b/Firmware/tmc2130.h
@@ -70,8 +70,8 @@ extern void tmc2130_set_pwm_grad(uint8_t axis, uint8_t pwm_ampl);
 extern uint16_t tmc2130_rd_MSCNT(uint8_t axis);
 extern uint32_t tmc2130_rd_MSCURACT(uint8_t axis);
 
-extern uint8_t tmc2130_usteps2mres(uint16_t usteps);
-#define tmc2130_mres2usteps(mres) ((uint16_t)256 >> mres)
+extern uint8_t tmc2130_usteps2mres(uint16_t usteps);
+#define tmc2130_mres2usteps(mres) ((uint16_t)256 >> mres)
 
 extern bool tmc2130_wait_standstill_xy(int timeout);
 
@@ -107,6 +107,7 @@ struct
 #pragma pack(pop)
 
 extern uint16_t tmc2130_get_res(uint8_t axis);
+extern void tmc2130_set_res(uint8_t axis, uint16_t res);
 extern uint8_t tmc2130_get_pwr(uint8_t axis);
 extern void tmc2130_set_pwr(uint8_t axis, uint8_t pwr);
 extern uint8_t tmc2130_get_inv(uint8_t axis);