diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index b73f5f96..6fd431aa 100644
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -871,19 +871,19 @@ void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac1000)
 	printf_P(PSTR(" factor: %s\n"), ftostr43(fac));
 	uint8_t vA = 0;                //value of currentA
 	uint8_t va = 0;                //previous vA
-	uint8_t d0 = 0;                //delta0
-	uint8_t d1 = 1;                //delta1
+	int8_t d0 = 0;                //delta0
+	int8_t d1 = 1;                //delta1
 	uint8_t w[4] = {1,1,1,1};      //W bits (MSLUTSEL)
 	uint8_t x[3] = {255,255,255};  //X segment bounds (MSLUTSEL)
 	uint8_t s = 0;                 //current segment
 	int8_t b;                      //encoded bit value
-	uint8_t dA;                    //delta value
+	int8_t dA;                    //delta value
 	int i;                         //microstep index
 	uint32_t reg;                  //tmc2130 register
 	tmc2130_wr_MSLUTSTART(axis, 0, amp);
 	for (i = 0; i < 256; i++)
 	{
-		if ((i & 31) == 0)
+		if ((i & 0x1f) == 0)
 			reg = 0;
 		// calculate value
 		if (fac == 0) // default TMC wave