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@7:51cb60bf3e2d, 2021-03-22 (annotated)

Committer:
charly
Date:
Mon Mar 22 21:38:03 2021 +0000
Revision:
7:51cb60bf3e2d
Parent:
6:6ad7bc10ac20
Child:
8:f44d70665051
push()

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 5:7f250f463aa2 15 DigitalOut ledCW(LED1); // Show totation 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 2:94c5b3f09463 32 #define MICROSTEPS 128 // # of microsteps
charly 5:7f250f463aa2 33 #define RMSCURRENT 800 // RMS current of Stepper Coil in mA
charly 5:7f250f463aa2 34 #define MAXSPEED 5000 // Maximaum speed (5000 with RMS800 @12V/06Amax)
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 6:6ad7bc10ac20 41
charly 5:7f250f463aa2 42 volatile bool enc_pressed = false; // Button of rotaryencoder was pressed
charly 5:7f250f463aa2 43 volatile bool enc_rotated = false; // rotary encoder was totaded left or right
charly 5:7f250f463aa2 44 volatile bool enc_action = false; // any change happened
charly 6:6ad7bc10ac20 45 volatile bool diag_event = false; // DIAG-Pin of TMC
charly 4:12bfa2c1729f 46 int lastGet;
charly 4:12bfa2c1729f 47 int thisGet;
charly 4:12bfa2c1729f 48
charly 4:12bfa2c1729f 49 //interrup-Handler for button on rotary-encoder
charly 4:12bfa2c1729f 50 void trigger_sw() {
charly 4:12bfa2c1729f 51 enc_pressed = true; // just set the flag, rest is done outside isr
charly 4:12bfa2c1729f 52 }
charly 4:12bfa2c1729f 53
charly 4:12bfa2c1729f 54 //interrup-Handler for rotary-encoder rotation
charly 4:12bfa2c1729f 55 void trigger_rotated() {
charly 4:12bfa2c1729f 56 enc_rotated = true; // just set the flag, rest is done outside isr
charly 4:12bfa2c1729f 57 }
charly 4:12bfa2c1729f 58
charly 6:6ad7bc10ac20 59 //interrup-Handler for TMC2209-DIAG-Pin
charly 6:6ad7bc10ac20 60 void trigger_diag() {
charly 6:6ad7bc10ac20 61 diag_event = true; // just set the flag, rest is done outside isr
charly 6:6ad7bc10ac20 62 }
charly 6:6ad7bc10ac20 63
charly 0:3f4cfbeda9d3 64 // Assumes little endian
charly 0:3f4cfbeda9d3 65 void printBits(size_t const size, void const * const ptr)
charly 0:3f4cfbeda9d3 66 {
charly 0:3f4cfbeda9d3 67 unsigned char *b = (unsigned char*) ptr;
charly 0:3f4cfbeda9d3 68 unsigned char byte;
charly 0:3f4cfbeda9d3 69 int i, j;
charly 0:3f4cfbeda9d3 70 // puts("#");
charly 0:3f4cfbeda9d3 71 for (i = size-1; i >= 0; i--) {
charly 0:3f4cfbeda9d3 72 for (j = 7; j >= 0; j--) {
charly 0:3f4cfbeda9d3 73 byte = (b[i] >> j) & 1;
charly 0:3f4cfbeda9d3 74 printf("%u", byte);
charly 0:3f4cfbeda9d3 75 }
charly 0:3f4cfbeda9d3 76 }
charly 0:3f4cfbeda9d3 77 // puts("#");
charly 0:3f4cfbeda9d3 78 }
charly 0:3f4cfbeda9d3 79
charly 0:3f4cfbeda9d3 80 int main()
charly 0:3f4cfbeda9d3 81 {
charly 0:3f4cfbeda9d3 82 printf("\r\nConnected to mbed\r\n");
charly 4:12bfa2c1729f 83
charly 5:7f250f463aa2 84 //Intitiallize RotaryEncoder
charly 4:12bfa2c1729f 85 // call trigger_sw() when button of rotary-encoder is pressed
charly 4:12bfa2c1729f 86 wheel.attachSW(&trigger_sw);
charly 4:12bfa2c1729f 87 // call trigger_rot() when the shaft is rotaded left or right
charly 4:12bfa2c1729f 88 wheel.attachROT(&trigger_rotated);
charly 4:12bfa2c1729f 89 lastGet = 0;
charly 6:6ad7bc10ac20 90 // set enc_rotated, so startup
charly 4:12bfa2c1729f 91 enc_rotated = true;
charly 5:7f250f463aa2 92 enc_action = true;
charly 5:7f250f463aa2 93 ledCW = 1;
charly 5:7f250f463aa2 94 ledCCW = 1;
charly 6:6ad7bc10ac20 95
charly 5:7f250f463aa2 96
charly 5:7f250f463aa2 97 // Initialize Stepper
charly 5:7f250f463aa2 98 printf("connecting to TMC-Module...\r\n");
charly 2:94c5b3f09463 99 stepper.begin(); // UART: Init SW UART (if selected) with default baudrate
charly 5:7f250f463aa2 100 printf("TMC-Version: %02X\r\n",stepper.version());
charly 7:51cb60bf3e2d 101 stepper.push(); // initialize all registers??? required?
charly 7:51cb60bf3e2d 102
charly 2:94c5b3f09463 103 stepper.toff(3); // Enables driver in software - 3, 5 ????
charly 2:94c5b3f09463 104 stepper.rms_current(RMSCURRENT); // Set motor RMS current in mA / min 500 for 24V/speed:3000
charly 1:60419aa0c030 105 // 1110, 800
charly 5:7f250f463aa2 106 // working: 800 12V/0,6Amax, Speed up to 5200=4U/min
charly 5:7f250f463aa2 107
charly 0:3f4cfbeda9d3 108 stepper.microsteps(MICROSTEPS); // Set microsteps to 1:Fullstep ... 256: 1/256th
charly 1:60419aa0c030 109 stepper.en_spreadCycle(true); // Toggle spreadCycle on TMC2208/2209/2224: default false, true: much faster!!!!
charly 0:3f4cfbeda9d3 110 stepper.pwm_autoscale(true); // Needed for stealthChop
charly 0:3f4cfbeda9d3 111
charly 6:6ad7bc10ac20 112 uint8_t gstat = stepper.GSTAT();
charly 6:6ad7bc10ac20 113 printf("GSTAT(): "); printBits(sizeof(gstat),&gstat);printf("\r\n");
charly 6:6ad7bc10ac20 114
charly 5:7f250f463aa2 115 uint32_t status = stepper.DRV_STATUS();
charly 5:7f250f463aa2 116 printf("DRV_STATUS(): "); printBits(sizeof(status),&status);printf("\r\n");
charly 5:7f250f463aa2 117
charly 6:6ad7bc10ac20 118 uint32_t ioin = stepper.IOIN();
charly 6:6ad7bc10ac20 119 printf("IOIN(): "); printBits(sizeof(ioin),&ioin);printf("\r\n");
charly 6:6ad7bc10ac20 120
charly 7:51cb60bf3e2d 121 diag.rise(&trigger_diag);
charly 7:51cb60bf3e2d 122
charly 5:7f250f463aa2 123 //bool shaft = false; //direction CW or CCW
charly 1:60419aa0c030 124
charly 0:3f4cfbeda9d3 125 while(1) {
charly 5:7f250f463aa2 126 /* Spped-UP/Down-Cycles
charly 0:3f4cfbeda9d3 127 // printf("TSTEP(): %i\r\n", stepper.TSTEP());
charly 0:3f4cfbeda9d3 128 uint32_t status = stepper.DRV_STATUS();
charly 0:3f4cfbeda9d3 129 printf("DRV_STATUS(): "); printBits(sizeof(status),&status);printf("\r\n");
charly 0:3f4cfbeda9d3 130 uint32_t ioin = stepper.IOIN();
charly 0:3f4cfbeda9d3 131 printf("IOIN(): "); printBits(sizeof(ioin),&ioin);printf("\r\n");
charly 0:3f4cfbeda9d3 132 // uint32_t otp = stepper.OTP_READ();
charly 0:3f4cfbeda9d3 133 // printf("OTP_READ(): ");printBits(sizeof(otp),&otp);printf("\r\n");
charly 0:3f4cfbeda9d3 134
charly 1:60419aa0c030 135 printf("VACTUAL(): %zu \r\n", stepper.VACTUAL());
charly 0:3f4cfbeda9d3 136 // increase
charly 1:60419aa0c030 137 uint32_t maxspeed = 3000; //max 3400 or 3000
charly 1:60419aa0c030 138 uint32_t actspeed = 0;
charly 0:3f4cfbeda9d3 139 while (actspeed < maxspeed) {
charly 0:3f4cfbeda9d3 140 actspeed += 200;
charly 1:60419aa0c030 141 if (actspeed > maxspeed) {
charly 1:60419aa0c030 142 actspeed = maxspeed;
charly 1:60419aa0c030 143 }
charly 3:209a9c414f54 144 printf("actspeed: %i",actspeed);
charly 0:3f4cfbeda9d3 145 stepper.VACTUAL(actspeed*MICROSTEPS);// Set Speed to value
charly 1:60419aa0c030 146 ThisThread::sleep_for(25ms); //wait
charly 0:3f4cfbeda9d3 147 }
charly 0:3f4cfbeda9d3 148 printf("VACTUAL(): %zu \r\n", stepper.VACTUAL());
charly 3:209a9c414f54 149 ThisThread::sleep_for(5s);
charly 0:3f4cfbeda9d3 150 // decrease
charly 0:3f4cfbeda9d3 151 maxspeed = 0;
charly 0:3f4cfbeda9d3 152 while (actspeed > maxspeed) {
charly 0:3f4cfbeda9d3 153 actspeed -= 200;
charly 1:60419aa0c030 154 if (actspeed < 0) {
charly 1:60419aa0c030 155 actspeed = 0;
charly 1:60419aa0c030 156 }
charly 3:209a9c414f54 157 printf("actspeed: %i",actspeed);
charly 0:3f4cfbeda9d3 158 stepper.VACTUAL(actspeed*MICROSTEPS);// Set Speed to value
charly 1:60419aa0c030 159 ThisThread::sleep_for(25ms); //wait
charly 0:3f4cfbeda9d3 160 }
charly 1:60419aa0c030 161
charly 1:60419aa0c030 162 // stepper.VACTUAL(400*MICROSTEPS);// Set Speed to value
charly 0:3f4cfbeda9d3 163 ThisThread::sleep_for(5s); //wait
charly 1:60419aa0c030 164 // inverse direction
charly 1:60419aa0c030 165 shaft = !shaft;
charly 1:60419aa0c030 166 stepper.shaft(shaft);
charly 2:94c5b3f09463 167 // Read Interace-Count
charly 2:94c5b3f09463 168 printf("IFCNT(): %zu \r\n",stepper.IFCNT());
charly 1:60419aa0c030 169 printf("...\r\n");
charly 4:12bfa2c1729f 170 */
charly 5:7f250f463aa2 171 ////// Control motor-speed by rotary-encoder
charly 5:7f250f463aa2 172
charly 6:6ad7bc10ac20 173 // DIAG-PIN showed Error-Condition?
charly 6:6ad7bc10ac20 174 if (diag_event) {
charly 6:6ad7bc10ac20 175 diag_event = false;
charly 6:6ad7bc10ac20 176 printf("DIAG occured!\r\n");
charly 6:6ad7bc10ac20 177 gstat = stepper.GSTAT();
charly 6:6ad7bc10ac20 178 printf("GSTAT(): "); printBits(sizeof(gstat),&gstat);printf("\r\n");
charly 6:6ad7bc10ac20 179 status = stepper.DRV_STATUS();
charly 6:6ad7bc10ac20 180 printf("DRV_STATUS(): "); printBits(sizeof(status),&status);printf("\r\n");
charly 6:6ad7bc10ac20 181 //safty turn off friver
charly 6:6ad7bc10ac20 182 printf("Shutting Down Motordriver...\r\n");
charly 6:6ad7bc10ac20 183 stepper.toff(0);
charly 6:6ad7bc10ac20 184 status = stepper.DRV_STATUS();
charly 6:6ad7bc10ac20 185 printf("DRV_STATUS(): "); printBits(sizeof(status),&status);printf("\r\n");
charly 6:6ad7bc10ac20 186 ioin = stepper.IOIN();
charly 6:6ad7bc10ac20 187 printf("IOIN(): "); printBits(sizeof(ioin),&ioin);printf("\r\n");
charly 6:6ad7bc10ac20 188 printf("stopping programm - manual RESET required!!!!!!\r\n");
charly 6:6ad7bc10ac20 189 while (1) {};
charly 6:6ad7bc10ac20 190
charly 6:6ad7bc10ac20 191 }
charly 6:6ad7bc10ac20 192
charly 4:12bfa2c1729f 193 // shaft has been rotated?
charly 4:12bfa2c1729f 194 if (enc_rotated) {
charly 4:12bfa2c1729f 195 enc_rotated = false;
charly 5:7f250f463aa2 196 enc_action = true;
charly 4:12bfa2c1729f 197 thisGet = wheel.Get();
charly 5:7f250f463aa2 198 if (thisGet*100 > MAXSPEED) { //on upper limit?
charly 5:7f250f463aa2 199 wheel.Set( MAXSPEED/100);
charly 5:7f250f463aa2 200 thisGet = wheel.Get();
charly 5:7f250f463aa2 201 }
charly 5:7f250f463aa2 202 if (thisGet*100 < MAXSPEED*(-1)) { //on lower limit?
charly 5:7f250f463aa2 203 wheel.Set( MAXSPEED*(-1)/100);
charly 5:7f250f463aa2 204 thisGet = wheel.Get();
charly 5:7f250f463aa2 205 }
charly 4:12bfa2c1729f 206 stepper.VACTUAL(thisGet*100*MICROSTEPS);// Set Speed to value
charly 4:12bfa2c1729f 207 printf("actspeed: %i\r\n",thisGet*100);
charly 5:7f250f463aa2 208
charly 4:12bfa2c1729f 209 }
charly 4:12bfa2c1729f 210 // Button pressed?
charly 4:12bfa2c1729f 211 if (enc_pressed) {
charly 4:12bfa2c1729f 212 enc_pressed = false;
charly 5:7f250f463aa2 213 enc_action = true;
charly 4:12bfa2c1729f 214 wheel.Set(0);
charly 4:12bfa2c1729f 215 thisGet = wheel.Get();
charly 4:12bfa2c1729f 216 stepper.VACTUAL(thisGet*100*MICROSTEPS);// Set Speed to value
charly 4:12bfa2c1729f 217 printf("actspeed: %i\r\n",thisGet*100);
charly 7:51cb60bf3e2d 218
charly 7:51cb60bf3e2d 219 gstat = stepper.GSTAT();
charly 7:51cb60bf3e2d 220 printf("GSTAT(): "); printBits(sizeof(gstat),&gstat);printf("\r\n");
charly 7:51cb60bf3e2d 221 status = stepper.DRV_STATUS();
charly 7:51cb60bf3e2d 222 printf("DRV_STATUS(): "); printBits(sizeof(status),&status);printf("\r\n");
charly 7:51cb60bf3e2d 223 ioin = stepper.IOIN();
charly 7:51cb60bf3e2d 224 printf("IOIN(): "); printBits(sizeof(ioin),&ioin);printf("\r\n");
charly 4:12bfa2c1729f 225
charly 4:12bfa2c1729f 226 }
charly 5:7f250f463aa2 227 // anything changed?
charly 5:7f250f463aa2 228 if (enc_action) {
charly 5:7f250f463aa2 229 enc_action = false;
charly 5:7f250f463aa2 230 // show direction of motor on leds
charly 5:7f250f463aa2 231 if (thisGet > 0) {
charly 5:7f250f463aa2 232 ledCW = 1;
charly 5:7f250f463aa2 233 ledCCW= 0;
charly 5:7f250f463aa2 234 }
charly 5:7f250f463aa2 235 if (thisGet < 0) {
charly 5:7f250f463aa2 236 ledCW = 0;
charly 5:7f250f463aa2 237 ledCCW= 1;
charly 5:7f250f463aa2 238 }
charly 5:7f250f463aa2 239 if (thisGet == 0) {
charly 5:7f250f463aa2 240 ledCW = 1;
charly 5:7f250f463aa2 241 ledCCW= 1;
charly 5:7f250f463aa2 242 }
charly 5:7f250f463aa2 243 }
charly 4:12bfa2c1729f 244 } // while 1
charly 0:3f4cfbeda9d3 245 }