Revision 0:06a18c897777, committed 2020-05-28

Comitter:

teamat

Date:

Thu May 28 14:33:13 2020 +0000

Commit message:

Finished

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 28 14:33:13 2020 +0000
@@ -0,0 +1,311 @@
+#include "mbed.h"
+
+#define M_PI 3.14159265358979323846f
+
+#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;
+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 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;
+
+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 twoPI = 6.28;
+
+bool newData = false;
+bool canSend = 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 / 3500.0f;
+}
+
+uint16_t getPendulumPos(){
+    cs=0;
+    wait_ms(1);
+    uint16_t d=spi.write((short)0x00);
+    d=d<<1;
+    d=d>>6;
+    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 - twoPI;    
+    }
+    return angle;
+}
+
+
+/*
+ ANGULAR SPEED CALC
+*/
+
+void getDeltaAng(){
+
+    angularPosNew = getPendulumAngle();
+
+    dAngle = fmod((angularPosNew - angularPosOld + 3.14), twoPI) - 3.14;
+    if (dAngle < -3.14) {
+        dAngle += twoPI;
+    }
+
+    angularPosOld = angularPosNew;
+
+}
+
+void getAngularSpeed(){
+    float deltaTime;
+    timeStart = float(t.read());
+    deltaTime = (timeStart - timeOld);
+    getDeltaAng();
+    anSpd = dAngle / deltaTime;
+    timeOld=timeStart;
+}
+
+/*
+    SPEED CALC
+*/
+
+
+void calcSpeed() {
+    xPosNew = (getPosMM() - posOffset) / 1000;
+    speed = xPosNew - xPosOld;
+    if (dir == DIR_LEFT) {
+        speed = -speed;
+    }
+    xPosOld = xPosNew;
+}
+
+void calcControl() {
+    control = (dir.read() * 2 - 1) *(period_us * timer_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 leftEnd() {
+    dir = DIR_RIGHT;
+    if (state == STATE_GOTO_START) {
+        state = STATE_GOTO_END_COUNTING;
+        pos = 0;
+    }
+    else if (isPendulumSwinging()) {
+        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_ERROR;
+    }
+}
+
+
+void sendData() {
+    myUnion.number[0] = t.read();
+    myUnion.number[1] = (getPosMM()- posOffset) / 1000 ;
+    myUnion.number[2] = speed;
+    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;
+    }    
+}
+
+
+void Rx_interrupt() {
+    data[cmdIndex] = rpc.getc();
+    if (cmdIndex == 1) {
+        newData = true;
+    }
+    cmdIndex = !cmdIndex;
+}
+
+void proceedCommands() {
+    newData = false;
+    switch (data[0]) {
+        case 45:
+            NVIC_SystemReset();
+        case 50:
+            canSend = true;
+            break;
+        case 55:
+            state = STATE_ERROR;
+        case 60:
+            led = !led;
+            state = STATE_GOTO_SWING;
+            break;
+        case 65:
+            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;
+    wait(5);
+    calcControl();
+    tick.attach_us(stepperFlip, timer_period_us);
+    state=STATE_GOTO_START;
+    dir=DIR_LEFT;
+    while(1) {
+        getAngularSpeed();
+        if (canSend) {
+            sendData();
+        }
+        if (newData) {
+            proceedCommands();    
+        }
+        switch(state) {
+            case STATE_WAITING:
+                break;
+            case STATE_GOTO_START:
+                break;
+            case STATE_GOTO_END_COUNTING:
+                break;
+            case STATE_GOTO_SWING:
+                speedTicker.attach(calcSpeed, 1);
+                state = STATE_SWING_LEFT;
+                break;
+            case STATE_SWING_LEFT:
+                break;
+            case STATE_SWING_RIGHT:
+                break;
+            default:
+                break;
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu May 28 14:33:13 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file