File content as of revision 0:06a18c897777:

#include "mbed.h"

#define M_PI 3.14159265358979323846f

#define STATE_ERROR 0
#define STATE_INIT 1
#define STATE_GOTO_START 2
#define STATE_GOTO_END_COUNTING 3
#define STATE_GOTO_MIDDLE 4
#define STATE_WAITING 5
#define STATE_GOTO_SWING 6
#define STATE_START_SWING 7
#define STATE_SWING_RIGHT 8
#define STATE_SWING_LEFT 9
#define STATE_EMPTY_SWING 10

#define DIR_LEFT 0
#define DIR_RIGHT 1

#define timer_period_us 10


DigitalOut RCout(D10);
DigitalOut dir(D9);
InterruptIn leftSwitch(D2);
InterruptIn rightSwitch(D3);
Ticker tick;
Ticker speedTicker;
Ticker swingTicker;
RawSerial rpc(D1,D0,9600);
DigitalOut led(D4);

int period_us = 1;
int timerCounter = 0;

long pos=0;

long railLength = 0;

uint8_t state=STATE_INIT;
uint8_t data[2] = {0, 0};
uint8_t cmdIndex = 0;

Timer t;
SPI spi(D11,D12,D13);// mosi, miso, sclk
DigitalOut cs(D5);

float angularPosNew = 0;
float angularPosOld = 0;

float timeOld = 0.0f;
float timeStart = 0.0f;

float dx = 0;
float xPosNew = 0;
float xPosOld = 0;
float speed = -1;
float posOffset = 0.0f;

double dAngle = 0.0f;
double anSpd = 0.0f;

float timeOldPos = 0.0f;
float timeStartPos = 0.0f;

float angle=0.0f;
float angleOffset = 0;

float control = 0.0f;

double twoPI = 6.28;

bool newData = false;
bool canSend = false;


typedef union {
    float number[6];
    uint8_t numberCh[24];
} my_union;

my_union myUnion;

bool isPendulumSwinging() {
    return  state == STATE_SWING_RIGHT || state == STATE_SWING_LEFT;
}

float getPosMM() {
    return pos / 3500.0f;
}

uint16_t getPendulumPos(){
    cs=0;
    wait_ms(1);
    uint16_t d=spi.write((short)0x00);
    d=d<<1;
    d=d>>6;
    cs=1;
    wait_ms(1);
    return (uint16_t)d;
}


float getPendulumAngle(){
    angle = getPendulumPos();
    angle = angle * 6.28f / 1024.0f;
    angle += angleOffset;
    if (angle > 3.14) {
        angle = angle - twoPI;    
    }
    return angle;
}


/*
 ANGULAR SPEED CALC
*/

void getDeltaAng(){

    angularPosNew = getPendulumAngle();

    dAngle = fmod((angularPosNew - angularPosOld + 3.14), twoPI) - 3.14;
    if (dAngle < -3.14) {
        dAngle += twoPI;
    }

    angularPosOld = angularPosNew;

}

void getAngularSpeed(){
    float deltaTime;
    timeStart = float(t.read());
    deltaTime = (timeStart - timeOld);
    getDeltaAng();
    anSpd = dAngle / deltaTime;
    timeOld=timeStart;
}

/*
    SPEED CALC
*/


void calcSpeed() {
    xPosNew = (getPosMM() - posOffset) / 1000;
    speed = xPosNew - xPosOld;
    if (dir == DIR_LEFT) {
        speed = -speed;
    }
    xPosOld = xPosNew;
}

void calcControl() {
    control = (dir.read() * 2 - 1) *(period_us * timer_period_us);
}



void stepperFlip() {
    if (timerCounter * timer_period_us > period_us) {
        timerCounter = 0;
        if (state != STATE_WAITING && state != STATE_ERROR && state != STATE_INIT && state){
            RCout = !RCout;
            pos += (dir.read() * 2 - 1);
        }
        if (state == STATE_GOTO_MIDDLE && pos == railLength / 2) {
            posOffset = 40;
            state = STATE_WAITING;
        }
        if (state == STATE_SWING_LEFT && state==STATE_SWING_RIGHT) {
            pos += (dir.read() * 2 - 1);
            RCout = !RCout;
        }
    } else {
        timerCounter += 1;
    }
}

void leftEnd() {
    dir = DIR_RIGHT;
    if (state == STATE_GOTO_START) {
        state = STATE_GOTO_END_COUNTING;
        pos = 0;
    }
    else if (isPendulumSwinging()) {
        state = STATE_ERROR;
    }
}

void rightEnd() {
    dir=DIR_LEFT;
    if (state == STATE_GOTO_END_COUNTING) {
        railLength=pos;
        state = STATE_GOTO_MIDDLE;
    }
    else if (isPendulumSwinging()) {
        state = STATE_ERROR;
    }
}


void sendData() {
    myUnion.number[0] = t.read();
    myUnion.number[1] = (getPosMM()- posOffset) / 1000 ;
    myUnion.number[2] = speed;
    myUnion.number[3] = angle;
    myUnion.number[4] = anSpd;
    myUnion.number[5] = control;
    for(int i = 0; i < 24; i++) {
        rpc.putc(myUnion.numberCh[i]);
    }
}


void changeDir() {
    if (dir == DIR_LEFT) {
        dir = DIR_RIGHT;    
    } else {
        dir = DIR_LEFT;
    }    
}


void Rx_interrupt() {
    data[cmdIndex] = rpc.getc();
    if (cmdIndex == 1) {
        newData = true;
    }
    cmdIndex = !cmdIndex;
}

void proceedCommands() {
    newData = false;
    switch (data[0]) {
        case 45:
            NVIC_SystemReset();
        case 50:
            canSend = true;
            break;
        case 55:
            state = STATE_ERROR;
        case 60:
            led = !led;
            state = STATE_GOTO_SWING;
            break;
        case 65:
            if (data[1] == 1) {
                dir = DIR_RIGHT;
            } else if (data[1] == 10) {
                dir = DIR_LEFT;
            }
            break;
        case 70:
            period_us = data[1];
            calcControl();
            break;
        default:
            break;    
    }
}


int main() {
    RCout = 1;
    wait_ms(500);
    spi.format(16,2);
    spi.frequency(1000000);
    t.start();
    leftSwitch.rise(&leftEnd);
    rightSwitch.rise(&rightEnd);
    rpc.attach(&Rx_interrupt, Serial::RxIrq);
    for (int i=5; i>0; i--) {
        getPendulumAngle();
        wait_ms(500);
    }
    angleOffset= 3.14 - angle;
    wait(5);
    calcControl();
    tick.attach_us(stepperFlip, timer_period_us);
    state=STATE_GOTO_START;
    dir=DIR_LEFT;
    while(1) {
        getAngularSpeed();
        if (canSend) {
            sendData();
        }
        if (newData) {
            proceedCommands();    
        }
        switch(state) {
            case STATE_WAITING:
                break;
            case STATE_GOTO_START:
                break;
            case STATE_GOTO_END_COUNTING:
                break;
            case STATE_GOTO_SWING:
                speedTicker.attach(calcSpeed, 1);
                state = STATE_SWING_LEFT;
                break;
            case STATE_SWING_LEFT:
                break;
            case STATE_SWING_RIGHT:
                break;
            default:
                break;
        }
    }
}