Diff: main.cpp

Revision:

0:1cc40073c858

Child:

1:85ab39fea863

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat May 16 12:36:40 2020 +0000
@@ -0,0 +1,164 @@
+#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;
+
+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;
+    if (angle >= angleOffset) {
+        angle -= angleOffset;
+    } else {
+        //angle += (angleOffset + 0.244346);
+        angle += angleOffset + 0.23970866018;
+    }
+    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] = anSpd;
+    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=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());
+    }
+}