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
- Committer:
- tuxic
- Date:
- 2014-06-17
- Revision:
- 6:cfbef7079a6f
- Parent:
- 5:63fe92075b71
File content as of revision 6:cfbef7079a6f:
#include "mbed.h" #include <math.h> #include "cmsis_os.h" // https://mbed.org/handbook/CMSIS-RTOS // http://mbed.org/users/AjK/notebook/regarding-interrupts-use-and-blocking/ DigitalOut StepLED(LED1); DigitalOut CmdLED(LED2); DigitalOut DirLED(LED3); DigitalOut TestLED(LED4); DigitalOut xDir(p22); DigitalOut xStep(p24); DigitalIn xHome(p8); typedef struct { float steps; // number of steps to take float accel; // accelleration in steps/sec^2 float maxspeed; // maximum speed in steps/sec float minspeed; // minimum speed in steps/sec bool dir; // direction } stepcmd; osPoolDef(cmdpool, 16, stepcmd); osPoolId cmdpool; osMessageQDef(queue, 16, stepcmd); osMessageQId queue; unsigned int qcnt = 0; Timeout timer,steptimer; Timer counter; void decelmove(); void accelmove(); void flatmove(); void stepper(); void stepdown(); const bool xStepHigh = 1; const float pulseTime = 5; // pulse time in microseconds bool N_ESTOP = 1; float asteps, lsteps, dsteps, speed, accel, maxspeed, minspeed, ttime, dtime, wtime , etime, rtime, tsteps; char output[256]; Serial pc(USBTX, USBRX); // tx, rx void stepdown() { xStep = !xStepHigh; tsteps++; } void decelmove() { if (N_ESTOP && (dsteps-- > 0)) { wtime = 1000000 / (speed - accel * (dtime/1000000)); timer.attach_us(decelmove, wtime); xStep = xStepHigh; steptimer.attach_us(stepdown, pulseTime); dtime += wtime; } else { ttime += dtime; etime = ttime; counter.stop(); rtime = counter.read_us(); timer.attach_us(stepper, wtime); } } void flatmove() { if (N_ESTOP && (lsteps-- > 0)) { timer.attach_us(flatmove, wtime); xStep = xStepHigh; steptimer.attach_us(stepdown, pulseTime); ttime += wtime; dtime = 0; } else { // ttime was only accelleration, not flatmove! // after calculating wtime, set to 0 for use in decelmove speed = 1000000.0 / wtime; timer.attach_us(decelmove, wtime); } } void accelmove() { TestLED = 1; if (N_ESTOP && (asteps-- > 0)) { wtime = 1000000 / (speed + accel * (ttime/1000000)); if (wtime < pulseTime) wtime = pulseTime; timer.attach_us(accelmove, wtime); xStep = xStepHigh; steptimer.attach_us(stepdown, pulseTime); ttime += wtime; } else { timer.attach_us(flatmove, wtime); } } void stepper() { osEvent evt = osMessageGet(queue, 0); if (evt.status == osEventMessage) { stepcmd *cmd = (stepcmd*)evt.value.p; xDir = DirLED = cmd->dir; accel = cmd->accel; speed = cmd->minspeed; lsteps = cmd->steps; ttime = 0; tsteps = 0; wtime = 1000000 / speed; float atime = 0; if (accel == 0) { // linear move asteps = dsteps = 0; } else { // calculate time to reach max speed: // speed = startspeed + accel * time; atime = (cmd->maxspeed - cmd->minspeed) / cmd->accel; asteps = speed * atime + 0.5 * accel * pow(atime,2); if ((lsteps /2) > asteps) { // Accellerate to max speed, then flat, then decellerate dsteps = asteps; lsteps -= 2 * asteps; } else { // Accellerate to half of the space, then decellerate asteps = lsteps/ 2; dsteps = lsteps-asteps; lsteps = 0; } } sprintf(output, "atime: %2.6f steps: %f, %f, %f wtime: %f\n\r", atime, asteps, lsteps, dsteps, wtime); counter.reset(); counter.start(); timer.attach_us(accelmove, wtime); osPoolFree(cmdpool, cmd); qcnt--; } else { timer.attach(stepper, 2); } } void home(float speed) { int wtime = int(1000000.0/speed); xDir = 1; while (! xHome) { xStep = 1; wait_us(5); xStep = 0; wait_us(wtime); } xDir = 0; for (int x=0; x<1000; x++) { xStep = 1; wait_us(5); xStep = 0; wait_us(wtime); } } int main (void) { printf("Keys:\n\r"); printf("0-1: Adjust minimum speed (m/s)\n\r"); printf("3-4: Adjust maximum speed (m/s)\n\r"); printf("5-6: Adjust Accelleration (m/s^2)\n\r"); printf("7-8: Adjust total distance (m)\n\r"); printf("9-0: Steps per meter\n\r"); printf("h = home, p = print result, q = E_STOP, t=toggle direction\n\r"); printf("SPACE = move\n\r"); printf("CTRL-Break = reset\n\r\n\r"); TestLED = 0; cmdpool = osPoolCreate(osPool(cmdpool)); queue = osMessageCreate(osMessageQ(queue), NULL); timer.attach(stepper, 2); bool mydir = false; etime = 0; // 902.51 met kleine motor: 0.25/0.8/1.1/80000 = 12.5 micron float startspeed = 0.25; // speed in m/s float maxspeed = 0.8; // speed in m/s float accel = 1.1; // accelleration in m/s^2 float distance = 1.26; // maximum distance on the rod in m float steps = 80000; // steps per meter float precision = 1000000 / steps; pc.printf("Settings: min: %3.2f, max %3.2f, accel %3.2f, dist %3.2f, steps %ld\n\r", startspeed, maxspeed, accel, distance, steps); while (true) { if (pc.readable()) { char c = pc.getc(); switch (c) { case 'h': home(startspeed * steps); break; case '1': if (startspeed > 0) startspeed -= 0.01; break; case '2': startspeed += 0.01; break; case '3': if (maxspeed > startspeed) maxspeed -=0.01; break; case '4': maxspeed += 0.01; break; case '5': if (accel > 0) accel -= 0.01; break; case '6': accel += 0.01; break; case '7': if (distance > 0) distance -= 0.01; break; case '8': distance += 0.01; break; case '9': if (steps > 0) steps -= 100; break; case '0': steps += 100; break; case 't': mydir = !mydir; break; case 'p': 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); break; case 'q': N_ESTOP = ! N_ESTOP; break; case ' ': if (qcnt < 15) { stepcmd *cmd = (stepcmd*)osPoolAlloc(cmdpool); cmd -> minspeed = startspeed *steps; cmd -> maxspeed = maxspeed * steps; cmd -> accel = accel * steps; cmd -> steps = distance * steps; pc.printf("Stepsettings: min: %f, max %f, accel %f, distance: %f\n\r", cmd->minspeed, cmd->maxspeed, cmd->accel, cmd->steps, precision); cmd -> dir = mydir; osStatus res = osMessagePut(queue, (uint32_t)cmd, 0); while (res != osOK) { osDelay(500); res = osMessagePut(queue, (uint32_t)cmd, 0); } pc.printf("Command sent\n\r", ++qcnt); mydir = !mydir; osDelay(100); } else { // pc.printf("Storage full\n\r"); } break; } precision = 1000000 / steps; 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", startspeed, maxspeed, accel, distance, steps, mydir, precision); } else { osDelay(2000); } CmdLED = ! CmdLED; if (strlen(output)>0) { pc.printf("%s\n\r", output); sprintf(output, ""); } } }