first

Dependents:   17robo_fuzi 17robo_tokyo_kaede

Files at this revision

API Documentation at this revision

Comitter:
echo_piyo
Date:
Sun Sep 24 05:24:33 2017 +0000
Commit message:
????2??

Changed in this revision

sbdbt.cpp Show annotated file Show diff for this revision Revisions of this file
sbdbt.h Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 59256c750167 sbdbt.cpp
--- /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
diff -r 000000000000 -r 59256c750167 sbdbt.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sbdbt.h	Sun Sep 24 05:24:33 2017 +0000
@@ -0,0 +1,89 @@
+/*
+ver1.0
+Sbdbt Name(tx,rx)
+sbdbtに接続するtx,rx
+
+ver2.0
+Sbdbt Name(tx,rx)
+sbdbtに接続するtx,rx
+
+begin(baudrate)
+sbdbtと通信するbaudrateを決定
+
+ボタンデータ 0/1
+shikaku, l1, l2, r1, r2, start, select, 
+up, down, right, left, sankaku, batu, maru
+
+アナログスティックデータ -1 ~ 1
+left_x, left_y, right_x, right_y;
+
+sbdbtから受信したデータ
+open_data[data_byte];
+*/
+
+#ifndef MBED_SBDBT_H
+#define MBED_SBDBT_H
+
+#include "mbed.h"
+#include "bit_test.h"
+
+#define start_byte 0b10000000
+#define Shikaku 0
+#define L1      1
+#define L2      2
+#define R1      3
+#define R2      4
+#define Start   0b00000011
+#define Select  0b00001100
+#define Up      0
+#define Down    1
+#define Right   2
+#define Left    3
+#define Sankaku 4
+#define Batu    5
+#define Maru    6
+#define EvenNeutral 0b01000000
+#define OddNeutral  0b00111111
+#define data_byte  8
+#define input_byte 7
+
+class Sbdbt{
+public  :
+    Sbdbt(PinName mbed_tx, PinName mbed_rx, PinName pin_pairing);
+    void begin(long baudrate);
+    int get_pairingState();
+    
+    short shikaku;
+    short l1;
+    short l2;
+    short r1;
+    short r2;
+    short start;
+    short select;
+    short up;
+    short down;
+    short right;
+    short left;
+    short sankaku;
+    short batu;
+    short maru;
+    float left_x;
+    float left_y;
+    float right_x;
+    float right_y;
+    int open_data[data_byte];
+    
+private :
+    Serial SBDBT;
+    DigitalIn pairing;
+    
+    int read, data_start, checksum, byte,
+        buffer[data_byte], data[data_byte];
+    int Left_X, Left_Y, Right_X, Right_Y ;
+    
+    void init();
+    void check();
+    void data_receive();
+};
+
+#endif
\ No newline at end of file