Karl Zweimüller / TMCStepper

Library for Trinamic TMC2209 stepper modules to drive bipolar stepper motors. Ported and adapted from https://github.com/teemuatlut/TMCStepper

Dependents:   TMC2209-Test2

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?

UserRevisionLine numberNew 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 }