TMC2208Stepper.cpp

00001 #include "TMCStepper.h"
00002 #include "TMC_MACROS.h"
00003 //#include "SERIAL_SWITCH.h"
00004 
00005 // Protected
00006 // addr needed for TMC2209
00007 TMC2208Stepper::TMC2208Stepper(Stream * SerialPort, float RS, uint8_t addr) :
00008     TMCStepper(RS),
00009     slave_address(addr)
00010     {
00011         HWSerial = SerialPort;
00012         defaults();
00013     }
00014 
00015 TMC2208Stepper::TMC2208Stepper(Stream * SerialPort, float RS, uint8_t addr, uint16_t mul_pin1, uint16_t mul_pin2) :
00016     TMC2208Stepper(SerialPort, RS)
00017     {
00018 //        SSwitch *SMulObj = new SSwitch(mul_pin1, mul_pin2, addr);
00019 //        sswitch = SMulObj;
00020     }
00021 
00022 #if SW_CAPABLE_PLATFORM
00023     // Protected
00024     // addr needed for TMC2209
00025     TMC2208Stepper::TMC2208Stepper(PinName SW_RX_pin, PinName SW_TX_pin, float RS, uint8_t addr) :
00026         TMCStepper(RS),
00027         RXTX_pin(SW_RX_pin == SW_TX_pin ? SW_RX_pin : 0),
00028         slave_address(addr)
00029         {
00030             BufferedSerial *SWSerialObj = new BufferedSerial(SW_TX_pin, SW_RX_pin);
00031             SWSerial = SWSerialObj;
00032             defaults();
00033         }
00034 
00035     void TMC2208Stepper::beginSerial(uint32_t baudrate) {
00036         if (SWSerial != nullptr)
00037         {
00038             //SWSerial->begin(baudrate);
00039             //SWSerial->end();
00040             SWSerial->set_baud(baudrate);
00041             SWSerial->set_format(
00042                         /* bits */ 8,
00043                         /* parity */ BufferedSerial::None,
00044                         /* stop bit */ 1
00045                     );
00046             SWSerial->set_blocking(false);  //set to non-blocking read
00047         }
00048 //        #if defined(ARDUINO_ARCH_AVR)
00049 //            if (RXTX_pin > 0) {
00050 //                digitalWrite(RXTX_pin, HIGH);
00051 //                pinMode(RXTX_pin, OUTPUT);
00052 //           }
00053 //        #endif
00054     }
00055 #endif
00056 
00057 void TMC2208Stepper::begin() {
00058     #if SW_CAPABLE_PLATFORM
00059         beginSerial(115200);
00060         //beginSerial(9600);
00061     #endif
00062     pdn_disable(true);
00063     mstep_reg_select(true);
00064     //Wait to initialize
00065     wait_us(replyDelay*1000);
00066 
00067 }
00068 
00069 void TMC2208Stepper::defaults() {
00070     GCONF_register.i_scale_analog = 1;
00071     GCONF_register.internal_rsense = 0; // OTP
00072     GCONF_register.en_spreadcycle = 0; // OTP
00073     GCONF_register.multistep_filt = 1; // OTP
00074     IHOLD_IRUN_register.iholddelay = 1; // OTP
00075     TPOWERDOWN_register.sr = 20;
00076     CHOPCONF_register.sr = 0x10000053;
00077     PWMCONF_register.sr = 0xC10D0024;
00078   //MSLUT0_register.sr = ???;
00079   //MSLUT1_register.sr = ???;
00080   //MSLUT2_register.sr = ???;
00081   //MSLUT3_register.sr = ???;
00082   //MSLUT4_register.sr = ???;
00083   //MSLUT5_register.sr = ???;
00084   //MSLUT6_register.sr = ???;
00085   //MSLUT7_register.sr = ???;
00086   //MSLUTSTART_register.start_sin90 = 247;
00087 }
00088 
00089 void TMC2208Stepper::push() {
00090     GCONF(GCONF_register.sr);
00091     IHOLD_IRUN(IHOLD_IRUN_register.sr);
00092     SLAVECONF(SLAVECONF_register.sr);
00093     TPOWERDOWN(TPOWERDOWN_register.sr);
00094     TPWMTHRS(TPWMTHRS_register.sr);
00095     VACTUAL(VACTUAL_register.sr);
00096     CHOPCONF(CHOPCONF_register.sr);
00097     PWMCONF(PWMCONF_register.sr);
00098 }
00099 
00100 bool TMC2208Stepper::isEnabled() { return !enn() && toff(); }
00101 
00102 uint8_t TMC2208Stepper::calcCRC(uint8_t datagram[], uint8_t len) {
00103     uint8_t crc = 0;
00104     for (uint8_t i = 0; i < len; i++) {
00105         uint8_t currentByte = datagram[i];
00106         for (uint8_t j = 0; j < 8; j++) {
00107             if ((crc >> 7) ^ (currentByte & 0x01)) {
00108                 crc = (crc << 1) ^ 0x07;
00109             } else {
00110                 crc = (crc << 1);
00111             }
00112             crc &= 0xff;
00113             currentByte = currentByte >> 1;
00114         }
00115     }
00116     return crc;
00117 }
00118 
00119 __attribute__((weak))
00120 int TMC2208Stepper::available() {
00121     int out = 0;
00122     #if SW_CAPABLE_PLATFORM
00123         if (SWSerial != nullptr) {
00124 //            out = SWSerial->available();
00125 out = 1;
00126         } else
00127     #endif
00128         if (HWSerial != nullptr) {
00129 //            out = HWSerial->available();
00130 out = 1;
00131         }
00132 
00133     return out;
00134 }
00135 
00136 __attribute__((weak))
00137 void TMC2208Stepper::preWriteCommunication() {
00138     if (HWSerial != nullptr) {
00139 //        if (sswitch != nullptr)
00140 //            sswitch->active();
00141     }
00142 }
00143 
00144 __attribute__((weak))
00145 void TMC2208Stepper::preReadCommunication() {
00146     #if SW_CAPABLE_PLATFORM
00147         if (SWSerial != nullptr) {
00148 //            SWSerial->listen();
00149         } else
00150     #endif
00151         if (HWSerial != nullptr) {
00152 //            if (sswitch != nullptr)
00153 //                sswitch->active();
00154         }
00155 }
00156 
00157 __attribute__((weak))
00158 int16_t TMC2208Stepper::serial_read() {
00159     int16_t out = 0;
00160     int16_t count = 0;
00161      
00162     #if SW_CAPABLE_PLATFORM
00163         if (SWSerial != nullptr) {
00164 //            out = SWSerial->read();
00165             count = SWSerial->read(&out, 1); // read one character
00166 //            if (SWSerial->readable()) {
00167 //                SWSerial->read(&out, 1); // read one character
00168 //            }
00169 
00170         } else
00171     #endif
00172         if (HWSerial != nullptr) {
00173 //            out = HWSerial->read();
00174             HWSerial->read(&out, 1); // read one character
00175         }
00176     if (count >= 1) {
00177 //       printf("<%02X|",out);
00178        return out;
00179     } else {
00180         return -1;
00181     }
00182 }
00183 
00184 __attribute__((weak))
00185 uint8_t TMC2208Stepper::serial_write(const uint8_t data) {
00186     int out = 0;
00187     #if SW_CAPABLE_PLATFORM
00188         if (SWSerial != nullptr) {
00189 //            printf(">%02X|",data);
00190             return SWSerial->write(&data,1);
00191         } else
00192     #endif
00193         if (HWSerial != nullptr) {
00194             return HWSerial->write(&data,1);
00195         }
00196 
00197     return out;
00198 }
00199 
00200 __attribute__((weak))
00201 void TMC2208Stepper::postWriteCommunication() {}
00202 
00203 __attribute__((weak))
00204 void TMC2208Stepper::postReadCommunication() {
00205     #if SW_CAPABLE_PLATFORM
00206 //        if (SWSerial != nullptr) {
00207 //            SWSerial->end();
00208 //        }
00209     #endif
00210 }
00211 
00212 void TMC2208Stepper::write(uint8_t addr, uint32_t regVal) {
00213     uint8_t len = 7;
00214     addr |= TMC_WRITE;
00215     uint8_t datagram[] = {TMC2208_SYNC, slave_address, addr, (uint8_t)(regVal>>24), (uint8_t)(regVal>>16), (uint8_t)(regVal>>8), (uint8_t)(regVal>>0), 0x00};
00216 
00217     datagram[len] = calcCRC(datagram, len);
00218 
00219     preWriteCommunication();
00220 
00221     for(uint8_t i=0; i<=len; i++) {
00222         bytesWritten += serial_write(datagram[i]);
00223     }
00224     postWriteCommunication();
00225 
00226     //delay(replyDelay);
00227     wait_us(replyDelay*1000);
00228 }
00229 
00230 uint64_t TMC2208Stepper::_sendDatagram(uint8_t datagram[], const uint8_t len, uint16_t timeout) {
00231 
00232 //  while (available() > 0) serial_read(); // Flush
00233     SWSerial->sync();
00234 /*    uint8_t dummy;
00235     while (SWSerial->readable()) {
00236         SWSerial->read(&dummy, 1); // read one character
00237     }
00238 */    
00239 /*    uint8_t dummy;
00240     while (SWSerial->read(&dummy, 1) >= 0) {
00241         ;
00242     }    
00243 */
00244 
00245     #if defined(ARDUINO_ARCH_AVR)
00246         if (RXTX_pin > 0) {
00247             digitalWrite(RXTX_pin, HIGH);
00248             pinMode(RXTX_pin, OUTPUT);
00249         }
00250     #endif
00251 
00252     for(int i=0; i<=len; i++) serial_write(datagram[i]);
00253 
00254     #if defined(ARDUINO_ARCH_AVR)
00255         if (RXTX_pin > 0) {
00256             pinMode(RXTX_pin, INPUT_PULLUP);
00257         }
00258     #endif
00259 
00260     //delay(this->replyDelay);
00261     wait_us(this->replyDelay*1000);
00262 
00263     // scan for the rx frame and read it
00264 
00265 //    uint32_t ms = millis();
00266     uint32_t sync_target = (static_cast<uint32_t>(datagram[0])<<16) | 0xFF00 | datagram[2];
00267     uint32_t sync = 0;
00268     
00269     // 64-bit time doesn't wrap for half a billion years, at least
00270     uint64_t start_ms, now;
00271     start_ms = Kernel::get_ms_count();
00272 
00273 
00274     do {
00275 /*        uint32_t ms2 = millis();
00276         if (ms2 != ms) {
00277             // 1ms tick
00278             ms = ms2;
00279             timeout--;
00280         }
00281         if (!timeout) return 0;
00282 */
00283         // 64-bit time doesn't wrap for half a billion years, at least
00284         now = Kernel::get_ms_count();
00285         if (now - start_ms > timeout) return 0;
00286         
00287         int16_t res = serial_read();
00288         if (res < 0) continue;
00289 
00290         sync <<= 8;
00291         sync |= res & 0xFF;
00292         sync &= 0xFFFFFF;
00293 
00294     } while (sync != sync_target);
00295 
00296     uint64_t out = sync;
00297  //   ms = millis();
00298  //   timeout = this->abort_window;
00299     
00300     start_ms = Kernel::get_ms_count();
00301     
00302     for(uint8_t i=0; i<5;) {
00303 /*        uint32_t ms2 = millis();
00304         if (ms2 != ms) {
00305             // 1ms tick
00306             ms = ms2;
00307             timeout--;
00308         }
00309         if (!timeout) return 0;
00310 */
00311         now = Kernel::get_ms_count();
00312         if (now - start_ms > timeout) return 0;
00313         
00314         int16_t res = serial_read();
00315         if (res < 0) continue;
00316 
00317         out <<= 8;
00318         out |= res & 0xFF;
00319 
00320         i++;
00321     }
00322 
00323     #if defined(ARDUINO_ARCH_AVR)
00324         if (RXTX_pin > 0) {
00325             digitalWrite(RXTX_pin, HIGH);
00326             pinMode(RXTX_pin, OUTPUT);
00327         }
00328     #endif
00329 
00330 //    while (available() > 0) serial_read(); // Flush
00331     SWSerial->sync();
00332 
00333     return out;
00334 
00335 
00336 }
00337 
00338 uint32_t TMC2208Stepper::read(uint8_t addr) {
00339     constexpr uint8_t len = 3;
00340     addr |= TMC_READ;
00341     uint8_t datagram[] = {TMC2208_SYNC, slave_address, addr, 0x00};
00342     datagram[len] = calcCRC(datagram, len);
00343     uint64_t out = 0x00000000UL;
00344 
00345     for (uint8_t i = 0; i < max_retries; i++) {
00346         preReadCommunication();
00347         out = _sendDatagram(datagram, len, abort_window);
00348         postReadCommunication();
00349 
00350 //        delay(replyDelay);
00351 //        wait_us(replyDelay*1000);
00352 
00353         CRCerror = false;
00354         uint8_t out_datagram[] = {
00355             static_cast<uint8_t>(out>>56),
00356             static_cast<uint8_t>(out>>48),
00357             static_cast<uint8_t>(out>>40),
00358             static_cast<uint8_t>(out>>32),
00359             static_cast<uint8_t>(out>>24),
00360             static_cast<uint8_t>(out>>16),
00361             static_cast<uint8_t>(out>> 8),
00362             static_cast<uint8_t>(out>> 0)
00363         };
00364         uint8_t crc = calcCRC(out_datagram, 7);
00365         if ((crc != static_cast<uint8_t>(out)) || crc == 0 ) {
00366             CRCerror = true;
00367             out = 0;
00368         } else {
00369             break;
00370         }
00371     }
00372 
00373     return out>>8;
00374 }
00375 
00376 uint8_t TMC2208Stepper::IFCNT() {
00377     return read(IFCNT_t::address);
00378 }
00379 
00380 void TMC2208Stepper::SLAVECONF(uint16_t input) {
00381     SLAVECONF_register.sr = input&0xF00;
00382     write(SLAVECONF_register.address, SLAVECONF_register.sr);
00383 }
00384 uint16_t TMC2208Stepper::SLAVECONF() {
00385     return SLAVECONF_register.sr;
00386 }
00387 void TMC2208Stepper::senddelay(uint8_t B)   { SLAVECONF_register.senddelay = B; write(SLAVECONF_register.address, SLAVECONF_register.sr); }
00388 uint8_t TMC2208Stepper::senddelay()         { return SLAVECONF_register.senddelay; }
00389 
00390 void TMC2208Stepper::OTP_PROG(uint16_t input) {
00391     write(OTP_PROG_t::address, input);
00392 }
00393 
00394 uint32_t TMC2208Stepper::OTP_READ() {
00395     return read(OTP_READ_t::address);
00396 }
00397 
00398 uint32_t TMC2208Stepper::IOIN() {
00399     return read(TMC2208_n::IOIN_t::address);
00400 }
00401 bool TMC2208Stepper::enn()          { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.enn;      }
00402 bool TMC2208Stepper::ms1()          { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.ms1;      }
00403 bool TMC2208Stepper::ms2()          { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.ms2;      }
00404 bool TMC2208Stepper::diag()         { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.diag;     }
00405 bool TMC2208Stepper::pdn_uart()     { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.pdn_uart; }
00406 bool TMC2208Stepper::step()         { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.step;     }
00407 bool TMC2208Stepper::sel_a()        { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.sel_a;    }
00408 bool TMC2208Stepper::dir()          { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.dir;      }
00409 uint8_t TMC2208Stepper::version()   { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.version;  }
00410 
00411 /*
00412 uint32_t TMC2224Stepper::IOIN() {
00413     return read(TMC2224_n::IOIN_t::address);
00414 }
00415 bool TMC2224Stepper::enn()          { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.enn;      }
00416 bool TMC2224Stepper::ms1()          { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.ms1;      }
00417 bool TMC2224Stepper::ms2()          { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.ms2;      }
00418 bool TMC2224Stepper::pdn_uart()     { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.pdn_uart; }
00419 bool TMC2224Stepper::spread()       { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.spread;   }
00420 bool TMC2224Stepper::step()         { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.step;     }
00421 bool TMC2224Stepper::sel_a()        { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.sel_a;    }
00422 bool TMC2224Stepper::dir()          { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.dir;      }
00423 uint8_t TMC2224Stepper::version()   { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.version;  }
00424 */
00425 uint16_t TMC2208Stepper::FACTORY_CONF() {
00426     return read(FACTORY_CONF_register.address);
00427 }
00428 void TMC2208Stepper::FACTORY_CONF(uint16_t input) {
00429     FACTORY_CONF_register.sr = input;
00430     write(FACTORY_CONF_register.address, FACTORY_CONF_register.sr);
00431 }
00432 void TMC2208Stepper::fclktrim(uint8_t B){ FACTORY_CONF_register.fclktrim = B; write(FACTORY_CONF_register.address, FACTORY_CONF_register.sr); }
00433 void TMC2208Stepper::ottrim(uint8_t B)  { FACTORY_CONF_register.ottrim = B; write(FACTORY_CONF_register.address, FACTORY_CONF_register.sr); }
00434 uint8_t TMC2208Stepper::fclktrim()      { FACTORY_CONF_t r{0}; r.sr = FACTORY_CONF(); return r.fclktrim; }
00435 uint8_t TMC2208Stepper::ottrim()        { FACTORY_CONF_t r{0}; r.sr = FACTORY_CONF(); return r.ottrim; }
00436 
00437 void TMC2208Stepper::VACTUAL(uint32_t input) {
00438     VACTUAL_register.sr = input;
00439     write(VACTUAL_register.address, VACTUAL_register.sr);
00440 }
00441 uint32_t TMC2208Stepper::VACTUAL() {
00442     return VACTUAL_register.sr;
00443 }
00444 
00445 uint32_t TMC2208Stepper::PWM_SCALE() {
00446     return read(TMC2208_n::PWM_SCALE_t::address);
00447 }
00448 uint8_t TMC2208Stepper::pwm_scale_sum() {
00449     TMC2208_n::PWM_SCALE_t r{0};
00450     r.sr = PWM_SCALE();
00451     return r.pwm_scale_sum;
00452 }
00453 
00454 int16_t TMC2208Stepper::pwm_scale_auto() {
00455     TMC2208_n::PWM_SCALE_t r{0};
00456     r.sr = PWM_SCALE();
00457     return r.pwm_scale_auto;
00458     // Not two's complement? 9nth bit determines sign
00459     /*
00460     uint32_t d = PWM_SCALE();
00461     int16_t response = (d>>PWM_SCALE_AUTO_bp)&0xFF;
00462     if (((d&PWM_SCALE_AUTO_bm) >> 24) & 0x1) return -response;
00463     else return response;
00464     */
00465 }
00466 
00467 // R: PWM_AUTO
00468 uint32_t TMC2208Stepper::PWM_AUTO() {
00469     return read(PWM_AUTO_t::address);
00470 }
00471 uint8_t TMC2208Stepper::pwm_ofs_auto()  { PWM_AUTO_t r{0}; r.sr = PWM_AUTO(); return r.pwm_ofs_auto; }
00472 uint8_t TMC2208Stepper::pwm_grad_auto() { PWM_AUTO_t r{0}; r.sr = PWM_AUTO(); return r.pwm_grad_auto; }