File content as of revision 1:85ab39fea863:

#include "mbed.h"
#define smoothingNumber 3
#define M_PI 3.14159265358979323846f

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

InterruptIn leftSwitch(D2);
InterruptIn rightSwitch(D3);

int driveSpeed = 0;

double angularPosNew = 0;
double angularPosOld = 0;
double dAngle = 0.0f;
double PIPI = 6.28;
float anSpd = 0.0f;
float prevAnSpd = 0.0f;
float beforeOffset = 0;

float timeOld = 0.0f;
float timeStart = 0.0f;
float proceeded = 0.0f;
float test= 0.0f;

void Rx_interrupt();

bool canSend = false;

RawSerial rpc(D1,D0,9600);

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

my_union myUnion;

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 angle=0.f;
float angleOffset=0;

float getPendulumAngle(){
    angle = getPendulumPos();
    angle = angle * 6.28f / 1024.0f;
    beforeOffset = angle;
    angle += angleOffset;
    /*if (angle >= angleOffset) {
        angle += angleOffset;
    } else {
        //angle += (angleOffset + 0.244346);
        //angle += angleOffset + 0.23970866018;
        //angle += angleOffset;
        angle -= angleOffset;
    }*/
    if (angle > PIPI + 0.01) {
        proceeded = fmod((angle + 3.14), PIPI) - 3.14;
        if (proceeded < -3.14) {
            proceeded += PIPI;
        }
/*        if (proceeded < 0) {
            proceeded = -proceeded;
        }*/
        angle = proceeded;
    }
    test = fmodf(angle,M_PI);
    if (test > 1) {
        test = -test;    
    }

    return angle;
}

void deltaPos(){
    
    /*
    for(int i=0;i<smoothingNumber;i++){
        angularPosNew+=getPendulumAngle();
        //wait_ms(1);
        }
    angularPosNew/=smoothingNumber;
    
    */
    angularPosNew = getPendulumAngle();

    dAngle = fmod((angularPosNew - angularPosOld + 3.14), PIPI) - 3.14;
    if (dAngle < -3.14) {
        dAngle += PIPI;
    }
     
    /*
    if (dAngle < 0) {
        dAngle += 6.34;
    }
    if (dAngle > M_PI) {
        dAngle -= 6.34;
    }
    */
    angularPosOld = angularPosNew;
}



void angularSpeed(){
    float deltaTime;
    timeStart = float(t.read());
    deltaTime = (timeStart - timeOld);
    deltaPos();
    anSpd = (dAngle) / deltaTime;
    /*
    int test = anSpd;
    if (test == 0) {
        anSpd = 0;
    }*/
    timeOld=timeStart;
}


void Rx_interrupt() {

    int password = rpc.getc();
    if (password == 50) {
        canSend = true;
    }

    return;
}

void leftEnd() {
    }


void rightEnd() {
    }

void sendData() {
    angularSpeed();
    myUnion.number[0] = t.read();
    myUnion.number[1] = angleOffset;
    myUnion.number[2] = beforeOffset;
    myUnion.number[3] = 228.0;
    myUnion.number[4] = angularPosNew;
    myUnion.number[5] = test;
    for(int i = 0 ; i < 24; i++) {
        rpc.putc(myUnion.numberCh[i]);    
    }
}

int main() {
    leftSwitch.rise(&leftEnd);
    rightSwitch.rise(&rightEnd);
    spi.format(16,2);
    spi.frequency(1000000);
    t.start();
    for (int i=5; i>0; i--) {
        getPendulumAngle();
        wait_ms(500);
    }
    angleOffset= 3.14 - angle;
    rpc.attach(&Rx_interrupt, Serial::RxIrq);
    while(1) {
    wait_us(5);
    if (canSend) {
        sendData();
    }
    //wait_ms(8);
    //angleOffset=angle;
    //printf("deltaPos: %f\r\nspeed: %f\r\n",getPendulumAngle(),angularSpeed());
    //printf("dPos: %f, without %f spd: %f \r\n",getPendulumAngle(), getPendulumAngle() - 0.006191, angularSpeed());
    //printf("speed: %f\r\n",angularSpeed());
    }
}