Library for Trinamic TMC2209 stepper modules to drive bipolar stepper motors. Ported and adapted from https://github.com/teemuatlut/TMCStepper
TMC2208Stepper.cpp@0:f4343071c8b1, 2021-03-18 (annotated)
- Committer:
- charly
- Date:
- Thu Mar 18 20:50:27 2021 +0000
- Revision:
- 0:f4343071c8b1
- Child:
- 1:5ba0c258c4ed
First version of TMCStepper for Trinamic TMC2209-modules.; Software ported and adapted from https://github.com/teemuatlut/TMCStepper
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
charly | 0:f4343071c8b1 | 1 | #include "TMCStepper.h" |
charly | 0:f4343071c8b1 | 2 | #include "TMC_MACROS.h" |
charly | 0:f4343071c8b1 | 3 | //#include "SERIAL_SWITCH.h" |
charly | 0:f4343071c8b1 | 4 | |
charly | 0:f4343071c8b1 | 5 | // Protected |
charly | 0:f4343071c8b1 | 6 | // addr needed for TMC2209 |
charly | 0:f4343071c8b1 | 7 | TMC2208Stepper::TMC2208Stepper(Stream * SerialPort, float RS, uint8_t addr) : |
charly | 0:f4343071c8b1 | 8 | TMCStepper(RS), |
charly | 0:f4343071c8b1 | 9 | slave_address(addr) |
charly | 0:f4343071c8b1 | 10 | { |
charly | 0:f4343071c8b1 | 11 | HWSerial = SerialPort; |
charly | 0:f4343071c8b1 | 12 | defaults(); |
charly | 0:f4343071c8b1 | 13 | } |
charly | 0:f4343071c8b1 | 14 | |
charly | 0:f4343071c8b1 | 15 | TMC2208Stepper::TMC2208Stepper(Stream * SerialPort, float RS, uint8_t addr, uint16_t mul_pin1, uint16_t mul_pin2) : |
charly | 0:f4343071c8b1 | 16 | TMC2208Stepper(SerialPort, RS) |
charly | 0:f4343071c8b1 | 17 | { |
charly | 0:f4343071c8b1 | 18 | // SSwitch *SMulObj = new SSwitch(mul_pin1, mul_pin2, addr); |
charly | 0:f4343071c8b1 | 19 | // sswitch = SMulObj; |
charly | 0:f4343071c8b1 | 20 | } |
charly | 0:f4343071c8b1 | 21 | |
charly | 0:f4343071c8b1 | 22 | #if SW_CAPABLE_PLATFORM |
charly | 0:f4343071c8b1 | 23 | // Protected |
charly | 0:f4343071c8b1 | 24 | // addr needed for TMC2209 |
charly | 0:f4343071c8b1 | 25 | TMC2208Stepper::TMC2208Stepper(PinName SW_RX_pin, PinName SW_TX_pin, float RS, uint8_t addr) : |
charly | 0:f4343071c8b1 | 26 | TMCStepper(RS), |
charly | 0:f4343071c8b1 | 27 | RXTX_pin(SW_RX_pin == SW_TX_pin ? SW_RX_pin : 0), |
charly | 0:f4343071c8b1 | 28 | slave_address(addr) |
charly | 0:f4343071c8b1 | 29 | { |
charly | 0:f4343071c8b1 | 30 | BufferedSerial *SWSerialObj = new BufferedSerial(SW_TX_pin, SW_RX_pin); |
charly | 0:f4343071c8b1 | 31 | SWSerial = SWSerialObj; |
charly | 0:f4343071c8b1 | 32 | defaults(); |
charly | 0:f4343071c8b1 | 33 | } |
charly | 0:f4343071c8b1 | 34 | |
charly | 0:f4343071c8b1 | 35 | void TMC2208Stepper::beginSerial(uint32_t baudrate) { |
charly | 0:f4343071c8b1 | 36 | if (SWSerial != nullptr) |
charly | 0:f4343071c8b1 | 37 | { |
charly | 0:f4343071c8b1 | 38 | //SWSerial->begin(baudrate); |
charly | 0:f4343071c8b1 | 39 | //SWSerial->end(); |
charly | 0:f4343071c8b1 | 40 | SWSerial->set_baud(baudrate); |
charly | 0:f4343071c8b1 | 41 | SWSerial->set_format( |
charly | 0:f4343071c8b1 | 42 | /* bits */ 8, |
charly | 0:f4343071c8b1 | 43 | /* parity */ BufferedSerial::None, |
charly | 0:f4343071c8b1 | 44 | /* stop bit */ 1 |
charly | 0:f4343071c8b1 | 45 | ); |
charly | 0:f4343071c8b1 | 46 | SWSerial->set_blocking(true); |
charly | 0:f4343071c8b1 | 47 | } |
charly | 0:f4343071c8b1 | 48 | // #if defined(ARDUINO_ARCH_AVR) |
charly | 0:f4343071c8b1 | 49 | // if (RXTX_pin > 0) { |
charly | 0:f4343071c8b1 | 50 | // digitalWrite(RXTX_pin, HIGH); |
charly | 0:f4343071c8b1 | 51 | // pinMode(RXTX_pin, OUTPUT); |
charly | 0:f4343071c8b1 | 52 | // } |
charly | 0:f4343071c8b1 | 53 | // #endif |
charly | 0:f4343071c8b1 | 54 | } |
charly | 0:f4343071c8b1 | 55 | #endif |
charly | 0:f4343071c8b1 | 56 | |
charly | 0:f4343071c8b1 | 57 | void TMC2208Stepper::begin() { |
charly | 0:f4343071c8b1 | 58 | #if SW_CAPABLE_PLATFORM |
charly | 0:f4343071c8b1 | 59 | beginSerial(115200); |
charly | 0:f4343071c8b1 | 60 | //beginSerial(9600); |
charly | 0:f4343071c8b1 | 61 | #endif |
charly | 0:f4343071c8b1 | 62 | pdn_disable(true); |
charly | 0:f4343071c8b1 | 63 | mstep_reg_select(true); |
charly | 0:f4343071c8b1 | 64 | //Wait to initialize |
charly | 0:f4343071c8b1 | 65 | wait_us(replyDelay*1000); |
charly | 0:f4343071c8b1 | 66 | |
charly | 0:f4343071c8b1 | 67 | } |
charly | 0:f4343071c8b1 | 68 | |
charly | 0:f4343071c8b1 | 69 | void TMC2208Stepper::defaults() { |
charly | 0:f4343071c8b1 | 70 | GCONF_register.i_scale_analog = 1; |
charly | 0:f4343071c8b1 | 71 | GCONF_register.internal_rsense = 0; // OTP |
charly | 0:f4343071c8b1 | 72 | GCONF_register.en_spreadcycle = 0; // OTP |
charly | 0:f4343071c8b1 | 73 | GCONF_register.multistep_filt = 1; // OTP |
charly | 0:f4343071c8b1 | 74 | IHOLD_IRUN_register.iholddelay = 1; // OTP |
charly | 0:f4343071c8b1 | 75 | TPOWERDOWN_register.sr = 20; |
charly | 0:f4343071c8b1 | 76 | CHOPCONF_register.sr = 0x10000053; |
charly | 0:f4343071c8b1 | 77 | PWMCONF_register.sr = 0xC10D0024; |
charly | 0:f4343071c8b1 | 78 | //MSLUT0_register.sr = ???; |
charly | 0:f4343071c8b1 | 79 | //MSLUT1_register.sr = ???; |
charly | 0:f4343071c8b1 | 80 | //MSLUT2_register.sr = ???; |
charly | 0:f4343071c8b1 | 81 | //MSLUT3_register.sr = ???; |
charly | 0:f4343071c8b1 | 82 | //MSLUT4_register.sr = ???; |
charly | 0:f4343071c8b1 | 83 | //MSLUT5_register.sr = ???; |
charly | 0:f4343071c8b1 | 84 | //MSLUT6_register.sr = ???; |
charly | 0:f4343071c8b1 | 85 | //MSLUT7_register.sr = ???; |
charly | 0:f4343071c8b1 | 86 | //MSLUTSTART_register.start_sin90 = 247; |
charly | 0:f4343071c8b1 | 87 | } |
charly | 0:f4343071c8b1 | 88 | |
charly | 0:f4343071c8b1 | 89 | void TMC2208Stepper::push() { |
charly | 0:f4343071c8b1 | 90 | GCONF(GCONF_register.sr); |
charly | 0:f4343071c8b1 | 91 | IHOLD_IRUN(IHOLD_IRUN_register.sr); |
charly | 0:f4343071c8b1 | 92 | SLAVECONF(SLAVECONF_register.sr); |
charly | 0:f4343071c8b1 | 93 | TPOWERDOWN(TPOWERDOWN_register.sr); |
charly | 0:f4343071c8b1 | 94 | TPWMTHRS(TPWMTHRS_register.sr); |
charly | 0:f4343071c8b1 | 95 | VACTUAL(VACTUAL_register.sr); |
charly | 0:f4343071c8b1 | 96 | CHOPCONF(CHOPCONF_register.sr); |
charly | 0:f4343071c8b1 | 97 | PWMCONF(PWMCONF_register.sr); |
charly | 0:f4343071c8b1 | 98 | } |
charly | 0:f4343071c8b1 | 99 | |
charly | 0:f4343071c8b1 | 100 | bool TMC2208Stepper::isEnabled() { return !enn() && toff(); } |
charly | 0:f4343071c8b1 | 101 | |
charly | 0:f4343071c8b1 | 102 | uint8_t TMC2208Stepper::calcCRC(uint8_t datagram[], uint8_t len) { |
charly | 0:f4343071c8b1 | 103 | uint8_t crc = 0; |
charly | 0:f4343071c8b1 | 104 | for (uint8_t i = 0; i < len; i++) { |
charly | 0:f4343071c8b1 | 105 | uint8_t currentByte = datagram[i]; |
charly | 0:f4343071c8b1 | 106 | for (uint8_t j = 0; j < 8; j++) { |
charly | 0:f4343071c8b1 | 107 | if ((crc >> 7) ^ (currentByte & 0x01)) { |
charly | 0:f4343071c8b1 | 108 | crc = (crc << 1) ^ 0x07; |
charly | 0:f4343071c8b1 | 109 | } else { |
charly | 0:f4343071c8b1 | 110 | crc = (crc << 1); |
charly | 0:f4343071c8b1 | 111 | } |
charly | 0:f4343071c8b1 | 112 | crc &= 0xff; |
charly | 0:f4343071c8b1 | 113 | currentByte = currentByte >> 1; |
charly | 0:f4343071c8b1 | 114 | } |
charly | 0:f4343071c8b1 | 115 | } |
charly | 0:f4343071c8b1 | 116 | return crc; |
charly | 0:f4343071c8b1 | 117 | } |
charly | 0:f4343071c8b1 | 118 | |
charly | 0:f4343071c8b1 | 119 | __attribute__((weak)) |
charly | 0:f4343071c8b1 | 120 | int TMC2208Stepper::available() { |
charly | 0:f4343071c8b1 | 121 | int out = 0; |
charly | 0:f4343071c8b1 | 122 | #if SW_CAPABLE_PLATFORM |
charly | 0:f4343071c8b1 | 123 | if (SWSerial != nullptr) { |
charly | 0:f4343071c8b1 | 124 | // out = SWSerial->available(); |
charly | 0:f4343071c8b1 | 125 | out = 1; |
charly | 0:f4343071c8b1 | 126 | } else |
charly | 0:f4343071c8b1 | 127 | #endif |
charly | 0:f4343071c8b1 | 128 | if (HWSerial != nullptr) { |
charly | 0:f4343071c8b1 | 129 | // out = HWSerial->available(); |
charly | 0:f4343071c8b1 | 130 | out = 1; |
charly | 0:f4343071c8b1 | 131 | } |
charly | 0:f4343071c8b1 | 132 | |
charly | 0:f4343071c8b1 | 133 | return out; |
charly | 0:f4343071c8b1 | 134 | } |
charly | 0:f4343071c8b1 | 135 | |
charly | 0:f4343071c8b1 | 136 | __attribute__((weak)) |
charly | 0:f4343071c8b1 | 137 | void TMC2208Stepper::preWriteCommunication() { |
charly | 0:f4343071c8b1 | 138 | if (HWSerial != nullptr) { |
charly | 0:f4343071c8b1 | 139 | // if (sswitch != nullptr) |
charly | 0:f4343071c8b1 | 140 | // sswitch->active(); |
charly | 0:f4343071c8b1 | 141 | } |
charly | 0:f4343071c8b1 | 142 | } |
charly | 0:f4343071c8b1 | 143 | |
charly | 0:f4343071c8b1 | 144 | __attribute__((weak)) |
charly | 0:f4343071c8b1 | 145 | void TMC2208Stepper::preReadCommunication() { |
charly | 0:f4343071c8b1 | 146 | #if SW_CAPABLE_PLATFORM |
charly | 0:f4343071c8b1 | 147 | if (SWSerial != nullptr) { |
charly | 0:f4343071c8b1 | 148 | // SWSerial->listen(); |
charly | 0:f4343071c8b1 | 149 | } else |
charly | 0:f4343071c8b1 | 150 | #endif |
charly | 0:f4343071c8b1 | 151 | if (HWSerial != nullptr) { |
charly | 0:f4343071c8b1 | 152 | // if (sswitch != nullptr) |
charly | 0:f4343071c8b1 | 153 | // sswitch->active(); |
charly | 0:f4343071c8b1 | 154 | } |
charly | 0:f4343071c8b1 | 155 | } |
charly | 0:f4343071c8b1 | 156 | |
charly | 0:f4343071c8b1 | 157 | __attribute__((weak)) |
charly | 0:f4343071c8b1 | 158 | int16_t TMC2208Stepper::serial_read() { |
charly | 0:f4343071c8b1 | 159 | int16_t out = 0; |
charly | 0:f4343071c8b1 | 160 | int16_t count = 0; |
charly | 0:f4343071c8b1 | 161 | |
charly | 0:f4343071c8b1 | 162 | #if SW_CAPABLE_PLATFORM |
charly | 0:f4343071c8b1 | 163 | if (SWSerial != nullptr) { |
charly | 0:f4343071c8b1 | 164 | // out = SWSerial->read(); |
charly | 0:f4343071c8b1 | 165 | count = SWSerial->read(&out, 1); // read one character |
charly | 0:f4343071c8b1 | 166 | // if (SWSerial->readable()) { |
charly | 0:f4343071c8b1 | 167 | // SWSerial->read(&out, 1); // read one character |
charly | 0:f4343071c8b1 | 168 | // } |
charly | 0:f4343071c8b1 | 169 | |
charly | 0:f4343071c8b1 | 170 | } else |
charly | 0:f4343071c8b1 | 171 | #endif |
charly | 0:f4343071c8b1 | 172 | if (HWSerial != nullptr) { |
charly | 0:f4343071c8b1 | 173 | // out = HWSerial->read(); |
charly | 0:f4343071c8b1 | 174 | HWSerial->read(&out, 1); // read one character |
charly | 0:f4343071c8b1 | 175 | } |
charly | 0:f4343071c8b1 | 176 | if (count >= 1) { |
charly | 0:f4343071c8b1 | 177 | // printf("<%02X|",out); |
charly | 0:f4343071c8b1 | 178 | return out; |
charly | 0:f4343071c8b1 | 179 | } else { |
charly | 0:f4343071c8b1 | 180 | return -1; |
charly | 0:f4343071c8b1 | 181 | } |
charly | 0:f4343071c8b1 | 182 | } |
charly | 0:f4343071c8b1 | 183 | |
charly | 0:f4343071c8b1 | 184 | __attribute__((weak)) |
charly | 0:f4343071c8b1 | 185 | uint8_t TMC2208Stepper::serial_write(const uint8_t data) { |
charly | 0:f4343071c8b1 | 186 | int out = 0; |
charly | 0:f4343071c8b1 | 187 | #if SW_CAPABLE_PLATFORM |
charly | 0:f4343071c8b1 | 188 | if (SWSerial != nullptr) { |
charly | 0:f4343071c8b1 | 189 | // printf(">%02X|",data); |
charly | 0:f4343071c8b1 | 190 | return SWSerial->write(&data,1); |
charly | 0:f4343071c8b1 | 191 | } else |
charly | 0:f4343071c8b1 | 192 | #endif |
charly | 0:f4343071c8b1 | 193 | if (HWSerial != nullptr) { |
charly | 0:f4343071c8b1 | 194 | return HWSerial->write(&data,1); |
charly | 0:f4343071c8b1 | 195 | } |
charly | 0:f4343071c8b1 | 196 | |
charly | 0:f4343071c8b1 | 197 | return out; |
charly | 0:f4343071c8b1 | 198 | } |
charly | 0:f4343071c8b1 | 199 | |
charly | 0:f4343071c8b1 | 200 | __attribute__((weak)) |
charly | 0:f4343071c8b1 | 201 | void TMC2208Stepper::postWriteCommunication() {} |
charly | 0:f4343071c8b1 | 202 | |
charly | 0:f4343071c8b1 | 203 | __attribute__((weak)) |
charly | 0:f4343071c8b1 | 204 | void TMC2208Stepper::postReadCommunication() { |
charly | 0:f4343071c8b1 | 205 | #if SW_CAPABLE_PLATFORM |
charly | 0:f4343071c8b1 | 206 | // if (SWSerial != nullptr) { |
charly | 0:f4343071c8b1 | 207 | // SWSerial->end(); |
charly | 0:f4343071c8b1 | 208 | // } |
charly | 0:f4343071c8b1 | 209 | #endif |
charly | 0:f4343071c8b1 | 210 | } |
charly | 0:f4343071c8b1 | 211 | |
charly | 0:f4343071c8b1 | 212 | void TMC2208Stepper::write(uint8_t addr, uint32_t regVal) { |
charly | 0:f4343071c8b1 | 213 | uint8_t len = 7; |
charly | 0:f4343071c8b1 | 214 | addr |= TMC_WRITE; |
charly | 0:f4343071c8b1 | 215 | 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}; |
charly | 0:f4343071c8b1 | 216 | |
charly | 0:f4343071c8b1 | 217 | datagram[len] = calcCRC(datagram, len); |
charly | 0:f4343071c8b1 | 218 | |
charly | 0:f4343071c8b1 | 219 | preWriteCommunication(); |
charly | 0:f4343071c8b1 | 220 | |
charly | 0:f4343071c8b1 | 221 | for(uint8_t i=0; i<=len; i++) { |
charly | 0:f4343071c8b1 | 222 | bytesWritten += serial_write(datagram[i]); |
charly | 0:f4343071c8b1 | 223 | } |
charly | 0:f4343071c8b1 | 224 | postWriteCommunication(); |
charly | 0:f4343071c8b1 | 225 | |
charly | 0:f4343071c8b1 | 226 | //delay(replyDelay); |
charly | 0:f4343071c8b1 | 227 | wait_us(replyDelay*1000); |
charly | 0:f4343071c8b1 | 228 | } |
charly | 0:f4343071c8b1 | 229 | |
charly | 0:f4343071c8b1 | 230 | uint64_t TMC2208Stepper::_sendDatagram(uint8_t datagram[], const uint8_t len, uint16_t timeout) { |
charly | 0:f4343071c8b1 | 231 | |
charly | 0:f4343071c8b1 | 232 | // while (available() > 0) serial_read(); // Flush |
charly | 0:f4343071c8b1 | 233 | SWSerial->sync(); |
charly | 0:f4343071c8b1 | 234 | /* uint8_t dummy; |
charly | 0:f4343071c8b1 | 235 | while (SWSerial->readable()) { |
charly | 0:f4343071c8b1 | 236 | SWSerial->read(&dummy, 1); // read one character |
charly | 0:f4343071c8b1 | 237 | } |
charly | 0:f4343071c8b1 | 238 | */ |
charly | 0:f4343071c8b1 | 239 | /* uint8_t dummy; |
charly | 0:f4343071c8b1 | 240 | while (SWSerial->read(&dummy, 1) >= 0) { |
charly | 0:f4343071c8b1 | 241 | ; |
charly | 0:f4343071c8b1 | 242 | } |
charly | 0:f4343071c8b1 | 243 | */ |
charly | 0:f4343071c8b1 | 244 | |
charly | 0:f4343071c8b1 | 245 | #if defined(ARDUINO_ARCH_AVR) |
charly | 0:f4343071c8b1 | 246 | if (RXTX_pin > 0) { |
charly | 0:f4343071c8b1 | 247 | digitalWrite(RXTX_pin, HIGH); |
charly | 0:f4343071c8b1 | 248 | pinMode(RXTX_pin, OUTPUT); |
charly | 0:f4343071c8b1 | 249 | } |
charly | 0:f4343071c8b1 | 250 | #endif |
charly | 0:f4343071c8b1 | 251 | |
charly | 0:f4343071c8b1 | 252 | for(int i=0; i<=len; i++) serial_write(datagram[i]); |
charly | 0:f4343071c8b1 | 253 | |
charly | 0:f4343071c8b1 | 254 | #if defined(ARDUINO_ARCH_AVR) |
charly | 0:f4343071c8b1 | 255 | if (RXTX_pin > 0) { |
charly | 0:f4343071c8b1 | 256 | pinMode(RXTX_pin, INPUT_PULLUP); |
charly | 0:f4343071c8b1 | 257 | } |
charly | 0:f4343071c8b1 | 258 | #endif |
charly | 0:f4343071c8b1 | 259 | |
charly | 0:f4343071c8b1 | 260 | //delay(this->replyDelay); |
charly | 0:f4343071c8b1 | 261 | wait_us(this->replyDelay*1000); |
charly | 0:f4343071c8b1 | 262 | |
charly | 0:f4343071c8b1 | 263 | // scan for the rx frame and read it |
charly | 0:f4343071c8b1 | 264 | |
charly | 0:f4343071c8b1 | 265 | // uint32_t ms = millis(); |
charly | 0:f4343071c8b1 | 266 | uint32_t sync_target = (static_cast<uint32_t>(datagram[0])<<16) | 0xFF00 | datagram[2]; |
charly | 0:f4343071c8b1 | 267 | uint32_t sync = 0; |
charly | 0:f4343071c8b1 | 268 | |
charly | 0:f4343071c8b1 | 269 | |
charly | 0:f4343071c8b1 | 270 | do { |
charly | 0:f4343071c8b1 | 271 | /* uint32_t ms2 = millis(); |
charly | 0:f4343071c8b1 | 272 | if (ms2 != ms) { |
charly | 0:f4343071c8b1 | 273 | // 1ms tick |
charly | 0:f4343071c8b1 | 274 | ms = ms2; |
charly | 0:f4343071c8b1 | 275 | timeout--; |
charly | 0:f4343071c8b1 | 276 | } |
charly | 0:f4343071c8b1 | 277 | if (!timeout) return 0; |
charly | 0:f4343071c8b1 | 278 | */ |
charly | 0:f4343071c8b1 | 279 | int16_t res = serial_read(); |
charly | 0:f4343071c8b1 | 280 | if (res < 0) continue; |
charly | 0:f4343071c8b1 | 281 | |
charly | 0:f4343071c8b1 | 282 | sync <<= 8; |
charly | 0:f4343071c8b1 | 283 | sync |= res & 0xFF; |
charly | 0:f4343071c8b1 | 284 | sync &= 0xFFFFFF; |
charly | 0:f4343071c8b1 | 285 | |
charly | 0:f4343071c8b1 | 286 | } while (sync != sync_target); |
charly | 0:f4343071c8b1 | 287 | |
charly | 0:f4343071c8b1 | 288 | uint64_t out = sync; |
charly | 0:f4343071c8b1 | 289 | // ms = millis(); |
charly | 0:f4343071c8b1 | 290 | // timeout = this->abort_window; |
charly | 0:f4343071c8b1 | 291 | |
charly | 0:f4343071c8b1 | 292 | for(uint8_t i=0; i<5;) { |
charly | 0:f4343071c8b1 | 293 | /* uint32_t ms2 = millis(); |
charly | 0:f4343071c8b1 | 294 | if (ms2 != ms) { |
charly | 0:f4343071c8b1 | 295 | // 1ms tick |
charly | 0:f4343071c8b1 | 296 | ms = ms2; |
charly | 0:f4343071c8b1 | 297 | timeout--; |
charly | 0:f4343071c8b1 | 298 | } |
charly | 0:f4343071c8b1 | 299 | if (!timeout) return 0; |
charly | 0:f4343071c8b1 | 300 | */ |
charly | 0:f4343071c8b1 | 301 | int16_t res = serial_read(); |
charly | 0:f4343071c8b1 | 302 | if (res < 0) continue; |
charly | 0:f4343071c8b1 | 303 | |
charly | 0:f4343071c8b1 | 304 | out <<= 8; |
charly | 0:f4343071c8b1 | 305 | out |= res & 0xFF; |
charly | 0:f4343071c8b1 | 306 | |
charly | 0:f4343071c8b1 | 307 | i++; |
charly | 0:f4343071c8b1 | 308 | } |
charly | 0:f4343071c8b1 | 309 | |
charly | 0:f4343071c8b1 | 310 | #if defined(ARDUINO_ARCH_AVR) |
charly | 0:f4343071c8b1 | 311 | if (RXTX_pin > 0) { |
charly | 0:f4343071c8b1 | 312 | digitalWrite(RXTX_pin, HIGH); |
charly | 0:f4343071c8b1 | 313 | pinMode(RXTX_pin, OUTPUT); |
charly | 0:f4343071c8b1 | 314 | } |
charly | 0:f4343071c8b1 | 315 | #endif |
charly | 0:f4343071c8b1 | 316 | |
charly | 0:f4343071c8b1 | 317 | // while (available() > 0) serial_read(); // Flush |
charly | 0:f4343071c8b1 | 318 | SWSerial->sync(); |
charly | 0:f4343071c8b1 | 319 | |
charly | 0:f4343071c8b1 | 320 | return out; |
charly | 0:f4343071c8b1 | 321 | |
charly | 0:f4343071c8b1 | 322 | |
charly | 0:f4343071c8b1 | 323 | } |
charly | 0:f4343071c8b1 | 324 | |
charly | 0:f4343071c8b1 | 325 | uint32_t TMC2208Stepper::read(uint8_t addr) { |
charly | 0:f4343071c8b1 | 326 | constexpr uint8_t len = 3; |
charly | 0:f4343071c8b1 | 327 | addr |= TMC_READ; |
charly | 0:f4343071c8b1 | 328 | uint8_t datagram[] = {TMC2208_SYNC, slave_address, addr, 0x00}; |
charly | 0:f4343071c8b1 | 329 | datagram[len] = calcCRC(datagram, len); |
charly | 0:f4343071c8b1 | 330 | uint64_t out = 0x00000000UL; |
charly | 0:f4343071c8b1 | 331 | |
charly | 0:f4343071c8b1 | 332 | for (uint8_t i = 0; i < max_retries; i++) { |
charly | 0:f4343071c8b1 | 333 | preReadCommunication(); |
charly | 0:f4343071c8b1 | 334 | out = _sendDatagram(datagram, len, abort_window); |
charly | 0:f4343071c8b1 | 335 | postReadCommunication(); |
charly | 0:f4343071c8b1 | 336 | |
charly | 0:f4343071c8b1 | 337 | // delay(replyDelay); |
charly | 0:f4343071c8b1 | 338 | // wait_us(replyDelay*1000); |
charly | 0:f4343071c8b1 | 339 | |
charly | 0:f4343071c8b1 | 340 | CRCerror = false; |
charly | 0:f4343071c8b1 | 341 | uint8_t out_datagram[] = { |
charly | 0:f4343071c8b1 | 342 | static_cast<uint8_t>(out>>56), |
charly | 0:f4343071c8b1 | 343 | static_cast<uint8_t>(out>>48), |
charly | 0:f4343071c8b1 | 344 | static_cast<uint8_t>(out>>40), |
charly | 0:f4343071c8b1 | 345 | static_cast<uint8_t>(out>>32), |
charly | 0:f4343071c8b1 | 346 | static_cast<uint8_t>(out>>24), |
charly | 0:f4343071c8b1 | 347 | static_cast<uint8_t>(out>>16), |
charly | 0:f4343071c8b1 | 348 | static_cast<uint8_t>(out>> 8), |
charly | 0:f4343071c8b1 | 349 | static_cast<uint8_t>(out>> 0) |
charly | 0:f4343071c8b1 | 350 | }; |
charly | 0:f4343071c8b1 | 351 | uint8_t crc = calcCRC(out_datagram, 7); |
charly | 0:f4343071c8b1 | 352 | if ((crc != static_cast<uint8_t>(out)) || crc == 0 ) { |
charly | 0:f4343071c8b1 | 353 | CRCerror = true; |
charly | 0:f4343071c8b1 | 354 | out = 0; |
charly | 0:f4343071c8b1 | 355 | } else { |
charly | 0:f4343071c8b1 | 356 | break; |
charly | 0:f4343071c8b1 | 357 | } |
charly | 0:f4343071c8b1 | 358 | } |
charly | 0:f4343071c8b1 | 359 | |
charly | 0:f4343071c8b1 | 360 | return out>>8; |
charly | 0:f4343071c8b1 | 361 | } |
charly | 0:f4343071c8b1 | 362 | |
charly | 0:f4343071c8b1 | 363 | uint8_t TMC2208Stepper::IFCNT() { |
charly | 0:f4343071c8b1 | 364 | return read(IFCNT_t::address); |
charly | 0:f4343071c8b1 | 365 | } |
charly | 0:f4343071c8b1 | 366 | |
charly | 0:f4343071c8b1 | 367 | void TMC2208Stepper::SLAVECONF(uint16_t input) { |
charly | 0:f4343071c8b1 | 368 | SLAVECONF_register.sr = input&0xF00; |
charly | 0:f4343071c8b1 | 369 | write(SLAVECONF_register.address, SLAVECONF_register.sr); |
charly | 0:f4343071c8b1 | 370 | } |
charly | 0:f4343071c8b1 | 371 | uint16_t TMC2208Stepper::SLAVECONF() { |
charly | 0:f4343071c8b1 | 372 | return SLAVECONF_register.sr; |
charly | 0:f4343071c8b1 | 373 | } |
charly | 0:f4343071c8b1 | 374 | void TMC2208Stepper::senddelay(uint8_t B) { SLAVECONF_register.senddelay = B; write(SLAVECONF_register.address, SLAVECONF_register.sr); } |
charly | 0:f4343071c8b1 | 375 | uint8_t TMC2208Stepper::senddelay() { return SLAVECONF_register.senddelay; } |
charly | 0:f4343071c8b1 | 376 | |
charly | 0:f4343071c8b1 | 377 | void TMC2208Stepper::OTP_PROG(uint16_t input) { |
charly | 0:f4343071c8b1 | 378 | write(OTP_PROG_t::address, input); |
charly | 0:f4343071c8b1 | 379 | } |
charly | 0:f4343071c8b1 | 380 | |
charly | 0:f4343071c8b1 | 381 | uint32_t TMC2208Stepper::OTP_READ() { |
charly | 0:f4343071c8b1 | 382 | return read(OTP_READ_t::address); |
charly | 0:f4343071c8b1 | 383 | } |
charly | 0:f4343071c8b1 | 384 | |
charly | 0:f4343071c8b1 | 385 | uint32_t TMC2208Stepper::IOIN() { |
charly | 0:f4343071c8b1 | 386 | return read(TMC2208_n::IOIN_t::address); |
charly | 0:f4343071c8b1 | 387 | } |
charly | 0:f4343071c8b1 | 388 | bool TMC2208Stepper::enn() { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.enn; } |
charly | 0:f4343071c8b1 | 389 | bool TMC2208Stepper::ms1() { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.ms1; } |
charly | 0:f4343071c8b1 | 390 | bool TMC2208Stepper::ms2() { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.ms2; } |
charly | 0:f4343071c8b1 | 391 | bool TMC2208Stepper::diag() { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.diag; } |
charly | 0:f4343071c8b1 | 392 | bool TMC2208Stepper::pdn_uart() { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.pdn_uart; } |
charly | 0:f4343071c8b1 | 393 | bool TMC2208Stepper::step() { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.step; } |
charly | 0:f4343071c8b1 | 394 | bool TMC2208Stepper::sel_a() { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.sel_a; } |
charly | 0:f4343071c8b1 | 395 | bool TMC2208Stepper::dir() { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.dir; } |
charly | 0:f4343071c8b1 | 396 | uint8_t TMC2208Stepper::version() { TMC2208_n::IOIN_t r{0}; r.sr = IOIN(); return r.version; } |
charly | 0:f4343071c8b1 | 397 | |
charly | 0:f4343071c8b1 | 398 | /* |
charly | 0:f4343071c8b1 | 399 | uint32_t TMC2224Stepper::IOIN() { |
charly | 0:f4343071c8b1 | 400 | return read(TMC2224_n::IOIN_t::address); |
charly | 0:f4343071c8b1 | 401 | } |
charly | 0:f4343071c8b1 | 402 | bool TMC2224Stepper::enn() { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.enn; } |
charly | 0:f4343071c8b1 | 403 | bool TMC2224Stepper::ms1() { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.ms1; } |
charly | 0:f4343071c8b1 | 404 | bool TMC2224Stepper::ms2() { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.ms2; } |
charly | 0:f4343071c8b1 | 405 | bool TMC2224Stepper::pdn_uart() { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.pdn_uart; } |
charly | 0:f4343071c8b1 | 406 | bool TMC2224Stepper::spread() { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.spread; } |
charly | 0:f4343071c8b1 | 407 | bool TMC2224Stepper::step() { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.step; } |
charly | 0:f4343071c8b1 | 408 | bool TMC2224Stepper::sel_a() { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.sel_a; } |
charly | 0:f4343071c8b1 | 409 | bool TMC2224Stepper::dir() { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.dir; } |
charly | 0:f4343071c8b1 | 410 | uint8_t TMC2224Stepper::version() { TMC2224_n::IOIN_t r{0}; r.sr = IOIN(); return r.version; } |
charly | 0:f4343071c8b1 | 411 | */ |
charly | 0:f4343071c8b1 | 412 | uint16_t TMC2208Stepper::FACTORY_CONF() { |
charly | 0:f4343071c8b1 | 413 | return read(FACTORY_CONF_register.address); |
charly | 0:f4343071c8b1 | 414 | } |
charly | 0:f4343071c8b1 | 415 | void TMC2208Stepper::FACTORY_CONF(uint16_t input) { |
charly | 0:f4343071c8b1 | 416 | FACTORY_CONF_register.sr = input; |
charly | 0:f4343071c8b1 | 417 | write(FACTORY_CONF_register.address, FACTORY_CONF_register.sr); |
charly | 0:f4343071c8b1 | 418 | } |
charly | 0:f4343071c8b1 | 419 | void TMC2208Stepper::fclktrim(uint8_t B){ FACTORY_CONF_register.fclktrim = B; write(FACTORY_CONF_register.address, FACTORY_CONF_register.sr); } |
charly | 0:f4343071c8b1 | 420 | void TMC2208Stepper::ottrim(uint8_t B) { FACTORY_CONF_register.ottrim = B; write(FACTORY_CONF_register.address, FACTORY_CONF_register.sr); } |
charly | 0:f4343071c8b1 | 421 | uint8_t TMC2208Stepper::fclktrim() { FACTORY_CONF_t r{0}; r.sr = FACTORY_CONF(); return r.fclktrim; } |
charly | 0:f4343071c8b1 | 422 | uint8_t TMC2208Stepper::ottrim() { FACTORY_CONF_t r{0}; r.sr = FACTORY_CONF(); return r.ottrim; } |
charly | 0:f4343071c8b1 | 423 | |
charly | 0:f4343071c8b1 | 424 | void TMC2208Stepper::VACTUAL(uint32_t input) { |
charly | 0:f4343071c8b1 | 425 | VACTUAL_register.sr = input; |
charly | 0:f4343071c8b1 | 426 | write(VACTUAL_register.address, VACTUAL_register.sr); |
charly | 0:f4343071c8b1 | 427 | } |
charly | 0:f4343071c8b1 | 428 | uint32_t TMC2208Stepper::VACTUAL() { |
charly | 0:f4343071c8b1 | 429 | return VACTUAL_register.sr; |
charly | 0:f4343071c8b1 | 430 | } |
charly | 0:f4343071c8b1 | 431 | |
charly | 0:f4343071c8b1 | 432 | uint32_t TMC2208Stepper::PWM_SCALE() { |
charly | 0:f4343071c8b1 | 433 | return read(TMC2208_n::PWM_SCALE_t::address); |
charly | 0:f4343071c8b1 | 434 | } |
charly | 0:f4343071c8b1 | 435 | uint8_t TMC2208Stepper::pwm_scale_sum() { |
charly | 0:f4343071c8b1 | 436 | TMC2208_n::PWM_SCALE_t r{0}; |
charly | 0:f4343071c8b1 | 437 | r.sr = PWM_SCALE(); |
charly | 0:f4343071c8b1 | 438 | return r.pwm_scale_sum; |
charly | 0:f4343071c8b1 | 439 | } |
charly | 0:f4343071c8b1 | 440 | |
charly | 0:f4343071c8b1 | 441 | int16_t TMC2208Stepper::pwm_scale_auto() { |
charly | 0:f4343071c8b1 | 442 | TMC2208_n::PWM_SCALE_t r{0}; |
charly | 0:f4343071c8b1 | 443 | r.sr = PWM_SCALE(); |
charly | 0:f4343071c8b1 | 444 | return r.pwm_scale_auto; |
charly | 0:f4343071c8b1 | 445 | // Not two's complement? 9nth bit determines sign |
charly | 0:f4343071c8b1 | 446 | /* |
charly | 0:f4343071c8b1 | 447 | uint32_t d = PWM_SCALE(); |
charly | 0:f4343071c8b1 | 448 | int16_t response = (d>>PWM_SCALE_AUTO_bp)&0xFF; |
charly | 0:f4343071c8b1 | 449 | if (((d&PWM_SCALE_AUTO_bm) >> 24) & 0x1) return -response; |
charly | 0:f4343071c8b1 | 450 | else return response; |
charly | 0:f4343071c8b1 | 451 | */ |
charly | 0:f4343071c8b1 | 452 | } |
charly | 0:f4343071c8b1 | 453 | |
charly | 0:f4343071c8b1 | 454 | // R: PWM_AUTO |
charly | 0:f4343071c8b1 | 455 | uint32_t TMC2208Stepper::PWM_AUTO() { |
charly | 0:f4343071c8b1 | 456 | return read(PWM_AUTO_t::address); |
charly | 0:f4343071c8b1 | 457 | } |
charly | 0:f4343071c8b1 | 458 | uint8_t TMC2208Stepper::pwm_ofs_auto() { PWM_AUTO_t r{0}; r.sr = PWM_AUTO(); return r.pwm_ofs_auto; } |
charly | 0:f4343071c8b1 | 459 | uint8_t TMC2208Stepper::pwm_grad_auto() { PWM_AUTO_t r{0}; r.sr = PWM_AUTO(); return r.pwm_grad_auto; } |