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, "");
        }
    }
}