Diff: main.cpp

Revision:

3:8708e61475fe

Parent:

2:32c9df18a589

--- a/main.cpp	Sat May 16 12:09:45 2020 +0000
+++ b/main.cpp	Sat May 16 12:31:09 2020 +0000
@@ -0,0 +1,298 @@
+#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);
+    }
+}
\ No newline at end of file