Karl Zweimüller / Mbed OS TMC2209-Test2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   TMCStepper mRotaryEncoder-os

source/TMCStepper.cpp@0:3f4cfbeda9d3, 2021-02-04 (annotated)

Committer:
charly
Date:
Thu Feb 04 19:46:56 2021 +0000
Revision:
0:3f4cfbeda9d3
Working Version with TMC2209

Who changed what in which revision?

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