File content as of revision 13:205002e3c176:

#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

#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;//300 26
int posCounter = 0;
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 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;
    }
    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 (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 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;
    }*/
}


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 changeDir() {
    if (dir == DIR_LEFT) {
        dir = DIR_RIGHT;    
    } else {
        dir = DIR_LEFT;
    }    
}

int command[2] = {0, 0};
bool newData = false;

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

void proceedCommands() {
    newData = false;
    switch (data[0]) {
        case 50:
            canSend = true;
            break;
        case 60:
            led = !led;
            state = STATE_GOTO_SWING;
            break;
        case 65:
            led = !led;
            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;
    calibrated = true;
    led = !led;
    //wait(12);
    //wait(3);
    //updatePeriod();
    calcControl();
    tick.attach_us(stepperFlip, timer_period_us);
    state=STATE_GOTO_START;
    dir=DIR_LEFT;
    //swingTicker.attach(changeDir, 1.5);
    while(1) {
        getAngularSpeed();
        if (canSend) {
            sendData();
        }
        if (newData) {
            proceedCommands();    
        }
        /*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;
        }
    }
}