|
@@ -20,8 +20,6 @@ extern long st_get_position(uint8_t axis);
|
|
|
extern void crashdet_stop_and_save_print();
|
|
|
extern void crashdet_stop_and_save_print2();
|
|
|
|
|
|
-//chipselect pins
|
|
|
-uint8_t tmc2130_cs[4] = { X_TMC2130_CS, Y_TMC2130_CS, Z_TMC2130_CS, E0_TMC2130_CS };
|
|
|
//mode
|
|
|
uint8_t tmc2130_mode = TMC2130_MODE_NORMAL;
|
|
|
//holding currents
|
|
@@ -30,7 +28,7 @@ uint8_t tmc2130_current_h[4] = TMC2130_CURRENTS_H;
|
|
|
uint8_t tmc2130_current_r[4] = TMC2130_CURRENTS_R;
|
|
|
|
|
|
//running currents for homing
|
|
|
-uint8_t tmc2130_current_r_home[4] = {10, 10, 20, 10};
|
|
|
+uint8_t tmc2130_current_r_home[4] = {8, 10, 20, 18};
|
|
|
|
|
|
|
|
|
//pwm_ampl
|
|
@@ -55,6 +53,12 @@ uint8_t tmc2130_sg_meassure = 0xff;
|
|
|
uint16_t tmc2130_sg_meassure_cnt = 0;
|
|
|
uint32_t tmc2130_sg_meassure_val = 0;
|
|
|
|
|
|
+uint8_t tmc2130_home_enabled = 0;
|
|
|
+uint8_t tmc2130_home_origin[2] = {0, 0};
|
|
|
+uint8_t tmc2130_home_bsteps[2] = {48, 48};
|
|
|
+uint8_t tmc2130_home_fsteps[2] = {48, 48};
|
|
|
+
|
|
|
+uint8_t tmc2130_wave_fac[4] = {0, 0, 0, 0};
|
|
|
|
|
|
bool tmc2130_sg_stop_on_crash = true;
|
|
|
uint8_t tmc2130_sg_diag_mask = 0x00;
|
|
@@ -104,21 +108,19 @@ bool skip_debug_msg = false;
|
|
|
#define TMC2130_REG_LOST_STEPS 0x73 // 20 bits
|
|
|
|
|
|
|
|
|
-uint16_t tmc2130_rd_TSTEP(uint8_t cs);
|
|
|
-uint16_t tmc2130_rd_MSCNT(uint8_t cs);
|
|
|
-uint16_t tmc2130_rd_DRV_STATUS(uint8_t cs);
|
|
|
+uint16_t tmc2130_rd_TSTEP(uint8_t axis);
|
|
|
+uint16_t tmc2130_rd_MSCNT(uint8_t axis);
|
|
|
+uint32_t tmc2130_rd_MSCURACT(uint8_t axis);
|
|
|
|
|
|
-void tmc2130_wr_CHOPCONF(uint8_t cs, uint8_t toff = 3, uint8_t hstrt = 4, uint8_t hend = 1, uint8_t fd3 = 0, uint8_t disfdcc = 0, uint8_t rndtf = 0, uint8_t chm = 0, uint8_t tbl = 2, uint8_t vsense = 0, uint8_t vhighfs = 0, uint8_t vhighchm = 0, uint8_t sync = 0, uint8_t mres = 0b0100, uint8_t intpol = 1, uint8_t dedge = 0, uint8_t diss2g = 0);
|
|
|
-void tmc2130_wr_PWMCONF(uint8_t cs, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t pwm_freq, uint8_t pwm_auto, uint8_t pwm_symm, uint8_t freewheel);
|
|
|
-void tmc2130_wr_TPWMTHRS(uint8_t cs, uint32_t val32);
|
|
|
-void tmc2130_wr_THIGH(uint8_t cs, uint32_t val32);
|
|
|
+void tmc2130_wr_CHOPCONF(uint8_t axis, uint8_t toff = 3, uint8_t hstrt = 4, uint8_t hend = 1, uint8_t fd3 = 0, uint8_t disfdcc = 0, uint8_t rndtf = 0, uint8_t chm = 0, uint8_t tbl = 2, uint8_t vsense = 0, uint8_t vhighfs = 0, uint8_t vhighchm = 0, uint8_t sync = 0, uint8_t mres = 0b0100, uint8_t intpol = 1, uint8_t dedge = 0, uint8_t diss2g = 0);
|
|
|
+void tmc2130_wr_PWMCONF(uint8_t axis, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t pwm_freq, uint8_t pwm_auto, uint8_t pwm_symm, uint8_t freewheel);
|
|
|
+void tmc2130_wr_TPWMTHRS(uint8_t axis, uint32_t val32);
|
|
|
+void tmc2130_wr_THIGH(uint8_t axis, uint32_t val32);
|
|
|
|
|
|
-uint8_t tmc2130_axis_by_cs(uint8_t cs);
|
|
|
-uint8_t tmc2130_calc_mres(uint16_t microstep_resolution);
|
|
|
|
|
|
-uint8_t tmc2130_wr(uint8_t cs, uint8_t addr, uint32_t wval);
|
|
|
-uint8_t tmc2130_rd(uint8_t cs, uint8_t addr, uint32_t* rval);
|
|
|
-uint8_t tmc2130_txrx(uint8_t cs, uint8_t addr, uint32_t wval, uint32_t* rval);
|
|
|
+uint8_t tmc2130_wr(uint8_t axis, uint8_t addr, uint32_t wval);
|
|
|
+uint8_t tmc2130_rd(uint8_t axis, uint8_t addr, uint32_t* rval);
|
|
|
+uint8_t tmc2130_txrx(uint8_t axis, uint8_t addr, uint32_t wval, uint32_t* rval);
|
|
|
|
|
|
|
|
|
void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_t current_r);
|
|
@@ -128,10 +130,10 @@ void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_
|
|
|
void tmc2130_init()
|
|
|
{
|
|
|
DBG(_n("tmc2130_init(), mode=%S\n"), tmc2130_mode?_n("STEALTH"):_n("NORMAL"));
|
|
|
- tmc2130_mres[0] = tmc2130_calc_mres(TMC2130_USTEPS_XY);
|
|
|
- tmc2130_mres[1] = tmc2130_calc_mres(TMC2130_USTEPS_XY);
|
|
|
- tmc2130_mres[2] = tmc2130_calc_mres(TMC2130_USTEPS_Z);
|
|
|
- tmc2130_mres[3] = tmc2130_calc_mres(TMC2130_USTEPS_E);
|
|
|
+/* tmc2130_mres[X_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
|
|
|
+ tmc2130_mres[Y_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
|
|
|
+ tmc2130_mres[Z_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_Z);
|
|
|
+ tmc2130_mres[E_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_E);*/
|
|
|
WRITE(X_TMC2130_CS, HIGH);
|
|
|
WRITE(Y_TMC2130_CS, HIGH);
|
|
|
WRITE(Z_TMC2130_CS, HIGH);
|
|
@@ -147,65 +149,33 @@ void tmc2130_init()
|
|
|
SPI.begin();
|
|
|
for (int axis = 0; axis < 2; axis++) // X Y axes
|
|
|
{
|
|
|
-/* if (tmc2130_current_r[axis] <= 31)
|
|
|
- {
|
|
|
- tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 1, 0, 0, 0, mres, TMC2130_INTPOL_XY, 0, 0);
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((tmc2130_current_r[axis] & 0x1f) << 8) | (tmc2130_current_h[axis] & 0x1f));
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 0, 0, 0, 0, mres, TMC2130_INTPOL_XY, 0, 0);
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | (((tmc2130_current_r[axis] >> 1) & 0x1f) << 8) | ((tmc2130_current_h[axis] >> 1) & 0x1f));
|
|
|
- }*/
|
|
|
tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
|
|
|
-
|
|
|
-// tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 1, 0, 0, 0, mres, TMC2130_INTPOL_XY, 0, 0);
|
|
|
-// tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((tmc2130_current_r[axis] & 0x1f) << 8) | (tmc2130_current_h[axis] & 0x1f));
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TPOWERDOWN, 0x00000000);
|
|
|
-// tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TCOOLTHRS, (tmc2130_mode == TMC2130_MODE_SILENT)?0:((axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y));
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, (tmc2130_mode == TMC2130_MODE_SILENT)?TMC2130_GCONF_SILENT:TMC2130_GCONF_SGSENS);
|
|
|
- tmc2130_wr_PWMCONF(tmc2130_cs[axis], tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
|
|
|
- tmc2130_wr_TPWMTHRS(tmc2130_cs[axis], TMC2130_TPWMTHRS);
|
|
|
- //tmc2130_wr_THIGH(tmc2130_cs[axis], TMC2130_THIGH);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_TPOWERDOWN, 0x00000000);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, (tmc2130_mode == TMC2130_MODE_SILENT)?0:((axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y));
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_GCONF, (tmc2130_mode == TMC2130_MODE_SILENT)?TMC2130_GCONF_SILENT:TMC2130_GCONF_SGSENS);
|
|
|
+ tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
|
|
|
+ tmc2130_wr_TPWMTHRS(axis, TMC2130_TPWMTHRS);
|
|
|
+ //tmc2130_wr_THIGH(axis, TMC2130_THIGH);
|
|
|
}
|
|
|
for (int axis = 2; axis < 3; axis++) // Z axis
|
|
|
{
|
|
|
-// uint8_t mres = tmc2130_mres(TMC2130_USTEPS_Z);
|
|
|
-/* if (tmc2130_current_r[axis] <= 31)
|
|
|
- {
|
|
|
- tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 1, 0, 0, 0, mres, TMC2130_INTPOL_Z, 0, 0);
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((tmc2130_current_r[axis] & 0x1f) << 8) | (tmc2130_current_h[axis] & 0x1f));
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 0, 0, 0, 0, mres, TMC2130_INTPOL_Z, 0, 0);
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | (((tmc2130_current_r[axis] >> 1) & 0x1f) << 8) | ((tmc2130_current_h[axis] >> 1) & 0x1f));
|
|
|
- }*/
|
|
|
tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
|
|
|
-
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TPOWERDOWN, 0x00000000);
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
|
|
|
-
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_TPOWERDOWN, 0x00000000);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
|
|
|
}
|
|
|
for (int axis = 3; axis < 4; axis++) // E axis
|
|
|
{
|
|
|
-// uint8_t mres = tmc2130_mres(TMC2130_USTEPS_E);
|
|
|
tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
|
|
|
-
|
|
|
-// tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 1, 0, 0, 0, mres, TMC2130_INTPOL_E, 0, 0);
|
|
|
-// tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((tmc2130_current_r[axis] & 0x1f) << 8) | (tmc2130_current_h[axis] & 0x1f));
|
|
|
-
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TPOWERDOWN, 0x00000000);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_TPOWERDOWN, 0x00000000);
|
|
|
#ifndef TMC2130_STEALTH_E
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
|
|
|
#else //TMC2130_STEALTH_E
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TCOOLTHRS, 0);
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SILENT);
|
|
|
- tmc2130_wr_PWMCONF(tmc2130_cs[axis], tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
|
|
|
- tmc2130_wr_TPWMTHRS(tmc2130_cs[axis], TMC2130_TPWMTHRS);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, 0);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SILENT);
|
|
|
+ tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
|
|
|
+ tmc2130_wr_TPWMTHRS(axis, TMC2130_TPWMTHRS);
|
|
|
#endif //TMC2130_STEALTH_E
|
|
|
}
|
|
|
|
|
@@ -217,6 +187,11 @@ void tmc2130_init()
|
|
|
tmc2130_sg_cnt[1] = 0;
|
|
|
tmc2130_sg_cnt[2] = 0;
|
|
|
tmc2130_sg_cnt[3] = 0;
|
|
|
+
|
|
|
+ tmc2130_set_wave(X_AXIS, tmc2130_wave_fac[X_AXIS]);
|
|
|
+ tmc2130_set_wave(Y_AXIS, tmc2130_wave_fac[Y_AXIS]);
|
|
|
+ tmc2130_set_wave(Z_AXIS, tmc2130_wave_fac[Z_AXIS]);
|
|
|
+ tmc2130_set_wave(E_AXIS, tmc2130_wave_fac[E_AXIS]);
|
|
|
}
|
|
|
|
|
|
uint8_t tmc2130_sample_diag()
|
|
@@ -279,11 +254,10 @@ bool tmc2130_update_sg()
|
|
|
{
|
|
|
if (tmc2130_sg_meassure <= E_AXIS)
|
|
|
{
|
|
|
- uint8_t cs = tmc2130_cs[tmc2130_sg_meassure];
|
|
|
- uint16_t sg = tmc2130_rd_DRV_STATUS(cs) & 0x3ff;
|
|
|
- tmc2130_sg_meassure_val += sg;
|
|
|
+ uint32_t val32 = 0;
|
|
|
+ tmc2130_rd(tmc2130_sg_meassure, TMC2130_REG_DRV_STATUS, &val32);
|
|
|
+ tmc2130_sg_meassure_val += (val32 & 0x3ff);
|
|
|
tmc2130_sg_meassure_cnt++;
|
|
|
-// printf_P(PSTR("tmc2130_update_sg - meassure - sg=%d\n"), sg);
|
|
|
return true;
|
|
|
}
|
|
|
return false;
|
|
@@ -296,18 +270,17 @@ void tmc2130_home_enter(uint8_t axes_mask)
|
|
|
for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) //X Y and Z axes
|
|
|
{
|
|
|
uint8_t mask = (X_AXIS_MASK << axis);
|
|
|
- uint8_t cs = tmc2130_cs[axis];
|
|
|
if (axes_mask & mask)
|
|
|
{
|
|
|
sg_homing_axes_mask |= mask;
|
|
|
//Configuration to spreadCycle
|
|
|
- tmc2130_wr(cs, TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
|
|
|
- tmc2130_wr(cs, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr_home[axis]) << 16));
|
|
|
-// tmc2130_wr(cs, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
|
|
|
- tmc2130_wr(cs, TMC2130_REG_TCOOLTHRS, (axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr_home[axis]) << 16));
|
|
|
+// tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, (axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y);
|
|
|
tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r_home[axis]);
|
|
|
if (mask & (X_AXIS_MASK | Y_AXIS_MASK | Z_AXIS_MASK))
|
|
|
- tmc2130_wr(cs, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS); //stallguard output DIAG1, DIAG1 = pushpull
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS); //stallguard output DIAG1, DIAG1 = pushpull
|
|
|
}
|
|
|
}
|
|
|
#endif //TMC2130_SG_HOMING
|
|
@@ -326,18 +299,18 @@ void tmc2130_home_exit()
|
|
|
{
|
|
|
if (tmc2130_mode == TMC2130_MODE_SILENT)
|
|
|
{
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SILENT); // Configuration back to stealthChop
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TCOOLTHRS, 0);
|
|
|
-// tmc2130_wr_PWMCONF(tmc2130_cs[i], tmc2130_pwm_ampl[i], tmc2130_pwm_grad[i], tmc2130_pwm_freq[i], tmc2130_pwm_auto[i], 0, 0);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SILENT); // Configuration back to stealthChop
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, 0);
|
|
|
+// tmc2130_wr_PWMCONF(i, tmc2130_pwm_ampl[i], tmc2130_pwm_grad[i], tmc2130_pwm_freq[i], tmc2130_pwm_auto[i], 0, 0);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
-// tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
|
|
|
+// tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
|
|
|
tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
|
|
|
-// tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TCOOLTHRS, (tmc2130_mode == TMC2130_MODE_SILENT)?0:((axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y));
|
|
|
- tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
|
|
|
+// tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, (tmc2130_mode == TMC2130_MODE_SILENT)?0:((axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y));
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
|
|
|
}
|
|
|
}
|
|
|
}
|
|
@@ -369,8 +342,8 @@ bool tmc2130_wait_standstill_xy(int timeout)
|
|
|
{
|
|
|
uint32_t drv_status_x = 0;
|
|
|
uint32_t drv_status_y = 0;
|
|
|
- tmc2130_rd(tmc2130_cs[X_AXIS], TMC2130_REG_DRV_STATUS, &drv_status_x);
|
|
|
- tmc2130_rd(tmc2130_cs[Y_AXIS], TMC2130_REG_DRV_STATUS, &drv_status_y);
|
|
|
+ tmc2130_rd(X_AXIS, TMC2130_REG_DRV_STATUS, &drv_status_x);
|
|
|
+ tmc2130_rd(Y_AXIS, TMC2130_REG_DRV_STATUS, &drv_status_y);
|
|
|
// DBG(_n("\tdrv_status_x=0x%08x drv_status_x=0x%08x\n"), drv_status_x, drv_status_y);
|
|
|
standstill = (drv_status_x & 0x80000000) && (drv_status_y & 0x80000000);
|
|
|
tmc2130_check_overtemp();
|
|
@@ -390,13 +363,13 @@ void tmc2130_check_overtemp()
|
|
|
{
|
|
|
uint32_t drv_status = 0;
|
|
|
skip_debug_msg = true;
|
|
|
- tmc2130_rd(tmc2130_cs[i], TMC2130_REG_DRV_STATUS, &drv_status);
|
|
|
+ tmc2130_rd(i, TMC2130_REG_DRV_STATUS, &drv_status);
|
|
|
if (drv_status & ((uint32_t)1 << 26))
|
|
|
{ // BIT 26 - over temp prewarning ~120C (+-20C)
|
|
|
SERIAL_ERRORRPGM(TMC_OVERTEMP_MSG);
|
|
|
SERIAL_ECHOLN(i);
|
|
|
for (int j = 0; j < 4; j++)
|
|
|
- tmc2130_wr(tmc2130_cs[j], TMC2130_REG_CHOPCONF, 0x00010000);
|
|
|
+ tmc2130_wr(j, TMC2130_REG_CHOPCONF, 0x00010000);
|
|
|
kill(TMC_OVERTEMP_MSG);
|
|
|
}
|
|
|
|
|
@@ -420,7 +393,6 @@ void tmc2130_check_overtemp()
|
|
|
|
|
|
void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_t current_r)
|
|
|
{
|
|
|
- uint8_t cs = tmc2130_cs[axis];
|
|
|
uint8_t intpol = 1;
|
|
|
uint8_t toff = TMC2130_TOFF_XYZ; // toff = 3 (fchop = 27.778kHz)
|
|
|
uint8_t hstrt = 5; //initial 4, modified to 5
|
|
@@ -443,13 +415,13 @@ void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_
|
|
|
}
|
|
|
if (current_r <= 31)
|
|
|
{
|
|
|
- tmc2130_wr_CHOPCONF(cs, toff, hstrt, hend, fd3, 0, rndtf, chm, tbl, 1, 0, 0, 0, mres, intpol, 0, 0);
|
|
|
- tmc2130_wr(cs, TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((current_r & 0x1f) << 8) | (current_h & 0x1f));
|
|
|
+ tmc2130_wr_CHOPCONF(axis, toff, hstrt, hend, fd3, 0, rndtf, chm, tbl, 1, 0, 0, 0, mres, intpol, 0, 0);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((current_r & 0x1f) << 8) | (current_h & 0x1f));
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- tmc2130_wr_CHOPCONF(cs, toff, hstrt, hend, fd3, 0, 0, 0, tbl, 0, 0, 0, 0, mres, intpol, 0, 0);
|
|
|
- tmc2130_wr(cs, TMC2130_REG_IHOLD_IRUN, 0x000f0000 | (((current_r >> 1) & 0x1f) << 8) | ((current_h >> 1) & 0x1f));
|
|
|
+ tmc2130_wr_CHOPCONF(axis, toff, hstrt, hend, fd3, 0, 0, 0, tbl, 0, 0, 0, 0, mres, intpol, 0, 0);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_IHOLD_IRUN, 0x000f0000 | (((current_r >> 1) & 0x1f) << 8) | ((current_h >> 1) & 0x1f));
|
|
|
}
|
|
|
}
|
|
|
|
|
@@ -485,7 +457,7 @@ void tmc2130_set_pwm_ampl(uint8_t axis, uint8_t pwm_ampl)
|
|
|
MYSERIAL.println((int)pwm_ampl);
|
|
|
tmc2130_pwm_ampl[axis] = pwm_ampl;
|
|
|
if (((axis == 0) || (axis == 1)) && (tmc2130_mode == TMC2130_MODE_SILENT))
|
|
|
- tmc2130_wr_PWMCONF(tmc2130_cs[axis], tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
|
|
|
+ tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
|
|
|
}
|
|
|
|
|
|
void tmc2130_set_pwm_grad(uint8_t axis, uint8_t pwm_grad)
|
|
@@ -496,32 +468,58 @@ void tmc2130_set_pwm_grad(uint8_t axis, uint8_t pwm_grad)
|
|
|
MYSERIAL.println((int)pwm_grad);
|
|
|
tmc2130_pwm_grad[axis] = pwm_grad;
|
|
|
if (((axis == 0) || (axis == 1)) && (tmc2130_mode == TMC2130_MODE_SILENT))
|
|
|
- tmc2130_wr_PWMCONF(tmc2130_cs[axis], tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
|
|
|
+ tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
|
|
|
}
|
|
|
|
|
|
-uint16_t tmc2130_rd_TSTEP(uint8_t cs)
|
|
|
+uint16_t tmc2130_rd_TSTEP(uint8_t axis)
|
|
|
{
|
|
|
uint32_t val32 = 0;
|
|
|
- tmc2130_rd(cs, TMC2130_REG_TSTEP, &val32);
|
|
|
+ tmc2130_rd(axis, TMC2130_REG_TSTEP, &val32);
|
|
|
if (val32 & 0x000f0000) return 0xffff;
|
|
|
return val32 & 0xffff;
|
|
|
}
|
|
|
|
|
|
-uint16_t tmc2130_rd_MSCNT(uint8_t cs)
|
|
|
+uint16_t tmc2130_rd_MSCNT(uint8_t axis)
|
|
|
{
|
|
|
uint32_t val32 = 0;
|
|
|
- tmc2130_rd(cs, TMC2130_REG_MSCNT, &val32);
|
|
|
+ tmc2130_rd(axis, TMC2130_REG_MSCNT, &val32);
|
|
|
return val32 & 0x3ff;
|
|
|
}
|
|
|
|
|
|
-uint16_t tmc2130_rd_DRV_STATUS(uint8_t cs)
|
|
|
+uint32_t tmc2130_rd_MSCURACT(uint8_t axis)
|
|
|
{
|
|
|
uint32_t val32 = 0;
|
|
|
- tmc2130_rd(cs, TMC2130_REG_DRV_STATUS, &val32);
|
|
|
+ tmc2130_rd(axis, TMC2130_REG_MSCURACT, &val32);
|
|
|
return val32;
|
|
|
}
|
|
|
|
|
|
-void tmc2130_wr_CHOPCONF(uint8_t cs, uint8_t toff, uint8_t hstrt, uint8_t hend, uint8_t fd3, uint8_t disfdcc, uint8_t rndtf, uint8_t chm, uint8_t tbl, uint8_t vsense, uint8_t vhighfs, uint8_t vhighchm, uint8_t sync, uint8_t mres, uint8_t intpol, uint8_t dedge, uint8_t diss2g)
|
|
|
+void tmc2130_wr_MSLUTSTART(uint8_t axis, uint8_t start_sin, uint8_t start_sin90)
|
|
|
+{
|
|
|
+ uint32_t val = 0;
|
|
|
+ val |= (uint32_t)start_sin;
|
|
|
+ val |= ((uint32_t)start_sin90) << 16;
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_MSLUTSTART, val);
|
|
|
+}
|
|
|
+
|
|
|
+void tmc2130_wr_MSLUTSEL(uint8_t axis, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t w0, uint8_t w1, uint8_t w2, uint8_t w3)
|
|
|
+{
|
|
|
+ uint32_t val = 0;
|
|
|
+ val |= ((uint32_t)w0);
|
|
|
+ val |= ((uint32_t)w1) << 2;
|
|
|
+ val |= ((uint32_t)w2) << 4;
|
|
|
+ val |= ((uint32_t)w3) << 6;
|
|
|
+ val |= ((uint32_t)x1) << 8;
|
|
|
+ val |= ((uint32_t)x2) << 16;
|
|
|
+ val |= ((uint32_t)x3) << 24;
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_MSLUTSEL, val);
|
|
|
+}
|
|
|
+
|
|
|
+void tmc2130_wr_MSLUT(uint8_t axis, uint8_t i, uint32_t val)
|
|
|
+{
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_MSLUT0 + (i & 7), val);
|
|
|
+}
|
|
|
+
|
|
|
+void tmc2130_wr_CHOPCONF(uint8_t axis, uint8_t toff, uint8_t hstrt, uint8_t hend, uint8_t fd3, uint8_t disfdcc, uint8_t rndtf, uint8_t chm, uint8_t tbl, uint8_t vsense, uint8_t vhighfs, uint8_t vhighchm, uint8_t sync, uint8_t mres, uint8_t intpol, uint8_t dedge, uint8_t diss2g)
|
|
|
{
|
|
|
uint32_t val = 0;
|
|
|
val |= (uint32_t)(toff & 15);
|
|
@@ -540,11 +538,11 @@ void tmc2130_wr_CHOPCONF(uint8_t cs, uint8_t toff, uint8_t hstrt, uint8_t hend,
|
|
|
val |= (uint32_t)(intpol & 1) << 28;
|
|
|
val |= (uint32_t)(dedge & 1) << 29;
|
|
|
val |= (uint32_t)(diss2g & 1) << 30;
|
|
|
- tmc2130_wr(cs, TMC2130_REG_CHOPCONF, val);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_CHOPCONF, val);
|
|
|
}
|
|
|
|
|
|
-//void tmc2130_wr_PWMCONF(uint8_t cs, uint8_t PWMautoScale, uint8_t PWMfreq, uint8_t PWMgrad, uint8_t PWMampl)
|
|
|
-void tmc2130_wr_PWMCONF(uint8_t cs, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t pwm_freq, uint8_t pwm_auto, uint8_t pwm_symm, uint8_t freewheel)
|
|
|
+//void tmc2130_wr_PWMCONF(uint8_t axis, uint8_t PWMautoScale, uint8_t PWMfreq, uint8_t PWMgrad, uint8_t PWMampl)
|
|
|
+void tmc2130_wr_PWMCONF(uint8_t axis, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t pwm_freq, uint8_t pwm_auto, uint8_t pwm_symm, uint8_t freewheel)
|
|
|
{
|
|
|
uint32_t val = 0;
|
|
|
val |= (uint32_t)(pwm_ampl & 255);
|
|
@@ -553,54 +551,32 @@ void tmc2130_wr_PWMCONF(uint8_t cs, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t
|
|
|
val |= (uint32_t)(pwm_auto & 1) << 18;
|
|
|
val |= (uint32_t)(pwm_symm & 1) << 19;
|
|
|
val |= (uint32_t)(freewheel & 3) << 20;
|
|
|
- tmc2130_wr(cs, TMC2130_REG_PWMCONF, val);
|
|
|
-// tmc2130_wr(cs, TMC2130_REG_PWMCONF, ((uint32_t)(PWMautoScale+PWMfreq) << 16) | ((uint32_t)PWMgrad << 8) | PWMampl); // TMC LJ -> For better readability changed to 0x00 and added PWMautoScale and PWMfreq
|
|
|
-}
|
|
|
-
|
|
|
-void tmc2130_wr_TPWMTHRS(uint8_t cs, uint32_t val32)
|
|
|
-{
|
|
|
- tmc2130_wr(cs, TMC2130_REG_TPWMTHRS, val32);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_PWMCONF, val);
|
|
|
+// tmc2130_wr(axis, TMC2130_REG_PWMCONF, ((uint32_t)(PWMautoScale+PWMfreq) << 16) | ((uint32_t)PWMgrad << 8) | PWMampl); // TMC LJ -> For better readability changed to 0x00 and added PWMautoScale and PWMfreq
|
|
|
}
|
|
|
|
|
|
-void tmc2130_wr_THIGH(uint8_t cs, uint32_t val32)
|
|
|
+void tmc2130_wr_TPWMTHRS(uint8_t axis, uint32_t val32)
|
|
|
{
|
|
|
- tmc2130_wr(cs, TMC2130_REG_THIGH, val32);
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_TPWMTHRS, val32);
|
|
|
}
|
|
|
|
|
|
-#if defined(TMC2130_DEBUG_RD) || defined(TMC2130_DEBUG_WR)
|
|
|
-uint8_t tmc2130_axis_by_cs(uint8_t cs)
|
|
|
+void tmc2130_wr_THIGH(uint8_t axis, uint32_t val32)
|
|
|
{
|
|
|
- switch (cs)
|
|
|
- {
|
|
|
- case X_TMC2130_CS: return 0;
|
|
|
- case Y_TMC2130_CS: return 1;
|
|
|
- case Z_TMC2130_CS: return 2;
|
|
|
- case E0_TMC2130_CS: return 3;
|
|
|
- }
|
|
|
- return -1;
|
|
|
+ tmc2130_wr(axis, TMC2130_REG_THIGH, val32);
|
|
|
}
|
|
|
-#endif //TMC2130_DEBUG
|
|
|
|
|
|
-uint8_t tmc2130_calc_mres(uint16_t microstep_resolution)
|
|
|
-{
|
|
|
- if (microstep_resolution == 256) return 0b0000;
|
|
|
- if (microstep_resolution == 128) return 0b0001;
|
|
|
- if (microstep_resolution == 64) return 0b0010;
|
|
|
- if (microstep_resolution == 32) return 0b0011;
|
|
|
- if (microstep_resolution == 16) return 0b0100;
|
|
|
- if (microstep_resolution == 8) return 0b0101;
|
|
|
- if (microstep_resolution == 4) return 0b0110;
|
|
|
- if (microstep_resolution == 2) return 0b0111;
|
|
|
- if (microstep_resolution == 1) return 0b1000;
|
|
|
- return 0;
|
|
|
-}
|
|
|
+uint8_t tmc2130_usteps2mres(uint16_t usteps)
|
|
|
+{
|
|
|
+ uint8_t mres = 8; while (mres && (usteps >>= 1)) mres--;
|
|
|
+ return mres;
|
|
|
+}
|
|
|
|
|
|
-uint8_t tmc2130_wr(uint8_t cs, uint8_t addr, uint32_t wval)
|
|
|
+uint8_t tmc2130_wr(uint8_t axis, uint8_t addr, uint32_t wval)
|
|
|
{
|
|
|
- uint8_t stat = tmc2130_txrx(cs, addr | 0x80, wval, 0);
|
|
|
+ uint8_t stat = tmc2130_txrx(axis, addr | 0x80, wval, 0);
|
|
|
#ifdef TMC2130_DEBUG_WR
|
|
|
MYSERIAL.print("tmc2130_wr(");
|
|
|
- MYSERIAL.print((unsigned char)tmc2130_axis_by_cs(cs), DEC);
|
|
|
+ MYSERIAL.print((unsigned char)axis, DEC);
|
|
|
MYSERIAL.print(", 0x");
|
|
|
MYSERIAL.print((unsigned char)addr, HEX);
|
|
|
MYSERIAL.print(", 0x");
|
|
@@ -611,16 +587,16 @@ uint8_t tmc2130_wr(uint8_t cs, uint8_t addr, uint32_t wval)
|
|
|
return stat;
|
|
|
}
|
|
|
|
|
|
-uint8_t tmc2130_rd(uint8_t cs, uint8_t addr, uint32_t* rval)
|
|
|
+uint8_t tmc2130_rd(uint8_t axis, uint8_t addr, uint32_t* rval)
|
|
|
{
|
|
|
uint32_t val32 = 0;
|
|
|
- uint8_t stat = tmc2130_txrx(cs, addr, 0x00000000, &val32);
|
|
|
+ uint8_t stat = tmc2130_txrx(axis, addr, 0x00000000, &val32);
|
|
|
if (rval != 0) *rval = val32;
|
|
|
#ifdef TMC2130_DEBUG_RD
|
|
|
if (!skip_debug_msg)
|
|
|
{
|
|
|
MYSERIAL.print("tmc2130_rd(");
|
|
|
- MYSERIAL.print((unsigned char)tmc2130_axis_by_cs(cs), DEC);
|
|
|
+ MYSERIAL.print((unsigned char)axis, DEC);
|
|
|
MYSERIAL.print(", 0x");
|
|
|
MYSERIAL.print((unsigned char)addr, HEX);
|
|
|
MYSERIAL.print(", 0x");
|
|
@@ -633,28 +609,50 @@ uint8_t tmc2130_rd(uint8_t cs, uint8_t addr, uint32_t* rval)
|
|
|
return stat;
|
|
|
}
|
|
|
|
|
|
-uint8_t tmc2130_txrx(uint8_t cs, uint8_t addr, uint32_t wval, uint32_t* rval)
|
|
|
+inline void tmc2130_cs_low(uint8_t axis)
|
|
|
+{
|
|
|
+ switch (axis)
|
|
|
+ {
|
|
|
+ case X_AXIS: WRITE(X_TMC2130_CS, LOW); break;
|
|
|
+ case Y_AXIS: WRITE(Y_TMC2130_CS, LOW); break;
|
|
|
+ case Z_AXIS: WRITE(Z_TMC2130_CS, LOW); break;
|
|
|
+ case E_AXIS: WRITE(E0_TMC2130_CS, LOW); break;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+inline void tmc2130_cs_high(uint8_t axis)
|
|
|
+{
|
|
|
+ switch (axis)
|
|
|
+ {
|
|
|
+ case X_AXIS: WRITE(X_TMC2130_CS, HIGH); break;
|
|
|
+ case Y_AXIS: WRITE(Y_TMC2130_CS, HIGH); break;
|
|
|
+ case Z_AXIS: WRITE(Z_TMC2130_CS, HIGH); break;
|
|
|
+ case E_AXIS: WRITE(E0_TMC2130_CS, HIGH); break;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+uint8_t tmc2130_txrx(uint8_t axis, uint8_t addr, uint32_t wval, uint32_t* rval)
|
|
|
{
|
|
|
//datagram1 - request
|
|
|
SPI.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE3));
|
|
|
- digitalWrite(cs, LOW);
|
|
|
+ tmc2130_cs_low(axis);
|
|
|
SPI.transfer(addr); // address
|
|
|
SPI.transfer((wval >> 24) & 0xff); // MSB
|
|
|
SPI.transfer((wval >> 16) & 0xff);
|
|
|
SPI.transfer((wval >> 8) & 0xff);
|
|
|
SPI.transfer(wval & 0xff); // LSB
|
|
|
- digitalWrite(cs, HIGH);
|
|
|
+ tmc2130_cs_high(axis);
|
|
|
SPI.endTransaction();
|
|
|
//datagram2 - response
|
|
|
SPI.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE3));
|
|
|
- digitalWrite(cs, LOW);
|
|
|
+ tmc2130_cs_low(axis);
|
|
|
uint8_t stat = SPI.transfer(0); // status
|
|
|
uint32_t val32 = 0;
|
|
|
val32 = SPI.transfer(0); // MSB
|
|
|
val32 = (val32 << 8) | SPI.transfer(0);
|
|
|
val32 = (val32 << 8) | SPI.transfer(0);
|
|
|
val32 = (val32 << 8) | SPI.transfer(0); // LSB
|
|
|
- digitalWrite(cs, HIGH);
|
|
|
+ tmc2130_cs_high(axis);
|
|
|
SPI.endTransaction();
|
|
|
if (rval != 0) *rval = val32;
|
|
|
return stat;
|
|
@@ -669,5 +667,357 @@ void tmc2130_eeprom_save_config()
|
|
|
|
|
|
}
|
|
|
|
|
|
+#define _GET_PWR_X (READ(X_ENABLE_PIN) == X_ENABLE_ON)
|
|
|
+#define _GET_PWR_Y (READ(Y_ENABLE_PIN) == Y_ENABLE_ON)
|
|
|
+#define _GET_PWR_Z (READ(Z_ENABLE_PIN) == Z_ENABLE_ON)
|
|
|
+#define _GET_PWR_E (READ(E0_ENABLE_PIN) == E_ENABLE_ON)
|
|
|
+
|
|
|
+#define _SET_PWR_X(ena) { WRITE(X_ENABLE_PIN, ena?X_ENABLE_ON:!X_ENABLE_ON); asm("nop"); }
|
|
|
+#define _SET_PWR_Y(ena) { WRITE(Y_ENABLE_PIN, ena?Y_ENABLE_ON:!Y_ENABLE_ON); asm("nop"); }
|
|
|
+#define _SET_PWR_Z(ena) { WRITE(Z_ENABLE_PIN, ena?Z_ENABLE_ON:!Z_ENABLE_ON); asm("nop"); }
|
|
|
+#define _SET_PWR_E(ena) { WRITE(E0_ENABLE_PIN, ena?E_ENABLE_ON:!E_ENABLE_ON); asm("nop"); }
|
|
|
+
|
|
|
+#define _GET_DIR_X (READ(X_DIR_PIN) == INVERT_X_DIR)
|
|
|
+#define _GET_DIR_Y (READ(Y_DIR_PIN) == INVERT_Y_DIR)
|
|
|
+#define _GET_DIR_Z (READ(Z_DIR_PIN) == INVERT_Z_DIR)
|
|
|
+#define _GET_DIR_E (READ(E0_DIR_PIN) == INVERT_E0_DIR)
|
|
|
+
|
|
|
+#define _SET_DIR_X(dir) { WRITE(X_DIR_PIN, dir?INVERT_X_DIR:!INVERT_X_DIR); asm("nop"); }
|
|
|
+#define _SET_DIR_Y(dir) { WRITE(Y_DIR_PIN, dir?INVERT_Y_DIR:!INVERT_Y_DIR); asm("nop"); }
|
|
|
+#define _SET_DIR_Z(dir) { WRITE(Z_DIR_PIN, dir?INVERT_Z_DIR:!INVERT_Z_DIR); asm("nop"); }
|
|
|
+#define _SET_DIR_E(dir) { WRITE(E0_DIR_PIN, dir?INVERT_E0_DIR:!INVERT_E0_DIR); asm("nop"); }
|
|
|
+
|
|
|
+#define _DO_STEP_X { WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN); asm("nop"); WRITE(X_STEP_PIN, INVERT_X_STEP_PIN); asm("nop"); }
|
|
|
+#define _DO_STEP_Y { WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN); asm("nop"); WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN); asm("nop"); }
|
|
|
+#define _DO_STEP_Z { WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN); asm("nop"); WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN); asm("nop"); }
|
|
|
+#define _DO_STEP_E { WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); asm("nop"); WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN); asm("nop"); }
|
|
|
+
|
|
|
+
|
|
|
+uint16_t tmc2130_get_res(uint8_t axis)
|
|
|
+{
|
|
|
+ return tmc2130_mres2usteps(tmc2130_mres[axis]);
|
|
|
+}
|
|
|
+
|
|
|
+uint8_t tmc2130_get_pwr(uint8_t axis)
|
|
|
+{
|
|
|
+ switch (axis)
|
|
|
+ {
|
|
|
+ case X_AXIS: return _GET_PWR_X;
|
|
|
+ case Y_AXIS: return _GET_PWR_Y;
|
|
|
+ case Z_AXIS: return _GET_PWR_Z;
|
|
|
+ case E_AXIS: return _GET_PWR_E;
|
|
|
+ }
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+void tmc2130_set_pwr(uint8_t axis, uint8_t pwr)
|
|
|
+{
|
|
|
+ switch (axis)
|
|
|
+ {
|
|
|
+ case X_AXIS: _SET_PWR_X(pwr); break;
|
|
|
+ case Y_AXIS: _SET_PWR_Y(pwr); break;
|
|
|
+ case Z_AXIS: _SET_PWR_Z(pwr); break;
|
|
|
+ case E_AXIS: _SET_PWR_E(pwr); break;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+uint8_t tmc2130_get_inv(uint8_t axis)
|
|
|
+{
|
|
|
+ switch (axis)
|
|
|
+ {
|
|
|
+ case X_AXIS: return INVERT_X_DIR;
|
|
|
+ case Y_AXIS: return INVERT_Y_DIR;
|
|
|
+ case Z_AXIS: return INVERT_Z_DIR;
|
|
|
+ case E_AXIS: return INVERT_E0_DIR;
|
|
|
+ }
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+uint8_t tmc2130_get_dir(uint8_t axis)
|
|
|
+{
|
|
|
+ switch (axis)
|
|
|
+ {
|
|
|
+ case X_AXIS: return _GET_DIR_X;
|
|
|
+ case Y_AXIS: return _GET_DIR_Y;
|
|
|
+ case Z_AXIS: return _GET_DIR_Z;
|
|
|
+ case E_AXIS: return _GET_DIR_E;
|
|
|
+ }
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void tmc2130_set_dir(uint8_t axis, uint8_t dir)
|
|
|
+{
|
|
|
+ switch (axis)
|
|
|
+ {
|
|
|
+ case X_AXIS: _SET_DIR_X(dir); break;
|
|
|
+ case Y_AXIS: _SET_DIR_Y(dir); break;
|
|
|
+ case Z_AXIS: _SET_DIR_Z(dir); break;
|
|
|
+ case E_AXIS: _SET_DIR_E(dir); break;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void tmc2130_do_step(uint8_t axis)
|
|
|
+{
|
|
|
+ switch (axis)
|
|
|
+ {
|
|
|
+ case X_AXIS: _DO_STEP_X; break;
|
|
|
+ case Y_AXIS: _DO_STEP_Y; break;
|
|
|
+ case Z_AXIS: _DO_STEP_Z; break;
|
|
|
+ case E_AXIS: _DO_STEP_E; break;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void tmc2130_do_steps(uint8_t axis, uint16_t steps, uint8_t dir, uint16_t delay_us)
|
|
|
+{
|
|
|
+ tmc2130_set_dir(axis, dir);
|
|
|
+ delayMicroseconds(100);
|
|
|
+ while (steps--)
|
|
|
+ {
|
|
|
+ tmc2130_do_step(axis);
|
|
|
+ delayMicroseconds(delay_us);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void tmc2130_goto_step(uint8_t axis, uint8_t step, uint8_t dir, uint16_t delay_us, uint16_t microstep_resolution)
|
|
|
+{
|
|
|
+ printf_P(PSTR("tmc2130_goto_step %d %d %d %d \n"), axis, step, dir, delay_us, microstep_resolution);
|
|
|
+ uint8_t shift; for (shift = 0; shift < 8; shift++) if (microstep_resolution == (256 >> shift)) break;
|
|
|
+ uint16_t cnt = 4 * (1 << (8 - shift));
|
|
|
+ uint16_t mscnt = tmc2130_rd_MSCNT(axis);
|
|
|
+ if (dir == 2)
|
|
|
+ {
|
|
|
+ dir = tmc2130_get_inv(axis)?0:1;
|
|
|
+ int steps = (int)step - (int)(mscnt >> shift);
|
|
|
+ if (steps < 0)
|
|
|
+ {
|
|
|
+ dir ^= 1;
|
|
|
+ steps = -steps;
|
|
|
+ }
|
|
|
+ if (steps > (cnt / 2))
|
|
|
+ {
|
|
|
+ dir ^= 1;
|
|
|
+ steps = cnt - steps;
|
|
|
+ }
|
|
|
+ cnt = steps;
|
|
|
+ }
|
|
|
+ tmc2130_set_dir(axis, dir);
|
|
|
+ delayMicroseconds(100);
|
|
|
+ mscnt = tmc2130_rd_MSCNT(axis);
|
|
|
+ while ((cnt--) && ((mscnt >> shift) != step))
|
|
|
+ {
|
|
|
+ tmc2130_do_step(axis);
|
|
|
+ delayMicroseconds(delay_us);
|
|
|
+ mscnt = tmc2130_rd_MSCNT(axis);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void tmc2130_get_wave(uint8_t axis, uint8_t* data, FILE* stream)
|
|
|
+{
|
|
|
+ uint8_t pwr = tmc2130_get_pwr(axis);
|
|
|
+ tmc2130_set_pwr(axis, 0);
|
|
|
+ tmc2130_setup_chopper(axis, tmc2130_usteps2mres(256), tmc2130_current_h[axis], tmc2130_current_r[axis]);
|
|
|
+ tmc2130_goto_step(axis, 0, 2, 100, 256);
|
|
|
+ tmc2130_set_dir(axis, tmc2130_get_inv(axis)?0:1);
|
|
|
+ for (int i = 0; i <= 255; i++)
|
|
|
+ {
|
|
|
+ uint32_t val = tmc2130_rd_MSCURACT(axis);
|
|
|
+ uint16_t mscnt = tmc2130_rd_MSCNT(axis);
|
|
|
+ int curA = (val & 0xff) | ((val << 7) & 0x8000);
|
|
|
+ if (stream)
|
|
|
+ {
|
|
|
+ if (mscnt == i)
|
|
|
+ fprintf_P(stream, PSTR("%d\t%d\n"), i, curA);
|
|
|
+ else //TODO - remove this check
|
|
|
+ fprintf_P(stream, PSTR("!! (i=%d MSCNT=%d)\n"), i, mscnt);
|
|
|
+ }
|
|
|
+ if (data) *(data++) = curA;
|
|
|
+ tmc2130_do_step(axis);
|
|
|
+ delayMicroseconds(100);
|
|
|
+ }
|
|
|
+ tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
|
|
|
+}
|
|
|
+
|
|
|
+void tmc2130_set_wave(uint8_t axis, uint8_t fac200)
|
|
|
+{
|
|
|
+// printf_P(PSTR("tmc2130_set_wave %d %d\n"), axis, fac200);
|
|
|
+ if (fac200 < TMC2130_WAVE_FAC200_MIN) fac200 = 0;
|
|
|
+ if (fac200 > TMC2130_WAVE_FAC200_MAX) fac200 = TMC2130_WAVE_FAC200_MAX;
|
|
|
+// printf_P(PSTR(" tmc2130_set_wave %d %d\n"), axis, fac200);
|
|
|
+ switch (fac200)
|
|
|
+ {
|
|
|
+ case 0: //default TMC wave 247/0
|
|
|
+ tmc2130_wr_MSLUTSTART(axis, 0, 247);
|
|
|
+ tmc2130_wr_MSLUT(axis, 0, 0xaaaab556);
|
|
|
+ tmc2130_wr_MSLUT(axis, 1, 0x4a9554aa);
|
|
|
+ tmc2130_wr_MSLUT(axis, 2, 0x24492929);
|
|
|
+ tmc2130_wr_MSLUT(axis, 3, 0x10104222);
|
|
|
+ tmc2130_wr_MSLUT(axis, 4, 0xf8000000);
|
|
|
+ tmc2130_wr_MSLUT(axis, 5, 0xb5bb777d);
|
|
|
+ tmc2130_wr_MSLUT(axis, 6, 0x49295556);
|
|
|
+ tmc2130_wr_MSLUT(axis, 7, 0x00404222);
|
|
|
+ tmc2130_wr_MSLUTSEL(axis, 2, 154, 255, 1, 2, 1, 1);
|
|
|
+ break;
|
|
|
+/* case 215: //calculated wave 247/1.075
|
|
|
+ tmc2130_wr_MSLUTSTART(axis, 0, 247);
|
|
|
+ tmc2130_wr_MSLUT(axis, 0, 0x4a52491e);
|
|
|
+ tmc2130_wr_MSLUT(axis, 1, 0xa54a54a9);
|
|
|
+ tmc2130_wr_MSLUT(axis, 2, 0x49249494);
|
|
|
+ tmc2130_wr_MSLUT(axis, 3, 0x10421122);
|
|
|
+ tmc2130_wr_MSLUT(axis, 4, 0x00000008);
|
|
|
+ tmc2130_wr_MSLUT(axis, 5, 0x6ddbdefc);
|
|
|
+ tmc2130_wr_MSLUT(axis, 6, 0x94a555ad);
|
|
|
+ tmc2130_wr_MSLUT(axis, 7, 0x00408444);
|
|
|
+ tmc2130_wr_MSLUTSEL(axis, 4, 161, 255, 1, 2, 1, 1);
|
|
|
+ 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_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_MSLUTSEL(axis, 5, 153, 255, 1, 2, 1, 1);
|
|
|
+ break;
|
|
|
+ case 220: //calculated wave 247/1.100
|
|
|
+ tmc2130_wr_MSLUTSTART(axis, 0, 247);
|
|
|
+ tmc2130_wr_MSLUT(axis, 0, 0xa492487e);
|
|
|
+ tmc2130_wr_MSLUT(axis, 1, 0x294a52a4);
|
|
|
+ tmc2130_wr_MSLUT(axis, 2, 0x492494a5);
|
|
|
+ tmc2130_wr_MSLUT(axis, 3, 0x82110912);
|
|
|
+ tmc2130_wr_MSLUT(axis, 4, 0x00000080);
|
|
|
+ tmc2130_wr_MSLUT(axis, 5, 0xdb777df8);
|
|
|
+ tmc2130_wr_MSLUT(axis, 6, 0x252aaad6);
|
|
|
+ tmc2130_wr_MSLUT(axis, 7, 0x00808449);
|
|
|
+ tmc2130_wr_MSLUTSEL(axis, 6, 162, 255, 1, 2, 1, 1);
|
|
|
+ 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_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_MSLUTSEL(axis, 9, 164, 255, 1, 2, 1, 1);
|
|
|
+ break;
|
|
|
+/* case 230: //calculated wave 247/1.150
|
|
|
+ tmc2130_wr_MSLUTSTART(axis, 0, 247);
|
|
|
+ tmc2130_wr_MSLUT(axis, 0, 0x24444076);
|
|
|
+ tmc2130_wr_MSLUT(axis, 1, 0x29294949);
|
|
|
+ tmc2130_wr_MSLUT(axis, 2, 0x24a494a5);
|
|
|
+ tmc2130_wr_MSLUT(axis, 3, 0x84222449);
|
|
|
+ tmc2130_wr_MSLUT(axis, 4, 0x00004020);
|
|
|
+ tmc2130_wr_MSLUT(axis, 5, 0xdbbbefe0);
|
|
|
+ tmc2130_wr_MSLUT(axis, 6, 0x495556b5);
|
|
|
+ tmc2130_wr_MSLUT(axis, 7, 0x00810889);
|
|
|
+ tmc2130_wr_MSLUTSEL(axis, 6, 164, 255, 1, 2, 1, 1);
|
|
|
+ break;*/
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void bubblesort_uint8(uint8_t* data, uint8_t size, uint8_t* data2)
|
|
|
+{
|
|
|
+ uint8_t changed = 1;
|
|
|
+ while (changed)
|
|
|
+ {
|
|
|
+ changed = 0;
|
|
|
+ for (uint8_t i = 0; i < (size - 1); i++)
|
|
|
+ if (data[i] > data[i+1])
|
|
|
+ {
|
|
|
+ uint8_t register d = data[i];
|
|
|
+ data[i] = data[i+1];
|
|
|
+ data[i+1] = d;
|
|
|
+ if (data2)
|
|
|
+ {
|
|
|
+ d = data2[i];
|
|
|
+ data2[i] = data2[i+1];
|
|
|
+ data2[i+1] = d;
|
|
|
+ }
|
|
|
+ changed = 1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+uint8_t clusterize_uint8(uint8_t* data, uint8_t size, uint8_t* ccnt, uint8_t* cval, uint8_t tol)
|
|
|
+{
|
|
|
+ uint8_t cnt = 1;
|
|
|
+ uint16_t sum = data[0];
|
|
|
+ uint8_t cl = 0;
|
|
|
+ for (uint8_t i = 1; i < size; i++)
|
|
|
+ {
|
|
|
+ uint8_t d = data[i];
|
|
|
+ uint8_t val = sum / cnt;
|
|
|
+ uint8_t dif = 0;
|
|
|
+ if (val > d) dif = val - d;
|
|
|
+ else dif = d - val;
|
|
|
+ if (dif <= tol)
|
|
|
+ {
|
|
|
+ cnt += 1;
|
|
|
+ sum += d;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if (ccnt) ccnt[cl] = cnt;
|
|
|
+ if (cval) cval[cl] = val;
|
|
|
+ cnt = 1;
|
|
|
+ sum = d;
|
|
|
+ cl += 1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (ccnt) ccnt[cl] = cnt;
|
|
|
+ if (cval) cval[cl] = sum / cnt;
|
|
|
+ return ++cl;
|
|
|
+}
|
|
|
+
|
|
|
+void tmc2130_home_calibrate(uint8_t axis)
|
|
|
+{
|
|
|
+ uint8_t step[16];
|
|
|
+ uint8_t cnt[16];
|
|
|
+ uint8_t val[16];
|
|
|
+ homeaxis(axis, 16, step);
|
|
|
+ bubblesort_uint8(step, 16, 0);
|
|
|
+ printf_P(PSTR("sorted samples:\n"));
|
|
|
+ for (uint8_t i = 0; i < 16; i++)
|
|
|
+ printf_P(PSTR(" i=%2d step=%2d\n"), i, step[i]);
|
|
|
+ uint8_t cl = clusterize_uint8(step, 16, cnt, val, 1);
|
|
|
+ printf_P(PSTR("clusters:\n"));
|
|
|
+ for (uint8_t i = 0; i < cl; i++)
|
|
|
+ printf_P(PSTR(" i=%2d cnt=%2d val=%2d\n"), i, cnt[i], val[i]);
|
|
|
+ bubblesort_uint8(cnt, cl, val);
|
|
|
+ tmc2130_home_origin[axis] = val[cl-1];
|
|
|
+ printf_P(PSTR("result value: %d\n"), tmc2130_home_origin[axis]);
|
|
|
+ if (axis == X_AXIS) eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_X_ORIGIN, tmc2130_home_origin[X_AXIS]);
|
|
|
+ else if (axis == Y_AXIS) eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_Y_ORIGIN, tmc2130_home_origin[Y_AXIS]);
|
|
|
+}
|
|
|
|
|
|
#endif //TMC2130
|