Simple program to test a steppermotor. Min/maxspeed, accelleration, distance and steps/m can be changed while the program is running for quick adjustment.
Fork of cmsis_rtos_queue by
main.cpp@6:cfbef7079a6f, 2014-06-17 (annotated)
- Committer:
- tuxic
- Date:
- Tue Jun 17 13:02:17 2014 +0000
- Revision:
- 6:cfbef7079a6f
- Parent:
- 5:63fe92075b71
working version with minimal documentation
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
emilmont | 1:85e3058aad0f | 1 | #include "mbed.h" |
tuxic | 6:cfbef7079a6f | 2 | #include <math.h> |
tuxic | 6:cfbef7079a6f | 3 | #include "cmsis_os.h" // https://mbed.org/handbook/CMSIS-RTOS |
tuxic | 6:cfbef7079a6f | 4 | // http://mbed.org/users/AjK/notebook/regarding-interrupts-use-and-blocking/ |
emilmont | 1:85e3058aad0f | 5 | |
tuxic | 3:08c16b8a78bf | 6 | DigitalOut StepLED(LED1); |
tuxic | 3:08c16b8a78bf | 7 | DigitalOut CmdLED(LED2); |
tuxic | 3:08c16b8a78bf | 8 | DigitalOut DirLED(LED3); |
tuxic | 6:cfbef7079a6f | 9 | DigitalOut TestLED(LED4); |
tuxic | 6:cfbef7079a6f | 10 | DigitalOut xDir(p22); |
tuxic | 6:cfbef7079a6f | 11 | DigitalOut xStep(p24); |
tuxic | 6:cfbef7079a6f | 12 | DigitalIn xHome(p8); |
tuxic | 3:08c16b8a78bf | 13 | |
emilmont | 1:85e3058aad0f | 14 | typedef struct { |
tuxic | 6:cfbef7079a6f | 15 | float steps; // number of steps to take |
tuxic | 6:cfbef7079a6f | 16 | float accel; // accelleration in steps/sec^2 |
tuxic | 6:cfbef7079a6f | 17 | float maxspeed; // maximum speed in steps/sec |
tuxic | 6:cfbef7079a6f | 18 | float minspeed; // minimum speed in steps/sec |
tuxic | 3:08c16b8a78bf | 19 | bool dir; // direction |
tuxic | 3:08c16b8a78bf | 20 | } stepcmd; |
emilmont | 1:85e3058aad0f | 21 | |
tuxic | 3:08c16b8a78bf | 22 | osPoolDef(cmdpool, 16, stepcmd); |
tuxic | 3:08c16b8a78bf | 23 | osPoolId cmdpool; |
emilmont | 1:85e3058aad0f | 24 | |
tuxic | 3:08c16b8a78bf | 25 | osMessageQDef(queue, 16, stepcmd); |
emilmont | 1:85e3058aad0f | 26 | osMessageQId queue; |
tuxic | 6:cfbef7079a6f | 27 | unsigned int qcnt = 0; |
tuxic | 6:cfbef7079a6f | 28 | Timeout timer,steptimer; |
tuxic | 6:cfbef7079a6f | 29 | Timer counter; |
tuxic | 6:cfbef7079a6f | 30 | |
tuxic | 6:cfbef7079a6f | 31 | void decelmove(); |
tuxic | 6:cfbef7079a6f | 32 | void accelmove(); |
tuxic | 6:cfbef7079a6f | 33 | void flatmove(); |
tuxic | 6:cfbef7079a6f | 34 | void stepper(); |
tuxic | 6:cfbef7079a6f | 35 | void stepdown(); |
tuxic | 6:cfbef7079a6f | 36 | |
tuxic | 6:cfbef7079a6f | 37 | const bool xStepHigh = 1; |
tuxic | 6:cfbef7079a6f | 38 | const float pulseTime = 5; // pulse time in microseconds |
tuxic | 6:cfbef7079a6f | 39 | bool N_ESTOP = 1; |
tuxic | 6:cfbef7079a6f | 40 | float asteps, lsteps, dsteps, speed, accel, maxspeed, minspeed, ttime, dtime, wtime , etime, rtime, tsteps; |
tuxic | 6:cfbef7079a6f | 41 | char output[256]; |
tuxic | 6:cfbef7079a6f | 42 | |
tuxic | 6:cfbef7079a6f | 43 | Serial pc(USBTX, USBRX); // tx, rx |
tuxic | 6:cfbef7079a6f | 44 | |
tuxic | 6:cfbef7079a6f | 45 | void stepdown() |
tuxic | 6:cfbef7079a6f | 46 | { |
tuxic | 6:cfbef7079a6f | 47 | xStep = !xStepHigh; |
tuxic | 6:cfbef7079a6f | 48 | tsteps++; |
tuxic | 6:cfbef7079a6f | 49 | } |
emilmont | 1:85e3058aad0f | 50 | |
tuxic | 6:cfbef7079a6f | 51 | void decelmove() |
tuxic | 6:cfbef7079a6f | 52 | { |
tuxic | 6:cfbef7079a6f | 53 | if (N_ESTOP && (dsteps-- > 0)) { |
tuxic | 6:cfbef7079a6f | 54 | wtime = 1000000 / (speed - accel * (dtime/1000000)); |
tuxic | 6:cfbef7079a6f | 55 | timer.attach_us(decelmove, wtime); |
tuxic | 6:cfbef7079a6f | 56 | xStep = xStepHigh; |
tuxic | 6:cfbef7079a6f | 57 | steptimer.attach_us(stepdown, pulseTime); |
tuxic | 6:cfbef7079a6f | 58 | dtime += wtime; |
tuxic | 6:cfbef7079a6f | 59 | } else { |
tuxic | 6:cfbef7079a6f | 60 | ttime += dtime; |
tuxic | 6:cfbef7079a6f | 61 | etime = ttime; |
tuxic | 6:cfbef7079a6f | 62 | counter.stop(); |
tuxic | 6:cfbef7079a6f | 63 | rtime = counter.read_us(); |
tuxic | 6:cfbef7079a6f | 64 | timer.attach_us(stepper, wtime); |
tuxic | 6:cfbef7079a6f | 65 | } |
tuxic | 6:cfbef7079a6f | 66 | } |
tuxic | 6:cfbef7079a6f | 67 | |
tuxic | 6:cfbef7079a6f | 68 | void flatmove() |
tuxic | 6:cfbef7079a6f | 69 | { |
tuxic | 6:cfbef7079a6f | 70 | if (N_ESTOP && (lsteps-- > 0)) { |
tuxic | 6:cfbef7079a6f | 71 | timer.attach_us(flatmove, wtime); |
tuxic | 6:cfbef7079a6f | 72 | xStep = xStepHigh; |
tuxic | 6:cfbef7079a6f | 73 | steptimer.attach_us(stepdown, pulseTime); |
tuxic | 6:cfbef7079a6f | 74 | ttime += wtime; |
tuxic | 6:cfbef7079a6f | 75 | dtime = 0; |
tuxic | 6:cfbef7079a6f | 76 | } else { |
tuxic | 6:cfbef7079a6f | 77 | // ttime was only accelleration, not flatmove! |
tuxic | 6:cfbef7079a6f | 78 | // after calculating wtime, set to 0 for use in decelmove |
tuxic | 6:cfbef7079a6f | 79 | speed = 1000000.0 / wtime; |
tuxic | 6:cfbef7079a6f | 80 | timer.attach_us(decelmove, wtime); |
tuxic | 6:cfbef7079a6f | 81 | } |
tuxic | 6:cfbef7079a6f | 82 | } |
tuxic | 6:cfbef7079a6f | 83 | |
tuxic | 6:cfbef7079a6f | 84 | void accelmove() |
tuxic | 4:09c35ecb3218 | 85 | { |
tuxic | 6:cfbef7079a6f | 86 | TestLED = 1; |
tuxic | 6:cfbef7079a6f | 87 | if (N_ESTOP && (asteps-- > 0)) { |
tuxic | 6:cfbef7079a6f | 88 | wtime = 1000000 / (speed + accel * (ttime/1000000)); |
tuxic | 6:cfbef7079a6f | 89 | if (wtime < pulseTime) wtime = pulseTime; |
tuxic | 6:cfbef7079a6f | 90 | timer.attach_us(accelmove, wtime); |
tuxic | 6:cfbef7079a6f | 91 | xStep = xStepHigh; |
tuxic | 6:cfbef7079a6f | 92 | steptimer.attach_us(stepdown, pulseTime); |
tuxic | 6:cfbef7079a6f | 93 | ttime += wtime; |
tuxic | 6:cfbef7079a6f | 94 | |
tuxic | 6:cfbef7079a6f | 95 | |
tuxic | 6:cfbef7079a6f | 96 | } else { |
tuxic | 6:cfbef7079a6f | 97 | timer.attach_us(flatmove, wtime); |
tuxic | 6:cfbef7079a6f | 98 | } |
tuxic | 6:cfbef7079a6f | 99 | } |
tuxic | 6:cfbef7079a6f | 100 | |
tuxic | 6:cfbef7079a6f | 101 | void stepper() |
tuxic | 6:cfbef7079a6f | 102 | { |
tuxic | 6:cfbef7079a6f | 103 | osEvent evt = osMessageGet(queue, 0); |
tuxic | 6:cfbef7079a6f | 104 | if (evt.status == osEventMessage) { |
tuxic | 6:cfbef7079a6f | 105 | stepcmd *cmd = (stepcmd*)evt.value.p; |
tuxic | 6:cfbef7079a6f | 106 | xDir = DirLED = cmd->dir; |
tuxic | 6:cfbef7079a6f | 107 | accel = cmd->accel; |
tuxic | 6:cfbef7079a6f | 108 | speed = cmd->minspeed; |
tuxic | 6:cfbef7079a6f | 109 | lsteps = cmd->steps; |
tuxic | 6:cfbef7079a6f | 110 | ttime = 0; |
tuxic | 6:cfbef7079a6f | 111 | tsteps = 0; |
tuxic | 6:cfbef7079a6f | 112 | wtime = 1000000 / speed; |
tuxic | 6:cfbef7079a6f | 113 | float atime = 0; |
tuxic | 6:cfbef7079a6f | 114 | if (accel == 0) { // linear move |
tuxic | 6:cfbef7079a6f | 115 | asteps = dsteps = 0; |
tuxic | 6:cfbef7079a6f | 116 | } else { |
tuxic | 6:cfbef7079a6f | 117 | // calculate time to reach max speed: |
tuxic | 6:cfbef7079a6f | 118 | // speed = startspeed + accel * time; |
tuxic | 6:cfbef7079a6f | 119 | atime = (cmd->maxspeed - cmd->minspeed) / cmd->accel; |
tuxic | 6:cfbef7079a6f | 120 | asteps = speed * atime + 0.5 * accel * pow(atime,2); |
tuxic | 6:cfbef7079a6f | 121 | if ((lsteps /2) > asteps) { |
tuxic | 6:cfbef7079a6f | 122 | // Accellerate to max speed, then flat, then decellerate |
tuxic | 6:cfbef7079a6f | 123 | dsteps = asteps; |
tuxic | 6:cfbef7079a6f | 124 | lsteps -= 2 * asteps; |
tuxic | 6:cfbef7079a6f | 125 | } else { |
tuxic | 6:cfbef7079a6f | 126 | // Accellerate to half of the space, then decellerate |
tuxic | 6:cfbef7079a6f | 127 | asteps = lsteps/ 2; |
tuxic | 6:cfbef7079a6f | 128 | dsteps = lsteps-asteps; |
tuxic | 6:cfbef7079a6f | 129 | lsteps = 0; |
tuxic | 6:cfbef7079a6f | 130 | } |
tuxic | 6:cfbef7079a6f | 131 | } |
tuxic | 6:cfbef7079a6f | 132 | sprintf(output, "atime: %2.6f steps: %f, %f, %f wtime: %f\n\r", atime, asteps, lsteps, dsteps, wtime); |
tuxic | 6:cfbef7079a6f | 133 | counter.reset(); |
tuxic | 6:cfbef7079a6f | 134 | counter.start(); |
tuxic | 6:cfbef7079a6f | 135 | timer.attach_us(accelmove, wtime); |
tuxic | 6:cfbef7079a6f | 136 | osPoolFree(cmdpool, cmd); |
tuxic | 6:cfbef7079a6f | 137 | qcnt--; |
tuxic | 6:cfbef7079a6f | 138 | } else { |
tuxic | 6:cfbef7079a6f | 139 | timer.attach(stepper, 2); |
tuxic | 6:cfbef7079a6f | 140 | } |
tuxic | 6:cfbef7079a6f | 141 | } |
tuxic | 6:cfbef7079a6f | 142 | void home(float speed) |
tuxic | 6:cfbef7079a6f | 143 | { |
tuxic | 6:cfbef7079a6f | 144 | int wtime = int(1000000.0/speed); |
tuxic | 6:cfbef7079a6f | 145 | xDir = 1; |
tuxic | 6:cfbef7079a6f | 146 | while (! xHome) { |
tuxic | 6:cfbef7079a6f | 147 | xStep = 1; |
tuxic | 6:cfbef7079a6f | 148 | wait_us(5); |
tuxic | 6:cfbef7079a6f | 149 | xStep = 0; |
tuxic | 6:cfbef7079a6f | 150 | wait_us(wtime); |
tuxic | 6:cfbef7079a6f | 151 | } |
tuxic | 6:cfbef7079a6f | 152 | xDir = 0; |
tuxic | 6:cfbef7079a6f | 153 | for (int x=0; x<1000; x++) { |
tuxic | 6:cfbef7079a6f | 154 | xStep = 1; |
tuxic | 6:cfbef7079a6f | 155 | wait_us(5); |
tuxic | 6:cfbef7079a6f | 156 | xStep = 0; |
tuxic | 6:cfbef7079a6f | 157 | wait_us(wtime); |
tuxic | 6:cfbef7079a6f | 158 | } |
emilmont | 1:85e3058aad0f | 159 | } |
emilmont | 1:85e3058aad0f | 160 | |
tuxic | 4:09c35ecb3218 | 161 | int main (void) |
tuxic | 4:09c35ecb3218 | 162 | { |
tuxic | 6:cfbef7079a6f | 163 | printf("Keys:\n\r"); |
tuxic | 6:cfbef7079a6f | 164 | printf("0-1: Adjust minimum speed (m/s)\n\r"); |
tuxic | 6:cfbef7079a6f | 165 | printf("3-4: Adjust maximum speed (m/s)\n\r"); |
tuxic | 6:cfbef7079a6f | 166 | printf("5-6: Adjust Accelleration (m/s^2)\n\r"); |
tuxic | 6:cfbef7079a6f | 167 | printf("7-8: Adjust total distance (m)\n\r"); |
tuxic | 6:cfbef7079a6f | 168 | printf("9-0: Steps per meter\n\r"); |
tuxic | 6:cfbef7079a6f | 169 | printf("h = home, p = print result, q = E_STOP, t=toggle direction\n\r"); |
tuxic | 6:cfbef7079a6f | 170 | printf("SPACE = move\n\r"); |
tuxic | 6:cfbef7079a6f | 171 | printf("CTRL-Break = reset\n\r\n\r"); |
tuxic | 6:cfbef7079a6f | 172 | TestLED = 0; |
tuxic | 3:08c16b8a78bf | 173 | cmdpool = osPoolCreate(osPool(cmdpool)); |
emilmont | 1:85e3058aad0f | 174 | queue = osMessageCreate(osMessageQ(queue), NULL); |
tuxic | 6:cfbef7079a6f | 175 | timer.attach(stepper, 2); |
tuxic | 6:cfbef7079a6f | 176 | bool mydir = false; |
tuxic | 6:cfbef7079a6f | 177 | etime = 0; |
tuxic | 6:cfbef7079a6f | 178 | // 902.51 met kleine motor: 0.25/0.8/1.1/80000 = 12.5 micron |
tuxic | 6:cfbef7079a6f | 179 | float startspeed = 0.25; // speed in m/s |
tuxic | 6:cfbef7079a6f | 180 | float maxspeed = 0.8; // speed in m/s |
tuxic | 6:cfbef7079a6f | 181 | float accel = 1.1; // accelleration in m/s^2 |
tuxic | 6:cfbef7079a6f | 182 | float distance = 1.26; // maximum distance on the rod in m |
tuxic | 6:cfbef7079a6f | 183 | float steps = 80000; // steps per meter |
tuxic | 6:cfbef7079a6f | 184 | float precision = 1000000 / steps; |
tuxic | 6:cfbef7079a6f | 185 | pc.printf("Settings: min: %3.2f, max %3.2f, accel %3.2f, dist %3.2f, steps %ld\n\r", |
tuxic | 6:cfbef7079a6f | 186 | startspeed, maxspeed, accel, distance, steps); |
emilmont | 1:85e3058aad0f | 187 | while (true) { |
tuxic | 6:cfbef7079a6f | 188 | if (pc.readable()) { |
tuxic | 6:cfbef7079a6f | 189 | char c = pc.getc(); |
tuxic | 6:cfbef7079a6f | 190 | switch (c) { |
tuxic | 6:cfbef7079a6f | 191 | case 'h': |
tuxic | 6:cfbef7079a6f | 192 | home(startspeed * steps); |
tuxic | 6:cfbef7079a6f | 193 | break; |
tuxic | 6:cfbef7079a6f | 194 | case '1': |
tuxic | 6:cfbef7079a6f | 195 | if (startspeed > 0) startspeed -= 0.01; |
tuxic | 6:cfbef7079a6f | 196 | break; |
tuxic | 6:cfbef7079a6f | 197 | case '2': |
tuxic | 6:cfbef7079a6f | 198 | startspeed += 0.01; |
tuxic | 6:cfbef7079a6f | 199 | break; |
tuxic | 6:cfbef7079a6f | 200 | case '3': |
tuxic | 6:cfbef7079a6f | 201 | if (maxspeed > startspeed) maxspeed -=0.01; |
tuxic | 6:cfbef7079a6f | 202 | break; |
tuxic | 6:cfbef7079a6f | 203 | case '4': |
tuxic | 6:cfbef7079a6f | 204 | maxspeed += 0.01; |
tuxic | 6:cfbef7079a6f | 205 | break; |
tuxic | 6:cfbef7079a6f | 206 | case '5': |
tuxic | 6:cfbef7079a6f | 207 | if (accel > 0) accel -= 0.01; |
tuxic | 6:cfbef7079a6f | 208 | break; |
tuxic | 6:cfbef7079a6f | 209 | case '6': |
tuxic | 6:cfbef7079a6f | 210 | accel += 0.01; |
tuxic | 6:cfbef7079a6f | 211 | break; |
tuxic | 6:cfbef7079a6f | 212 | case '7': |
tuxic | 6:cfbef7079a6f | 213 | if (distance > 0) distance -= 0.01; |
tuxic | 6:cfbef7079a6f | 214 | break; |
tuxic | 6:cfbef7079a6f | 215 | case '8': |
tuxic | 6:cfbef7079a6f | 216 | distance += 0.01; |
tuxic | 6:cfbef7079a6f | 217 | break; |
tuxic | 6:cfbef7079a6f | 218 | case '9': |
tuxic | 6:cfbef7079a6f | 219 | if (steps > 0) steps -= 100; |
tuxic | 6:cfbef7079a6f | 220 | break; |
tuxic | 6:cfbef7079a6f | 221 | case '0': |
tuxic | 6:cfbef7079a6f | 222 | steps += 100; |
tuxic | 6:cfbef7079a6f | 223 | break; |
tuxic | 6:cfbef7079a6f | 224 | case 't': |
tuxic | 6:cfbef7079a6f | 225 | mydir = !mydir; |
tuxic | 6:cfbef7079a6f | 226 | break; |
tuxic | 6:cfbef7079a6f | 227 | case 'p': |
tuxic | 6:cfbef7079a6f | 228 | pc.printf("Etime: %2.3f, Rtime: %2.3f a,l,d,steps: %f,%f,%f,%f\n\r", (float)(etime/1000000.0), rtime/1000000, asteps, lsteps, dsteps, tsteps); |
tuxic | 6:cfbef7079a6f | 229 | break; |
tuxic | 6:cfbef7079a6f | 230 | case 'q': |
tuxic | 6:cfbef7079a6f | 231 | N_ESTOP = ! N_ESTOP; |
tuxic | 6:cfbef7079a6f | 232 | break; |
tuxic | 6:cfbef7079a6f | 233 | case ' ': |
tuxic | 6:cfbef7079a6f | 234 | if (qcnt < 15) { |
tuxic | 6:cfbef7079a6f | 235 | stepcmd *cmd = (stepcmd*)osPoolAlloc(cmdpool); |
tuxic | 6:cfbef7079a6f | 236 | cmd -> minspeed = startspeed *steps; |
tuxic | 6:cfbef7079a6f | 237 | cmd -> maxspeed = maxspeed * steps; |
tuxic | 6:cfbef7079a6f | 238 | cmd -> accel = accel * steps; |
tuxic | 6:cfbef7079a6f | 239 | cmd -> steps = distance * steps; |
tuxic | 4:09c35ecb3218 | 240 | |
tuxic | 6:cfbef7079a6f | 241 | pc.printf("Stepsettings: min: %f, max %f, accel %f, distance: %f\n\r", |
tuxic | 6:cfbef7079a6f | 242 | cmd->minspeed, cmd->maxspeed, cmd->accel, cmd->steps, precision); |
tuxic | 6:cfbef7079a6f | 243 | cmd -> dir = mydir; |
tuxic | 6:cfbef7079a6f | 244 | osStatus res = osMessagePut(queue, (uint32_t)cmd, 0); |
tuxic | 6:cfbef7079a6f | 245 | while (res != osOK) { |
tuxic | 6:cfbef7079a6f | 246 | osDelay(500); |
tuxic | 6:cfbef7079a6f | 247 | res = osMessagePut(queue, (uint32_t)cmd, 0); |
tuxic | 6:cfbef7079a6f | 248 | } |
tuxic | 6:cfbef7079a6f | 249 | pc.printf("Command sent\n\r", ++qcnt); |
tuxic | 6:cfbef7079a6f | 250 | mydir = !mydir; |
tuxic | 6:cfbef7079a6f | 251 | osDelay(100); |
tuxic | 6:cfbef7079a6f | 252 | } else { |
tuxic | 6:cfbef7079a6f | 253 | // pc.printf("Storage full\n\r"); |
tuxic | 6:cfbef7079a6f | 254 | } |
tuxic | 6:cfbef7079a6f | 255 | break; |
tuxic | 4:09c35ecb3218 | 256 | } |
tuxic | 6:cfbef7079a6f | 257 | precision = 1000000 / steps; |
tuxic | 6:cfbef7079a6f | 258 | pc.printf("Settings: min: %3.2f, max %3.2f, accel %3.2f, dist %3.2f, steps %7.0f DIR: %d, precision: %2.3f\n\r", |
tuxic | 6:cfbef7079a6f | 259 | startspeed, maxspeed, accel, distance, steps, mydir, precision); |
tuxic | 4:09c35ecb3218 | 260 | } else { |
tuxic | 6:cfbef7079a6f | 261 | osDelay(2000); |
emilmont | 1:85e3058aad0f | 262 | } |
tuxic | 3:08c16b8a78bf | 263 | CmdLED = ! CmdLED; |
tuxic | 6:cfbef7079a6f | 264 | if (strlen(output)>0) { |
tuxic | 6:cfbef7079a6f | 265 | pc.printf("%s\n\r", output); |
tuxic | 6:cfbef7079a6f | 266 | sprintf(output, ""); |
tuxic | 6:cfbef7079a6f | 267 | } |
emilmont | 1:85e3058aad0f | 268 | } |
tuxic | 6:cfbef7079a6f | 269 | } |