Diff: sbdbt.cpp

Revision:

0:59256c750167

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sbdbt.cpp	Sun Sep 24 05:24:33 2017 +0000
@@ -0,0 +1,175 @@
+
+#include "sbdbt.h"
+#include "mbed.h"
+
+Sbdbt::Sbdbt(PinName mbed_tx, PinName mbed_rx, PinName pin_pairing) : SBDBT(mbed_tx,mbed_rx), pairing(pin_pairing){
+}
+
+void Sbdbt::begin(long baudrate){
+    init();
+    SBDBT.baud(baudrate);
+    SBDBT.attach(this, &Sbdbt::data_receive, Serial::RxIrq);
+}
+
+int Sbdbt::get_pairingState(){
+    return pairing;
+}
+
+void Sbdbt::init(){
+    open_data[0] = buffer[0] = data[0] = 128;
+    open_data[1] = buffer[1] = data[1] = 0;
+    open_data[2] = buffer[2] = data[2] = 0;
+    open_data[3] = buffer[3] = data[3] = 64;
+    open_data[4] = buffer[4] = data[4] = 64;
+    open_data[5] = buffer[5] = data[5] = 64;
+    open_data[6] = buffer[6] = data[6] = 64;
+    open_data[7] = buffer[7] = data[7] = 0;
+}
+
+void Sbdbt::check(){
+    if (bitTest(data[1],Shikaku)) {
+        shikaku = 1;
+    } else {
+        shikaku = 0;
+    }
+    if (bitTest(data[1],L1)) {
+        l1 = 1;
+    } else {
+        l1 = 0;
+    }
+    if (bitTest(data[1],L2)) {
+        l2 = 1;
+    } else {
+        l2 = 0;
+    }
+    if (bitTest(data[1],R1)) {
+        r1 = 1;
+    } else {
+        r1 = 0;
+    }
+    if (bitTest(data[1],R2)) {
+        r2 = 1;
+    } else {
+        r2 = 0;
+    }
+    if ((data[2] & Start) == Start) {
+        start = 1;
+        up = 0;
+        down = 0;
+    } else {
+        start = 0;
+        if (bitTest(data[2],Up)) {
+            up = 1;
+        } else {
+            up = 0;
+        }
+        if (bitTest(data[2],Down)) {
+            down = 1;
+        } else {
+            down = 0;
+        }
+    }
+    if ((data[2] & Select) == Select) {
+        select = 1;
+        right = 0;
+        left = 0;
+    } else {
+        select = 0;
+        if (bitTest(data[2],Right)) {
+            right = 1;
+        } else {
+            right = 0;
+        }
+        if (bitTest(data[2],Left)) {
+            left = 1;
+        } else {
+            left = 0;
+        }
+    }
+    if (bitTest(data[2],Sankaku)) {
+        sankaku = 1;
+    } else {
+        sankaku = 0;
+    }
+    if (bitTest(data[2],Batu)) {
+        batu = 1;
+    } else {
+        batu = 0;
+    }
+    if (bitTest(data[2],Maru)) {
+        maru = 1;
+    } else {
+        maru = 0;
+    }
+
+    Left_X  = data[3];
+    Left_Y  = data[4];
+    Right_X = data[5];
+    Right_Y = data[6];
+
+    if (Left_X <= EvenNeutral) {
+        left_x = (float) EvenNeutral - Left_X;
+    } else {
+        left_x = (float) OddNeutral  - Left_X;
+    }
+
+    if (Left_Y <= EvenNeutral) {
+        left_y = (float) EvenNeutral - Left_Y;
+    } else {
+        left_y = (float) OddNeutral  - Left_Y;
+    }
+
+    if (Right_X <= EvenNeutral) {
+        right_x = (float) EvenNeutral - Right_X;
+    } else {
+        right_x = (float) OddNeutral  - Right_X;
+    }
+
+    if (Right_Y <= EvenNeutral) {
+        right_y = (float) EvenNeutral - Right_Y;
+    } else {
+        right_y = (float) OddNeutral  - Right_Y;
+    }
+
+    left_x  = -left_x  / EvenNeutral;
+    left_y  = left_y  / EvenNeutral;
+    right_x = -right_x / EvenNeutral;
+    right_y = right_y / EvenNeutral;
+
+    if (fabs(left_x) < 0.24f) {
+        left_x = 0;
+    }
+    if (fabs(left_y) < 0.24f) {
+        left_y = 0;
+    }
+}
+
+void Sbdbt::data_receive (){
+    read = SBDBT.getc();
+    if (read == start_byte) {
+        data_start = 1;
+        checksum = 0;
+        byte = 1;
+        buffer[0] = read;
+    } else {
+        if (data_start == 1) {
+            buffer[byte] = read;
+            byte++;
+        }
+        if (byte > input_byte) {
+            int i;
+            for (i = 1 ; i < input_byte; i++) {
+                checksum = checksum + buffer[i];
+            }
+            if ((checksum & 0b01111111) == buffer[7]) {
+                for (i = 0; i < data_byte; i++) {
+                    data[i] = buffer[i];
+                    open_data[i] = data[i];
+                }
+                check();
+            }
+            byte = 0;
+            data_start = 0;
+        }
+    }
+}
\ No newline at end of file