File content as of revision 3:8708e61475fe:

#include "mbed.h"

#define M_PI 3.14159265358979323846f
#define smoothingNumber 6

#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

//Serial pc(D1,D0,4800);
DigitalOut RCout(D10);
DigitalOut dir(D9);
InterruptIn leftSwitch(D2);
InterruptIn rightSwitch(D3);
Ticker tick;
Ticker ledTicker;
DigitalOut  myled(LED2);
RawSerial rpc(D1,D0,9600);

int period_us = 26;//300 26

bool isSwinged = false;

long pos=0;

long railLength = 0;

uint8_t state=STATE_INIT;

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

int driveSpeed = 0;

float angularPosNew = 0;
float angularPosOld = 0;

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

float dx = 0;
float xPosNew = 0;
float xPosOld = 0;

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

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

float angle=0.f;
float angleOffset = 0;

double PIPI = 6.28;

bool canSend = false;

typedef union {
    float number[5];
    uint8_t numberCh[20];
} my_union;

my_union myUnion;

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

float getPosMM() {
    //return (pos-railLength/2) * 550.0f/railLength;
    //return (pos - railLength / 2) * (350.0f / railLength);
    return pos / 3500.0f;
}

uint16_t getPendulumPos(){
    cs=0;
    wait_ms(1);
    uint16_t d=spi.write((short)0x00);
    d=d<<1;//fucking shithole fakebit
    d=d>>6;//no need debug info
    cs=1;
    wait_ms(1);
    return (uint16_t)d;
}


float getPendulumAngle(){
    angle = getPendulumPos();
    angle = angle * 6.28f / 1024.0f;
    return angle;
}


/*
 ANGULAR SPEED CALC
*/

void getDeltaAng(){

    angularPosNew = getPendulumAngle();

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

    angularPosOld = angularPosNew;

}

void getAngularSpeed(){
    float deltaTime;

    timeStart = float(t.read());
    deltaTime = (timeStart - timeOld);
    getDeltaAng();
    anSpd = dAngle / deltaTime;
    timeOld=timeStart;
    //взятие по модулю, спросить
}

/*
    SPEED CALC
*/

void getDeltaPos()
{
    float delta = 0;
    xPosNew = getPosMM();
    delta = xPosNew - xPosOld;
    dx = delta;
    xPosOld = xPosNew;
}

float getSpeed(){
    float deltaTime;
    float speed;
    getDeltaPos();
    timeStartPos = float(t.read());
    deltaTime = (timeStartPos - timeOldPos);
    speed = (dx) * deltaTime;
    timeOldPos = timeStartPos;
    //взятие по модулю, спросить
    return speed;
}



void stepperFlip() {
    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) {
        state = STATE_WAITING;
    }
   if (state == STATE_SWING_LEFT && state==STATE_SWING_RIGHT) {
        pos += (dir.read() * 2 - 1);
        RCout = !RCout;
    }
}



void led() {
    myled = !myled;
}

void updateBlink(uint64_t t){
    ledTicker.detach();
    ledTicker.attach_us (&led, t / 2);
}

void updatePeriod(){
    tick.detach();
    tick.attach_us (&stepperFlip, period_us / 2.0f);
}

void leftEnd() {
    dir = DIR_RIGHT;
    if (state == STATE_GOTO_START) {
        state = STATE_GOTO_END_COUNTING;
        pos = 0;
    }
    else if (isPendulumSwinging()) {
        state = STATE_GOTO_MIDDLE;
        //angleOffset -= 0.006191;
        //state = STATE_ERROR;
    }
    //при втыкании в концевик меняем смещение (offset)
}

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

void getSwingDirectory() {
        int currentSpd = anSpd;
        if (currentSpd > 0 ) {
            state = STATE_SWING_RIGHT;
            dir = DIR_LEFT;
            //dir = DIR_LEFT;
            return;
        }
        if (currentSpd < 0) {
            state = STATE_SWING_LEFT;
            dir = DIR_RIGHT;
            //dir = DIR_RIGHT;
            return;
        }
}

void sendData() {
    getAngularSpeed();
    myUnion.number[0] = t.read();
    myUnion.number[1] = dx;
    myUnion.number[2] = getSpeed();
    myUnion.number[3] = dAngle;
    myUnion.number[4] = anSpd;
    for(int i = 0; i < 20; i++) {
        rpc.putc(myUnion.numberCh[i]);
    }
}

void Rx_interrupt() {
    int password = rpc.getc();
    if (password == 228) {
        canSend = true;
    }
    return;
}

int main() {
    RCout = 1;
    wait_ms(500);
    spi.format(16,2);
    spi.frequency(1000000);
    t.start();
    leftSwitch.rise(&leftEnd);
    rightSwitch.rise(&rightEnd);
    for (int i=5; i>0; i--) {
        getPendulumAngle();
        myled = !myled;
        wait_ms(500);
    }
    angleOffset=angle;
    updatePeriod();
    state=STATE_GOTO_START;
    dir=DIR_LEFT;
    rpc.attach(&Rx_interrupt, Serial::RxIrq);
    while(1) {
        if(canSend) {
            sendData();
        } else {
            getSpeed();
            getAngularSpeed();
        }
        switch(state) {
            case STATE_WAITING:
                state = STATE_GOTO_SWING;
                break;
            case STATE_GOTO_START:
                break;
            case STATE_GOTO_END_COUNTING:
                break;
            case STATE_GOTO_SWING:
                break;
            case STATE_SWING_LEFT:
                break;
            case STATE_SWING_RIGHT:
                break;
            default:
                break;
        }
        //wait_ms(5);
    }
}