Jaap Vermaas / Mbed 2 deprecated stepper-tester

Simple program to test a steppermotor. Min/maxspeed, accelleration, distance and steps/m can be changed while the program is running for quick adjustment.

Dependencies:   mbed-rtos mbed

Fork of cmsis_rtos_queue by mbed official

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?

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