File content as of revision 9:e8c6a414e226:

#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

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

int period_us = 26;//300 26
int posCounter = 0;

long pos=0;

long railLength = 0;

uint8_t state=STATE_INIT;

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

float radius = 0.0025;

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;
float posMap[4] = {0.0f, 0.0f, 0.0f, 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 PIPI = 6.28;

bool canSend = false;
bool periodUpdated = false;
bool calibrated = false;
bool dirUpdated = 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-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;
    angle += angleOffset;
    if (angle > 3.14) {
        angle = angle - PIPI;    
    }
    /*if (angle > PIPI + 0.01) {
        angle = fmod((angle + 3.14), PIPI) - 3.14;
        if (angle < -3.14) {
            angle += PIPI;
        }
    }*/
    /*if (calibrated) {
        float test = fmodf(angle, M_PI);
        if (test >= 1) {
            angle = -test;    
        } else {
            angle = test;
        }
    }*/
    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 calcSpeed() {
    /*posMap[posCounter] = getPosMM() / 1000;
    if (posCounter == 3) {
        posCounter = 0;
        float pathSum = 0;
        for (int i = 0; i < 3; i++) {
            float pathDiff = posMap[i + 1] - posMap[i]; 
            if ((pathDiff) < 0) {
                pathSum += -pathDiff;
            } else {
                pathSum += pathDiff;
            }
        }
        speed = pathSum;  
    } else {
        posCounter += 1;
    }*/
    xPosNew = (getPosMM() - posOffset) / 1000;
    speed = xPosNew - xPosOld;
    if (dir == DIR_LEFT) {
        speed = -speed;    
    }
    //speed = -speed;
    
    xPosOld = xPosNew;
}

void calcControl() {
    //float frequency = 1000000 / (period_us / 2);
    //float rates = frequency / 6400;
    //control = rates * PIPI * radius;
    if (dir = DIR_RIGHT) {
        control = -period_us;
    } else {
        control = period_us;
    }
}



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


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

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;
    }
    if (state == STATE_GOTO_SWING) {
        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_GOTO_MIDDLE;
        //angleOffset += 0.006191;
        state = STATE_ERROR;
    }
    if (state == STATE_GOTO_SWING) {
        state = STATE_ERROR;
    }
}

int swingCounter = 0;
void getSwingDirectory() {
        swingCounter += 1;
        control = -control;
        if (dir == DIR_RIGHT) {
            state = STATE_SWING_RIGHT;
            dir = DIR_LEFT;
            return;
        }
        if (dir == DIR_LEFT) {
            state = STATE_SWING_LEFT;
            dir = DIR_RIGHT;
            return;
        }
}

void sendData() {
    myUnion.number[0] = t.read();
    myUnion.number[1] = (getPosMM()- posOffset) / 1000 ;
    myUnion.number[2] = speed;
    //myUnion.number[3] = dAngle;
    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 Rx_interrupt() {
    int command = rpc.getc();
    switch (command) {
        case 50:
            canSend = true;
            break;
        case 60:
            state = STATE_GOTO_SWING;
            speedTicker.attach(calcSpeed, 1);
            break;
        case 65:
            int direction = rpc.getc();
            if (direction <= 0) {
                if (dir != DIR_RIGHT) {
                dir = DIR_RIGHT;
                }
            } else if (direction > 0) {
                if (dir != DIR_LEFT) {
                    dir = DIR_LEFT;    
                }
            }
            break;
        case 70:
            int newPeriod = rpc.getc();
            if (newPeriod < 26) {
                tick.detach();
            } else if (period_us != newPeriod) {
                period_us = newPeriod;
                periodUpdated = true;
            }
            break;
        default:
            break;
    }
    return;
}

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;
    calibrated = true;
    //wait(12);
    wait(7);
    updatePeriod();
    calcControl();
    state=STATE_GOTO_START;
    dir=DIR_LEFT;
    while(1) {
        getAngularSpeed();
        if (canSend) {
            sendData();
        }
        /*if (periodUpdated) {
            calcControl();
            updatePeriod();
            periodUpdated = false;
        }*/
        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:
                //speedTicker.attach(calcSpeed, 1);
                //swingTicker.attach(getSwingDirectory, 1);
                //state = STATE_SWING_LEFT;
                break;
            case STATE_SWING_LEFT:
                /*if (swingCounter == 6) {
                    swingCounter = 0;
                    period_us += 4;
                    periodUpdated = true;
                }*/
                break;
            case STATE_SWING_RIGHT:
                /*if (swingCounter == 6) {
                    swingCounter = 0;
                    period_us += 4;
                    periodUpdated = true;
                }*/
                break;
            default:
                break;
        }
        wait_ms(100);
    }
}