File content as of revision 1:36f06541698a:

#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() {
   /* if (getPendulumAngle() >= 0) {
        state = STATE_SWING_RIGHT;
        dir = DIR_LEFT;
    } else if (getPendulumAngle() * 100 < 0) {
        state = STATE_SWING_LEFT;
        dir = DIR_RIGHT;
    } else if (getPendulumAngle() * 100 > 10) {
            if (getPendulumAngle() * 100 < 0) {

               } else {
               state = STATE_SWING_LEFT;
                dir = DIR_RIGHT;
        }
    } else if (getPendulumAngle()  == 0.006191) {
        state = STATE_SWING_RIGHT;
        dir = DIR_LEFT;
        }
    return ;*/

    //ебля с угловой скоростью и положением
    /*
    if (int(getPendulumAngle()* 1000) == -12 && int(getAngularSpeed() * 1000000) < 3) {
        state = STATE_SWING_LEFT;
        dir = DIR_RIGHT;
        } else if (int(getPendulumAngle() * 1000) == 6 && int(getAngularSpeed() * 1000000) < 3) {
            state = STATE_SWING_RIGHT;
            dir = DIR_LEFT;
        } else if (getPendulumAngle() == 0 && int(getAngularSpeed() * 1000000) < 3) {
            state = STATE_SWING_RIGHT;
            dir = DIR_LEFT;
        } else if (int(getPendulumAngle() * 1000) == -6 && int(getAngularSpeed() * 1000000) < 3) {
            state = STATE_SWING_LEFT;
            dir = DIR_RIGHT;
        }  */
        int currentSpd = anSpd;
        /*if (currentSpd >= 0 && currentSpd <= 1000000000) {
            state = STATE_SWING_LEFT;
            dir = DIR_LEFT;
            return;
        }
        if (currentSpd >= -1000000000 && currentSpd <= ) {
            state = STATE_SWING_RIGHT;
            dir = DIR_RIGHT;
            return;
        }*/
       // printf("angl %f spd %f crspd %d\r\n", angle, getAngularSpeed(), currentSpd);
        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 resumeSwinging(){
 if (state == STATE_EMPTY_SWING) {
        /*getSwingDirectory();
        if (state == STATE_SWING_LEFT) {
        dir = DIR_LEFT;
        state = STATE_SWING_RIGHT;
         } else if (state == STATE_SWING_RIGHT) {
         dir = DIR_RIGHT;
        state = STATE_SWING_LEFT;
         }*/
         state = STATE_GOTO_MIDDLE;
     }
 }
 /*
void initialSwinging() {
    swingTimer.start();
    while(swingTimer.read() < 5)
    {
        updateBlink(13);
        printf("hi");
        if (getPendulumAngle() >= 0) {
        dir = DIR_LEFT;
    } else if (getPendulumAngle() * 100 < 0) {
        dir = DIR_RIGHT;
    } else if (getPendulumAngle() * 100 > 10) {
            if (getPendulumAngle() * 100 < 0) {
               } else {
                dir = DIR_RIGHT;
        }
    } else if (getPendulumAngle()  == 0.006191) {
        dir = DIR_LEFT;
        }
    }
    swingTimer.stop();
    isSwinged = true;
    getSwingDirectory();
}*/

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);
    t.start();
    leftSwitch.rise(&leftEnd);
    rightSwitch.rise(&rightEnd);
    for (int i=5; i>0; i--) {
        getPendulumAngle();
        myled = !myled;
        wait_ms(500);
    }
    //angleOffset=angle;
    //angleOffset = 0.006191;
    //angleOffset = 0;
    /*for (int i=5; i>0; i--) {
        getPendulumAngle();
        myled = !myled;
        wait_ms(500);
    }
    angleOffset=angle;*/
    updatePeriod();
    state=STATE_GOTO_START;
    dir=DIR_LEFT;
    spi.format(16,2);
    spi.frequency(1000000);
    rpc.attach(&Rx_interrupt, Serial::RxIrq);
    while(1) {
        //angleOffset = 0.006191;
       // printf("st: %d  - %d, pos: %d is: %f spd: %d\r\n", state, railLength, pos,getPendulumAngle(), int(getAngularSpeed() * 1000000));
       // printf("delta: %f, angle: %f spd %d\r\n",deltaPos(), getPendulumAngle(), int(getAngularSpeed() * 1000000));


       //rpc.printf("%f %f %f %f %f\n",dx, getSpeed(), angle , getAngularSpeed(), t.read());
        if(canSend) {
            sendData();
        } else {
            getSpeed();
            getAngularSpeed();
        }
        //printf("pos %f spd %f time %f\n", getPosMM(), getSpeed(), t.read());

       //printf("%d\n", pos);
     // printf("angl %f spd %f\r\n",angle, getAngularSpeed());
        switch(state) {
            case STATE_WAITING:
                //updateBlink(13400);
                //wait(1);
               // updateBlink(25000);
               updateBlink(1000);
                /*if (isSwinged)
                {
                    state = STATE_GOTO_SWING;
                }
                initialSwinging();*/
                state = STATE_GOTO_SWING;
                break;
            case STATE_GOTO_START:
                //updateBlink(25000);
                updateBlink(1000);
                break;
            case STATE_GOTO_END_COUNTING:
                //updateBlink(25000);
                updateBlink(1000);
                break;
            case STATE_GOTO_SWING:
                //updateBlink(25000);
                updateBlink(1000);
               // dir = DIR_LEFT;
                //getSwingDirectory();
                //updateBlink(50);
                getSwingDirectory();
                //printf("off %f\n", angleOffset);
                break;
            case STATE_SWING_LEFT:

                getSwingDirectory();
                //printf("off %f\n", angleOffset);
                /*if (pos > 55000) {
                    state = STATE_GOTO_MIDDLE;
                    dir=DIR_LEFT;
                    } else if(pos < 5000) {
                        state = STATE_GOTO_MIDDLE;
                        dir=DIR_RIGHT;
                        }*/
                break;
            case STATE_SWING_RIGHT:

                getSwingDirectory();
                //printf("off %f\n", angleOffset);
               /* if (pos < 5000) {
                    state = STATE_GOTO_MIDDLE;
                    dir=DIR_RIGHT;
                    } else if(pos > 55000) {
                        state = STATE_GOTO_MIDDLE;
                        dir=DIR_LEFT;
                    }*/
                break;
                /*
            case STATE_EMPTY_SWING:
                resumeSwinging();
                break;*/
            default:
                //updateBlink(25000);
                updateBlink(1000);
                break;
        }
        //wait_ms(5);
    }
}