Karl Zweimüller / Mbed OS TMC2209-Test2

Testprogram for TMC2209-Library. Uses Speed-Control via VACTUAL instead of Step/Dir

Dependencies:   TMCStepper mRotaryEncoder-os

main.cpp@8:f44d70665051, 2021-04-22 (annotated)

Committer:
charly
Date:
Thu Apr 22 18:11:49 2021 +0000
Revision:
8:f44d70665051
Parent:
7:51cb60bf3e2d
Child:
9:babbfb61d347
work in progress

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charly 0:3f4cfbeda9d3 1 #include "mbed.h"
charly 0:3f4cfbeda9d3 2 #include "platform/mbed_thread.h"
charly 2:94c5b3f09463 3 #include "TMCStepper.h"
charly 4:12bfa2c1729f 4 #include "mRotaryEncoder.h"
charly 2:94c5b3f09463 5
charly 2:94c5b3f09463 6 /*
charly 2:94c5b3f09463 7 Testprogram for TMCStepper-Library
charly 2:94c5b3f09463 8 TMCStepper based on https://github.com/teemuatlut/TMCStepper for Arduino
charly 2:94c5b3f09463 9 by https://github.com/teemuatlut
charly 2:94c5b3f09463 10 +++++
charly 2:94c5b3f09463 11 Tested with https://github.com/bigtreetech/BIGTREETECH-TMC2209-V1.2
charly 2:94c5b3f09463 12 */
charly 0:3f4cfbeda9d3 13
charly 0:3f4cfbeda9d3 14
charly 8:f44d70665051 15 DigitalOut ledCW(LED1); // Show rotation clockwise
charly 5:7f250f463aa2 16 DigitalOut ledCCW(LED2); // Show rotation counterclockwise
charly 5:7f250f463aa2 17
charly 0:3f4cfbeda9d3 18 //Virtual serial port over USB with 15200 baud 8N1
charly 0:3f4cfbeda9d3 19 static BufferedSerial host(USBTX, USBRX,115200);
charly 0:3f4cfbeda9d3 20
charly 4:12bfa2c1729f 21 //mRotaryEncoder(PinName pinA, PinName pinB, PinName pinSW, PinMode pullMode=PullUp, int debounceTime_us=1000)
charly 5:7f250f463aa2 22 mRotaryEncoder wheel(p16, p17, p18,PullUp,3000); // default 1500
charly 4:12bfa2c1729f 23
charly 2:94c5b3f09463 24 // hardware parameters:
charly 0:3f4cfbeda9d3 25 // MOTOR Steps per Revolution ( 1/8 Microsteps, 200Steps per Rev / 1.8 degrees per FullStep)
charly 0:3f4cfbeda9d3 26 #define MSPR 1600
charly 0:3f4cfbeda9d3 27 // Gear Ratio
charly 0:3f4cfbeda9d3 28 #define GR 288
charly 0:3f4cfbeda9d3 29
charly 0:3f4cfbeda9d3 30 #define DRIVER_ADDRESS 0b00 // TMC2209 Driver address according to MS1 and MS2
charly 5:7f250f463aa2 31 #define R_SENSE 0.11f // R-Sense in OHM. Match to your driver
charly 8:f44d70665051 32 #define MICROSTEPS 256 // # of microsteps
charly 8:f44d70665051 33 #define RMSCURRENT 1000 // RMS current of Stepper Coil in mA
charly 8:f44d70665051 34 #define MAXSPEED 5000 // Maximaum speed (5000 with RMS800 @12V/0.6Amax)
charly 0:3f4cfbeda9d3 35
charly 2:94c5b3f09463 36 // A TMCStepper-object with UART and given address and R-Sense
charly 0:3f4cfbeda9d3 37 //RX, TX, RS, Addr
charly 0:3f4cfbeda9d3 38 TMC2209Stepper stepper(p14, p13, R_SENSE, DRIVER_ADDRESS);
charly 0:3f4cfbeda9d3 39
charly 6:6ad7bc10ac20 40 InterruptIn diag(p21);
charly 8:f44d70665051 41 DigitalOut enn(p22);
charly 6:6ad7bc10ac20 42
charly 5:7f250f463aa2 43 volatile bool enc_pressed = false; // Button of rotaryencoder was pressed
charly 5:7f250f463aa2 44 volatile bool enc_rotated = false; // rotary encoder was totaded left or right
charly 5:7f250f463aa2 45 volatile bool enc_action = false; // any change happened
charly 6:6ad7bc10ac20 46 volatile bool diag_event = false; // DIAG-Pin of TMC
charly 4:12bfa2c1729f 47 int lastGet;
charly 4:12bfa2c1729f 48 int thisGet;
charly 4:12bfa2c1729f 49
charly 4:12bfa2c1729f 50 //interrup-Handler for button on rotary-encoder
charly 4:12bfa2c1729f 51 void trigger_sw() {
charly 4:12bfa2c1729f 52 enc_pressed = true; // just set the flag, rest is done outside isr
charly 4:12bfa2c1729f 53 }
charly 4:12bfa2c1729f 54
charly 4:12bfa2c1729f 55 //interrup-Handler for rotary-encoder rotation
charly 4:12bfa2c1729f 56 void trigger_rotated() {
charly 4:12bfa2c1729f 57 enc_rotated = true; // just set the flag, rest is done outside isr
charly 4:12bfa2c1729f 58 }
charly 4:12bfa2c1729f 59
charly 6:6ad7bc10ac20 60 //interrup-Handler for TMC2209-DIAG-Pin
charly 6:6ad7bc10ac20 61 void trigger_diag() {
charly 6:6ad7bc10ac20 62 diag_event = true; // just set the flag, rest is done outside isr
charly 6:6ad7bc10ac20 63 }
charly 6:6ad7bc10ac20 64
charly 0:3f4cfbeda9d3 65 // Assumes little endian
charly 0:3f4cfbeda9d3 66 void printBits(size_t const size, void const * const ptr)
charly 0:3f4cfbeda9d3 67 {
charly 0:3f4cfbeda9d3 68 unsigned char *b = (unsigned char*) ptr;
charly 0:3f4cfbeda9d3 69 unsigned char byte;
charly 0:3f4cfbeda9d3 70 int i, j;
charly 0:3f4cfbeda9d3 71 // puts("#");
charly 0:3f4cfbeda9d3 72 for (i = size-1; i >= 0; i--) {
charly 0:3f4cfbeda9d3 73 for (j = 7; j >= 0; j--) {
charly 0:3f4cfbeda9d3 74 byte = (b[i] >> j) & 1;
charly 0:3f4cfbeda9d3 75 printf("%u", byte);
charly 0:3f4cfbeda9d3 76 }
charly 0:3f4cfbeda9d3 77 }
charly 0:3f4cfbeda9d3 78 // puts("#");
charly 0:3f4cfbeda9d3 79 }
charly 0:3f4cfbeda9d3 80
charly 0:3f4cfbeda9d3 81 int main()
charly 0:3f4cfbeda9d3 82 {
charly 0:3f4cfbeda9d3 83 printf("\r\nConnected to mbed\r\n");
charly 4:12bfa2c1729f 84
charly 5:7f250f463aa2 85 //Intitiallize RotaryEncoder
charly 4:12bfa2c1729f 86 // call trigger_sw() when button of rotary-encoder is pressed
charly 4:12bfa2c1729f 87 wheel.attachSW(&trigger_sw);
charly 4:12bfa2c1729f 88 // call trigger_rot() when the shaft is rotaded left or right
charly 4:12bfa2c1729f 89 wheel.attachROT(&trigger_rotated);
charly 4:12bfa2c1729f 90 lastGet = 0;
charly 6:6ad7bc10ac20 91 // set enc_rotated, so startup
charly 4:12bfa2c1729f 92 enc_rotated = true;
charly 5:7f250f463aa2 93 enc_action = true;
charly 5:7f250f463aa2 94 ledCW = 1;
charly 5:7f250f463aa2 95 ledCCW = 1;
charly 6:6ad7bc10ac20 96
charly 8:f44d70665051 97 // disable Driver
charly 8:f44d70665051 98 enn=1;
charly 5:7f250f463aa2 99
charly 5:7f250f463aa2 100 // Initialize Stepper
charly 5:7f250f463aa2 101 printf("connecting to TMC-Module...\r\n");
charly 2:94c5b3f09463 102 stepper.begin(); // UART: Init SW UART (if selected) with default baudrate
charly 5:7f250f463aa2 103 printf("TMC-Version: %02X\r\n",stepper.version());
charly 7:51cb60bf3e2d 104 stepper.push(); // initialize all registers??? required?
charly 7:51cb60bf3e2d 105
charly 2:94c5b3f09463 106 stepper.toff(3); // Enables driver in software - 3, 5 ????
charly 2:94c5b3f09463 107 stepper.rms_current(RMSCURRENT); // Set motor RMS current in mA / min 500 for 24V/speed:3000
charly 1:60419aa0c030 108 // 1110, 800
charly 5:7f250f463aa2 109 // working: 800 12V/0,6Amax, Speed up to 5200=4U/min
charly 5:7f250f463aa2 110
charly 0:3f4cfbeda9d3 111 stepper.microsteps(MICROSTEPS); // Set microsteps to 1:Fullstep ... 256: 1/256th
charly 1:60419aa0c030 112 stepper.en_spreadCycle(true); // Toggle spreadCycle on TMC2208/2209/2224: default false, true: much faster!!!!
charly 0:3f4cfbeda9d3 113 stepper.pwm_autoscale(true); // Needed for stealthChop
charly 0:3f4cfbeda9d3 114
charly 8:f44d70665051 115 // enable driver
charly 8:f44d70665051 116 enn = 0;
charly 8:f44d70665051 117
charly 6:6ad7bc10ac20 118 uint8_t gstat = stepper.GSTAT();
charly 6:6ad7bc10ac20 119 printf("GSTAT(): "); printBits(sizeof(gstat),&gstat);printf("\r\n");
charly 6:6ad7bc10ac20 120
charly 8:f44d70665051 121 uint32_t gconf = stepper.GCONF();
charly 8:f44d70665051 122 printf("GCONF(): "); printBits(sizeof(gconf),&gconf);printf("\r\n");
charly 8:f44d70665051 123
charly 5:7f250f463aa2 124 uint32_t status = stepper.DRV_STATUS();
charly 5:7f250f463aa2 125 printf("DRV_STATUS(): "); printBits(sizeof(status),&status);printf("\r\n");
charly 5:7f250f463aa2 126
charly 6:6ad7bc10ac20 127 uint32_t ioin = stepper.IOIN();
charly 6:6ad7bc10ac20 128 printf("IOIN(): "); printBits(sizeof(ioin),&ioin);printf("\r\n");
charly 6:6ad7bc10ac20 129
charly 8:f44d70665051 130 uint8_t ihold = stepper.ihold();
charly 8:f44d70665051 131 printf("IHOLD(): "); printBits(sizeof(ihold),&ihold);printf("\r\n");
charly 8:f44d70665051 132
charly 8:f44d70665051 133 uint8_t irun = stepper.irun();
charly 8:f44d70665051 134 printf("IRUN(): "); printBits(sizeof(irun),&irun);printf("\r\n");
charly 8:f44d70665051 135
charly 8:f44d70665051 136 uint8_t iholddelay = stepper.iholddelay();
charly 8:f44d70665051 137 printf("IHOLDDELAY(): "); printBits(sizeof(iholddelay),&iholddelay);printf("\r\n");
charly 8:f44d70665051 138
charly 8:f44d70665051 139 // do a peep by setting vactual to a too high speed
charly 8:f44d70665051 140
charly 8:f44d70665051 141 stepper.VACTUAL(50000*MICROSTEPS);
charly 8:f44d70665051 142 ThisThread::sleep_for(1s);
charly 8:f44d70665051 143
charly 8:f44d70665051 144 // initialize Automatic tunig (Chap 6.1)
charly 8:f44d70665051 145 stepper.VACTUAL(1);
charly 8:f44d70665051 146 stepper.VACTUAL(0);
charly 8:f44d70665051 147 ThisThread::sleep_for(100ms);
charly 8:f44d70665051 148 stepper.VACTUAL(500*MICROSTEPS);
charly 8:f44d70665051 149 ThisThread::sleep_for(2s);
charly 8:f44d70665051 150 stepper.VACTUAL(0);
charly 8:f44d70665051 151
charly 8:f44d70665051 152
charly 7:51cb60bf3e2d 153 diag.rise(&trigger_diag);
charly 7:51cb60bf3e2d 154
charly 5:7f250f463aa2 155 //bool shaft = false; //direction CW or CCW
charly 1:60419aa0c030 156
charly 0:3f4cfbeda9d3 157 while(1) {
charly 5:7f250f463aa2 158 /* Spped-UP/Down-Cycles
charly 0:3f4cfbeda9d3 159 // printf("TSTEP(): %i\r\n", stepper.TSTEP());
charly 0:3f4cfbeda9d3 160 uint32_t status = stepper.DRV_STATUS();
charly 0:3f4cfbeda9d3 161 printf("DRV_STATUS(): "); printBits(sizeof(status),&status);printf("\r\n");
charly 0:3f4cfbeda9d3 162 uint32_t ioin = stepper.IOIN();
charly 0:3f4cfbeda9d3 163 printf("IOIN(): "); printBits(sizeof(ioin),&ioin);printf("\r\n");
charly 0:3f4cfbeda9d3 164 // uint32_t otp = stepper.OTP_READ();
charly 0:3f4cfbeda9d3 165 // printf("OTP_READ(): ");printBits(sizeof(otp),&otp);printf("\r\n");
charly 0:3f4cfbeda9d3 166
charly 1:60419aa0c030 167 printf("VACTUAL(): %zu \r\n", stepper.VACTUAL());
charly 0:3f4cfbeda9d3 168 // increase
charly 1:60419aa0c030 169 uint32_t maxspeed = 3000; //max 3400 or 3000
charly 1:60419aa0c030 170 uint32_t actspeed = 0;
charly 0:3f4cfbeda9d3 171 while (actspeed < maxspeed) {
charly 0:3f4cfbeda9d3 172 actspeed += 200;
charly 1:60419aa0c030 173 if (actspeed > maxspeed) {
charly 1:60419aa0c030 174 actspeed = maxspeed;
charly 1:60419aa0c030 175 }
charly 3:209a9c414f54 176 printf("actspeed: %i",actspeed);
charly 0:3f4cfbeda9d3 177 stepper.VACTUAL(actspeed*MICROSTEPS);// Set Speed to value
charly 1:60419aa0c030 178 ThisThread::sleep_for(25ms); //wait
charly 0:3f4cfbeda9d3 179 }
charly 0:3f4cfbeda9d3 180 printf("VACTUAL(): %zu \r\n", stepper.VACTUAL());
charly 3:209a9c414f54 181 ThisThread::sleep_for(5s);
charly 0:3f4cfbeda9d3 182 // decrease
charly 0:3f4cfbeda9d3 183 maxspeed = 0;
charly 0:3f4cfbeda9d3 184 while (actspeed > maxspeed) {
charly 0:3f4cfbeda9d3 185 actspeed -= 200;
charly 1:60419aa0c030 186 if (actspeed < 0) {
charly 1:60419aa0c030 187 actspeed = 0;
charly 1:60419aa0c030 188 }
charly 3:209a9c414f54 189 printf("actspeed: %i",actspeed);
charly 0:3f4cfbeda9d3 190 stepper.VACTUAL(actspeed*MICROSTEPS);// Set Speed to value
charly 1:60419aa0c030 191 ThisThread::sleep_for(25ms); //wait
charly 0:3f4cfbeda9d3 192 }
charly 1:60419aa0c030 193
charly 1:60419aa0c030 194 // stepper.VACTUAL(400*MICROSTEPS);// Set Speed to value
charly 0:3f4cfbeda9d3 195 ThisThread::sleep_for(5s); //wait
charly 1:60419aa0c030 196 // inverse direction
charly 1:60419aa0c030 197 shaft = !shaft;
charly 1:60419aa0c030 198 stepper.shaft(shaft);
charly 2:94c5b3f09463 199 // Read Interace-Count
charly 2:94c5b3f09463 200 printf("IFCNT(): %zu \r\n",stepper.IFCNT());
charly 1:60419aa0c030 201 printf("...\r\n");
charly 4:12bfa2c1729f 202 */
charly 5:7f250f463aa2 203 ////// Control motor-speed by rotary-encoder
charly 5:7f250f463aa2 204
charly 6:6ad7bc10ac20 205 // DIAG-PIN showed Error-Condition?
charly 6:6ad7bc10ac20 206 if (diag_event) {
charly 6:6ad7bc10ac20 207 diag_event = false;
charly 6:6ad7bc10ac20 208 printf("DIAG occured!\r\n");
charly 6:6ad7bc10ac20 209 gstat = stepper.GSTAT();
charly 6:6ad7bc10ac20 210 printf("GSTAT(): "); printBits(sizeof(gstat),&gstat);printf("\r\n");
charly 6:6ad7bc10ac20 211 status = stepper.DRV_STATUS();
charly 6:6ad7bc10ac20 212 printf("DRV_STATUS(): "); printBits(sizeof(status),&status);printf("\r\n");
charly 6:6ad7bc10ac20 213 //safty turn off friver
charly 6:6ad7bc10ac20 214 printf("Shutting Down Motordriver...\r\n");
charly 8:f44d70665051 215 enn=1;
charly 6:6ad7bc10ac20 216 stepper.toff(0);
charly 6:6ad7bc10ac20 217 status = stepper.DRV_STATUS();
charly 6:6ad7bc10ac20 218 printf("DRV_STATUS(): "); printBits(sizeof(status),&status);printf("\r\n");
charly 6:6ad7bc10ac20 219 ioin = stepper.IOIN();
charly 6:6ad7bc10ac20 220 printf("IOIN(): "); printBits(sizeof(ioin),&ioin);printf("\r\n");
charly 8:f44d70665051 221 ihold = stepper.ihold();
charly 8:f44d70665051 222 printf("IHOLD(): "); printBits(sizeof(ihold),&ihold);printf("\r\n");
charly 8:f44d70665051 223 irun = stepper.irun();
charly 8:f44d70665051 224 printf("IRUN(): "); printBits(sizeof(irun),&irun);printf("\r\n");
charly 8:f44d70665051 225 iholddelay = stepper.iholddelay();
charly 8:f44d70665051 226 printf("IHOLDDELAY(): "); printBits(sizeof(iholddelay),&iholddelay);printf("\r\n");
charly 6:6ad7bc10ac20 227 printf("stopping programm - manual RESET required!!!!!!\r\n");
charly 8:f44d70665051 228 while (1) {
charly 8:f44d70665051 229 ledCW = 1;
charly 8:f44d70665051 230 ledCCW = 1;
charly 8:f44d70665051 231 ThisThread::sleep_for(200ms);
charly 8:f44d70665051 232 ledCW = 0;
charly 8:f44d70665051 233 ledCCW = 0;
charly 8:f44d70665051 234 ThisThread::sleep_for(200ms);
charly 8:f44d70665051 235 };
charly 6:6ad7bc10ac20 236
charly 6:6ad7bc10ac20 237 }
charly 6:6ad7bc10ac20 238
charly 4:12bfa2c1729f 239 // shaft has been rotated?
charly 4:12bfa2c1729f 240 if (enc_rotated) {
charly 4:12bfa2c1729f 241 enc_rotated = false;
charly 5:7f250f463aa2 242 enc_action = true;
charly 4:12bfa2c1729f 243 thisGet = wheel.Get();
charly 5:7f250f463aa2 244 if (thisGet*100 > MAXSPEED) { //on upper limit?
charly 5:7f250f463aa2 245 wheel.Set( MAXSPEED/100);
charly 5:7f250f463aa2 246 thisGet = wheel.Get();
charly 5:7f250f463aa2 247 }
charly 5:7f250f463aa2 248 if (thisGet*100 < MAXSPEED*(-1)) { //on lower limit?
charly 5:7f250f463aa2 249 wheel.Set( MAXSPEED*(-1)/100);
charly 5:7f250f463aa2 250 thisGet = wheel.Get();
charly 5:7f250f463aa2 251 }
charly 4:12bfa2c1729f 252 stepper.VACTUAL(thisGet*100*MICROSTEPS);// Set Speed to value
charly 4:12bfa2c1729f 253 printf("actspeed: %i\r\n",thisGet*100);
charly 5:7f250f463aa2 254
charly 4:12bfa2c1729f 255 }
charly 4:12bfa2c1729f 256 // Button pressed?
charly 4:12bfa2c1729f 257 if (enc_pressed) {
charly 4:12bfa2c1729f 258 enc_pressed = false;
charly 5:7f250f463aa2 259 enc_action = true;
charly 4:12bfa2c1729f 260 wheel.Set(0);
charly 4:12bfa2c1729f 261 thisGet = wheel.Get();
charly 4:12bfa2c1729f 262 stepper.VACTUAL(thisGet*100*MICROSTEPS);// Set Speed to value
charly 4:12bfa2c1729f 263 printf("actspeed: %i\r\n",thisGet*100);
charly 7:51cb60bf3e2d 264
charly 7:51cb60bf3e2d 265 gstat = stepper.GSTAT();
charly 7:51cb60bf3e2d 266 printf("GSTAT(): "); printBits(sizeof(gstat),&gstat);printf("\r\n");
charly 8:f44d70665051 267 gconf = stepper.GCONF();
charly 8:f44d70665051 268 printf("GCONF(): "); printBits(sizeof(gconf),&gconf);printf("\r\n");
charly 7:51cb60bf3e2d 269 status = stepper.DRV_STATUS();
charly 7:51cb60bf3e2d 270 printf("DRV_STATUS(): "); printBits(sizeof(status),&status);printf("\r\n");
charly 7:51cb60bf3e2d 271 ioin = stepper.IOIN();
charly 7:51cb60bf3e2d 272 printf("IOIN(): "); printBits(sizeof(ioin),&ioin);printf("\r\n");
charly 8:f44d70665051 273 ihold = stepper.ihold();
charly 8:f44d70665051 274 printf("IHOLD(): "); printBits(sizeof(ihold),&ihold);printf("\r\n");
charly 8:f44d70665051 275 irun = stepper.irun();
charly 8:f44d70665051 276 printf("IRUN(): "); printBits(sizeof(irun),&irun);printf("\r\n");
charly 8:f44d70665051 277 iholddelay = stepper.iholddelay();
charly 8:f44d70665051 278 printf("IHOLDDELAY(): "); printBits(sizeof(iholddelay),&iholddelay);printf("\r\n");
charly 4:12bfa2c1729f 279
charly 4:12bfa2c1729f 280 }
charly 5:7f250f463aa2 281 // anything changed?
charly 5:7f250f463aa2 282 if (enc_action) {
charly 5:7f250f463aa2 283 enc_action = false;
charly 5:7f250f463aa2 284 // show direction of motor on leds
charly 5:7f250f463aa2 285 if (thisGet > 0) {
charly 5:7f250f463aa2 286 ledCW = 1;
charly 5:7f250f463aa2 287 ledCCW= 0;
charly 5:7f250f463aa2 288 }
charly 5:7f250f463aa2 289 if (thisGet < 0) {
charly 5:7f250f463aa2 290 ledCW = 0;
charly 5:7f250f463aa2 291 ledCCW= 1;
charly 5:7f250f463aa2 292 }
charly 5:7f250f463aa2 293 if (thisGet == 0) {
charly 5:7f250f463aa2 294 ledCW = 1;
charly 5:7f250f463aa2 295 ledCCW= 1;
charly 5:7f250f463aa2 296 }
charly 5:7f250f463aa2 297 }
charly 4:12bfa2c1729f 298 } // while 1
charly 0:3f4cfbeda9d3 299 }