Library for Trinamic TMC2209 stepper modules to drive bipolar stepper motors. Ported and adapted from https://github.com/teemuatlut/TMCStepper
TMCStepper.cpp@2:b34e91b54373, 2021-12-02 (annotated)
- Committer:
- charly
- Date:
- Thu Dec 02 20:29:39 2021 +0000
- Revision:
- 2:b34e91b54373
- Parent:
- 0:f4343071c8b1
Changes from Original Project for TMC2209 integrated
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 | |
charly | 0:f4343071c8b1 | 3 | |
charly | 0:f4343071c8b1 | 4 | /* |
charly | 0:f4343071c8b1 | 5 | Requested current = mA = I_rms/1000 |
charly | 0:f4343071c8b1 | 6 | Equation for current: |
charly | 0:f4343071c8b1 | 7 | I_rms = (CS+1)/32 * V_fs/(R_sense+0.02ohm) * 1/sqrt(2) |
charly | 0:f4343071c8b1 | 8 | Solve for CS -> |
charly | 0:f4343071c8b1 | 9 | CS = 32*sqrt(2)*I_rms*(R_sense+0.02)/V_fs - 1 |
charly | 0:f4343071c8b1 | 10 | |
charly | 0:f4343071c8b1 | 11 | Example: |
charly | 0:f4343071c8b1 | 12 | vsense = 0b0 -> V_fs = 0.325V |
charly | 0:f4343071c8b1 | 13 | mA = 1640mA = I_rms/1000 = 1.64A |
charly | 0:f4343071c8b1 | 14 | R_sense = 0.10 Ohm |
charly | 0:f4343071c8b1 | 15 | -> |
charly | 0:f4343071c8b1 | 16 | CS = 32*sqrt(2)*1.64*(0.10+0.02)/0.325 - 1 = 26.4 |
charly | 0:f4343071c8b1 | 17 | CS = 26 |
charly | 0:f4343071c8b1 | 18 | */ |
charly | 0:f4343071c8b1 | 19 | |
charly | 0:f4343071c8b1 | 20 | uint16_t TMCStepper::cs2rms(uint8_t CS) { |
charly | 0:f4343071c8b1 | 21 | return (float)(CS+1)/32.0 * (vsense() ? 0.180 : 0.325)/(Rsense+0.02) / 1.41421 * 1000; |
charly | 0:f4343071c8b1 | 22 | } |
charly | 0:f4343071c8b1 | 23 | |
charly | 0:f4343071c8b1 | 24 | void TMCStepper::rms_current(uint16_t mA) { |
charly | 0:f4343071c8b1 | 25 | uint8_t CS = 32.0*1.41421*mA/1000.0*(Rsense+0.02)/0.325 - 1; |
charly | 0:f4343071c8b1 | 26 | // If Current Scale is too low, turn on high sensitivity R_sense and calculate again |
charly | 0:f4343071c8b1 | 27 | if (CS < 16) { |
charly | 0:f4343071c8b1 | 28 | vsense(true); |
charly | 0:f4343071c8b1 | 29 | CS = 32.0*1.41421*mA/1000.0*(Rsense+0.02)/0.180 - 1; |
charly | 0:f4343071c8b1 | 30 | } else { // If CS >= 16, turn off high_sense_r |
charly | 0:f4343071c8b1 | 31 | vsense(false); |
charly | 0:f4343071c8b1 | 32 | } |
charly | 0:f4343071c8b1 | 33 | |
charly | 0:f4343071c8b1 | 34 | if (CS > 31) |
charly | 0:f4343071c8b1 | 35 | CS = 31; |
charly | 0:f4343071c8b1 | 36 | |
charly | 0:f4343071c8b1 | 37 | irun(CS); |
charly | 0:f4343071c8b1 | 38 | ihold(CS*holdMultiplier); |
charly | 0:f4343071c8b1 | 39 | //val_mA = mA; |
charly | 0:f4343071c8b1 | 40 | } |
charly | 0:f4343071c8b1 | 41 | void TMCStepper::rms_current(uint16_t mA, float mult) { |
charly | 0:f4343071c8b1 | 42 | holdMultiplier = mult; |
charly | 0:f4343071c8b1 | 43 | rms_current(mA); |
charly | 0:f4343071c8b1 | 44 | } |
charly | 0:f4343071c8b1 | 45 | |
charly | 0:f4343071c8b1 | 46 | uint16_t TMCStepper::rms_current() { |
charly | 0:f4343071c8b1 | 47 | return cs2rms(irun()); |
charly | 0:f4343071c8b1 | 48 | } |
charly | 0:f4343071c8b1 | 49 | |
charly | 0:f4343071c8b1 | 50 | uint8_t TMCStepper::test_connection() { |
charly | 0:f4343071c8b1 | 51 | uint32_t drv_status = DRV_STATUS(); |
charly | 0:f4343071c8b1 | 52 | switch (drv_status) { |
charly | 0:f4343071c8b1 | 53 | case 0xFFFFFFFF: return 1; |
charly | 0:f4343071c8b1 | 54 | case 0: return 2; |
charly | 0:f4343071c8b1 | 55 | default: return 0; |
charly | 0:f4343071c8b1 | 56 | } |
charly | 0:f4343071c8b1 | 57 | } |
charly | 0:f4343071c8b1 | 58 | |
charly | 0:f4343071c8b1 | 59 | void TMCStepper::hysteresis_end(int8_t value) { hend(value+3); } |
charly | 0:f4343071c8b1 | 60 | int8_t TMCStepper::hysteresis_end() { return hend()-3; }; |
charly | 0:f4343071c8b1 | 61 | |
charly | 0:f4343071c8b1 | 62 | void TMCStepper::hysteresis_start(uint8_t value) { hstrt(value-1); } |
charly | 0:f4343071c8b1 | 63 | uint8_t TMCStepper::hysteresis_start() { return hstrt()+1; } |
charly | 0:f4343071c8b1 | 64 | |
charly | 0:f4343071c8b1 | 65 | void TMCStepper::microsteps(uint16_t ms) { |
charly | 0:f4343071c8b1 | 66 | switch(ms) { |
charly | 0:f4343071c8b1 | 67 | case 256: mres(0); break; |
charly | 0:f4343071c8b1 | 68 | case 128: mres(1); break; |
charly | 0:f4343071c8b1 | 69 | case 64: mres(2); break; |
charly | 0:f4343071c8b1 | 70 | case 32: mres(3); break; |
charly | 0:f4343071c8b1 | 71 | case 16: mres(4); break; |
charly | 0:f4343071c8b1 | 72 | case 8: mres(5); break; |
charly | 0:f4343071c8b1 | 73 | case 4: mres(6); break; |
charly | 0:f4343071c8b1 | 74 | case 2: mres(7); break; |
charly | 0:f4343071c8b1 | 75 | case 1: mres(8); break; |
charly | 0:f4343071c8b1 | 76 | default: break; |
charly | 0:f4343071c8b1 | 77 | } |
charly | 0:f4343071c8b1 | 78 | } |
charly | 0:f4343071c8b1 | 79 | |
charly | 0:f4343071c8b1 | 80 | uint16_t TMCStepper::microsteps() { |
charly | 0:f4343071c8b1 | 81 | switch(mres()) { |
charly | 0:f4343071c8b1 | 82 | case 0: return 256; |
charly | 0:f4343071c8b1 | 83 | case 1: return 128; |
charly | 0:f4343071c8b1 | 84 | case 2: return 64; |
charly | 0:f4343071c8b1 | 85 | case 3: return 32; |
charly | 0:f4343071c8b1 | 86 | case 4: return 16; |
charly | 0:f4343071c8b1 | 87 | case 5: return 8; |
charly | 0:f4343071c8b1 | 88 | case 6: return 4; |
charly | 0:f4343071c8b1 | 89 | case 7: return 2; |
charly | 0:f4343071c8b1 | 90 | case 8: return 1; |
charly | 0:f4343071c8b1 | 91 | } |
charly | 0:f4343071c8b1 | 92 | return 1; |
charly | 0:f4343071c8b1 | 93 | } |
charly | 0:f4343071c8b1 | 94 | |
charly | 0:f4343071c8b1 | 95 | void TMCStepper::blank_time(uint8_t value) { |
charly | 0:f4343071c8b1 | 96 | switch (value) { |
charly | 0:f4343071c8b1 | 97 | case 16: tbl(0b00); break; |
charly | 0:f4343071c8b1 | 98 | case 24: tbl(0b01); break; |
charly | 0:f4343071c8b1 | 99 | case 36: tbl(0b10); break; |
charly | 0:f4343071c8b1 | 100 | case 54: tbl(0b11); break; |
charly | 0:f4343071c8b1 | 101 | } |
charly | 0:f4343071c8b1 | 102 | } |
charly | 0:f4343071c8b1 | 103 | |
charly | 0:f4343071c8b1 | 104 | uint8_t TMCStepper::blank_time() { |
charly | 0:f4343071c8b1 | 105 | switch (tbl()) { |
charly | 0:f4343071c8b1 | 106 | case 0b00: return 16; |
charly | 0:f4343071c8b1 | 107 | case 0b01: return 24; |
charly | 0:f4343071c8b1 | 108 | case 0b10: return 36; |
charly | 0:f4343071c8b1 | 109 | case 0b11: return 54; |
charly | 0:f4343071c8b1 | 110 | } |
charly | 0:f4343071c8b1 | 111 | return 0; |
charly | 0:f4343071c8b1 | 112 | } |
charly | 0:f4343071c8b1 | 113 | |
charly | 0:f4343071c8b1 | 114 | /////////////////////////////////////////////////////////////////////////////////////// |
charly | 0:f4343071c8b1 | 115 | // R+C: GSTAT |
charly | 0:f4343071c8b1 | 116 | uint8_t TMCStepper::GSTAT() { return read(GSTAT_t::address); } |
charly | 0:f4343071c8b1 | 117 | void TMCStepper::GSTAT(uint8_t){ write(GSTAT_t::address, 0b111); } |
charly | 0:f4343071c8b1 | 118 | bool TMCStepper::reset() { GSTAT_t r; r.sr = GSTAT(); return r.reset; } |
charly | 0:f4343071c8b1 | 119 | bool TMCStepper::drv_err() { GSTAT_t r; r.sr = GSTAT(); return r.drv_err; } |
charly | 0:f4343071c8b1 | 120 | bool TMCStepper::uv_cp() { GSTAT_t r; r.sr = GSTAT(); return r.uv_cp; } |
charly | 0:f4343071c8b1 | 121 | /////////////////////////////////////////////////////////////////////////////////////// |
charly | 0:f4343071c8b1 | 122 | // W: TPOWERDOWN |
charly | 0:f4343071c8b1 | 123 | uint8_t TMCStepper::TPOWERDOWN() { return TPOWERDOWN_register.sr; } |
charly | 0:f4343071c8b1 | 124 | void TMCStepper::TPOWERDOWN(uint8_t input) { |
charly | 0:f4343071c8b1 | 125 | TPOWERDOWN_register.sr = input; |
charly | 0:f4343071c8b1 | 126 | write(TPOWERDOWN_register.address, TPOWERDOWN_register.sr); |
charly | 0:f4343071c8b1 | 127 | } |
charly | 0:f4343071c8b1 | 128 | /////////////////////////////////////////////////////////////////////////////////////// |
charly | 0:f4343071c8b1 | 129 | // R: TSTEP |
charly | 0:f4343071c8b1 | 130 | uint32_t TMCStepper::TSTEP() { return read(TSTEP_t::address); } |
charly | 0:f4343071c8b1 | 131 | /////////////////////////////////////////////////////////////////////////////////////// |
charly | 0:f4343071c8b1 | 132 | // W: TPWMTHRS |
charly | 0:f4343071c8b1 | 133 | uint32_t TMCStepper::TPWMTHRS() { return TPWMTHRS_register.sr; } |
charly | 0:f4343071c8b1 | 134 | void TMCStepper::TPWMTHRS(uint32_t input) { |
charly | 0:f4343071c8b1 | 135 | TPWMTHRS_register.sr = input; |
charly | 0:f4343071c8b1 | 136 | write(TPWMTHRS_register.address, TPWMTHRS_register.sr); |
charly | 0:f4343071c8b1 | 137 | } |
charly | 0:f4343071c8b1 | 138 | |
charly | 0:f4343071c8b1 | 139 | uint16_t TMCStepper::MSCNT() { |
charly | 0:f4343071c8b1 | 140 | return read(MSCNT_t::address); |
charly | 0:f4343071c8b1 | 141 | } |
charly | 0:f4343071c8b1 | 142 | |
charly | 0:f4343071c8b1 | 143 | uint32_t TMCStepper::MSCURACT() { return read(MSCURACT_t::address); } |
charly | 0:f4343071c8b1 | 144 | int16_t TMCStepper::cur_a() { |
charly | 0:f4343071c8b1 | 145 | MSCURACT_t r{0}; |
charly | 0:f4343071c8b1 | 146 | r.sr = MSCURACT(); |
charly | 0:f4343071c8b1 | 147 | int16_t value = r.cur_a; |
charly | 0:f4343071c8b1 | 148 | if (value > 255) value -= 512; |
charly | 0:f4343071c8b1 | 149 | return value; |
charly | 0:f4343071c8b1 | 150 | } |
charly | 0:f4343071c8b1 | 151 | int16_t TMCStepper::cur_b() { |
charly | 0:f4343071c8b1 | 152 | MSCURACT_t r{0}; |
charly | 0:f4343071c8b1 | 153 | r.sr = MSCURACT(); |
charly | 0:f4343071c8b1 | 154 | int16_t value = r.cur_b; |
charly | 0:f4343071c8b1 | 155 | if (value > 255) value -= 512; |
charly | 0:f4343071c8b1 | 156 | return value; |
charly | 0:f4343071c8b1 | 157 | } |