TMCStepper.cpp

00001 #include "TMCStepper.h"
00002 
00003 
00004 /*
00005   Requested current = mA = I_rms/1000
00006   Equation for current:
00007   I_rms = (CS+1)/32 * V_fs/(R_sense+0.02ohm) * 1/sqrt(2)
00008   Solve for CS ->
00009   CS = 32*sqrt(2)*I_rms*(R_sense+0.02)/V_fs - 1
00010 
00011   Example:
00012   vsense = 0b0 -> V_fs = 0.325V
00013   mA = 1640mA = I_rms/1000 = 1.64A
00014   R_sense = 0.10 Ohm
00015   ->
00016   CS = 32*sqrt(2)*1.64*(0.10+0.02)/0.325 - 1 = 26.4
00017   CS = 26
00018 */
00019 
00020 uint16_t TMCStepper::cs2rms(uint8_t CS) {
00021   return (float)(CS+1)/32.0 * (vsense() ? 0.180 : 0.325)/(Rsense+0.02) / 1.41421 * 1000;
00022 }
00023 
00024 void TMCStepper::rms_current(uint16_t mA) {
00025   uint8_t CS = 32.0*1.41421*mA/1000.0*(Rsense+0.02)/0.325 - 1;
00026   // If Current Scale is too low, turn on high sensitivity R_sense and calculate again
00027   if (CS < 16) {
00028     vsense(true);
00029     CS = 32.0*1.41421*mA/1000.0*(Rsense+0.02)/0.180 - 1;
00030   } else { // If CS >= 16, turn off high_sense_r
00031     vsense(false);
00032   }
00033 
00034   if (CS > 31)
00035     CS = 31;
00036 
00037   irun(CS);
00038   ihold(CS*holdMultiplier);
00039   //val_mA = mA;
00040 }
00041 void TMCStepper::rms_current(uint16_t mA, float mult) {
00042   holdMultiplier = mult;
00043   rms_current(mA);
00044 }
00045 
00046 uint16_t TMCStepper::rms_current() {
00047   return cs2rms(irun());
00048 }
00049 
00050 uint8_t TMCStepper::test_connection() {
00051   uint32_t drv_status = DRV_STATUS();
00052   switch (drv_status) {
00053       case 0xFFFFFFFF: return 1;
00054       case 0: return 2;
00055       default: return 0;
00056   }
00057 }
00058 
00059 void TMCStepper::hysteresis_end(int8_t value) { hend(value+3); }
00060 int8_t TMCStepper::hysteresis_end() { return hend()-3; };
00061 
00062 void TMCStepper::hysteresis_start(uint8_t value) { hstrt(value-1); }
00063 uint8_t TMCStepper::hysteresis_start() { return hstrt()+1; }
00064 
00065 void TMCStepper::microsteps(uint16_t ms) {
00066   switch(ms) {
00067     case 256: mres(0); break;
00068     case 128: mres(1); break;
00069     case  64: mres(2); break;
00070     case  32: mres(3); break;
00071     case  16: mres(4); break;
00072     case   8: mres(5); break;
00073     case   4: mres(6); break;
00074     case   2: mres(7); break;
00075     case   1: mres(8); break;
00076     default: break;
00077   }
00078 }
00079 
00080 uint16_t TMCStepper::microsteps() {
00081   switch(mres()) {
00082     case 0: return 256;
00083     case 1: return 128;
00084     case 2: return  64;
00085     case 3: return  32;
00086     case 4: return  16;
00087     case 5: return   8;
00088     case 6: return   4;
00089     case 7: return   2;
00090     case 8: return   1;
00091   }
00092   return 1;
00093 }
00094 
00095 void TMCStepper::blank_time(uint8_t value) {
00096   switch (value) {
00097     case 16: tbl(0b00); break;
00098     case 24: tbl(0b01); break;
00099     case 36: tbl(0b10); break;
00100     case 54: tbl(0b11); break;
00101   }
00102 }
00103 
00104 uint8_t TMCStepper::blank_time() {
00105   switch (tbl()) {
00106     case 0b00: return 16;
00107     case 0b01: return 24;
00108     case 0b10: return 36;
00109     case 0b11: return 54;
00110   }
00111   return 0;
00112 }
00113 
00114 ///////////////////////////////////////////////////////////////////////////////////////
00115 // R+C: GSTAT
00116 uint8_t TMCStepper::GSTAT()  { return read(GSTAT_t::address); }
00117 void  TMCStepper::GSTAT(uint8_t){ write(GSTAT_t::address, 0b111); }
00118 bool  TMCStepper::reset()    { GSTAT_t r; r.sr = GSTAT(); return r.reset; }
00119 bool  TMCStepper::drv_err()  { GSTAT_t r; r.sr = GSTAT(); return r.drv_err; }
00120 bool  TMCStepper::uv_cp()    { GSTAT_t r; r.sr = GSTAT(); return r.uv_cp; }
00121 ///////////////////////////////////////////////////////////////////////////////////////
00122 // W: TPOWERDOWN
00123 uint8_t TMCStepper::TPOWERDOWN() { return TPOWERDOWN_register.sr; }
00124 void TMCStepper::TPOWERDOWN(uint8_t input) {
00125   TPOWERDOWN_register.sr = input;
00126   write(TPOWERDOWN_register.address, TPOWERDOWN_register.sr);
00127 }
00128 ///////////////////////////////////////////////////////////////////////////////////////
00129 // R: TSTEP
00130 uint32_t TMCStepper::TSTEP() { return read(TSTEP_t::address); }
00131 ///////////////////////////////////////////////////////////////////////////////////////
00132 // W: TPWMTHRS
00133 uint32_t TMCStepper::TPWMTHRS() { return TPWMTHRS_register.sr; }
00134 void TMCStepper::TPWMTHRS(uint32_t input) {
00135   TPWMTHRS_register.sr = input;
00136   write(TPWMTHRS_register.address, TPWMTHRS_register.sr);
00137 }
00138 
00139 uint16_t TMCStepper::MSCNT() {
00140   return read(MSCNT_t::address);
00141 }
00142 
00143 uint32_t TMCStepper::MSCURACT() { return read(MSCURACT_t::address); }
00144 int16_t TMCStepper::cur_a() {
00145   MSCURACT_t r{0};
00146   r.sr = MSCURACT();
00147   int16_t value = r.cur_a;
00148   if (value > 255) value -= 512;
00149   return value;
00150 }
00151 int16_t TMCStepper::cur_b() {
00152   MSCURACT_t r{0};
00153   r.sr = MSCURACT();
00154   int16_t value = r.cur_b;
00155   if (value > 255) value -= 512;
00156   return value;
00157 }