ライブラリ化を行った後

Dependencies:   QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo

Fork of 17robo_Practice1 by kusano kiyoshige

sbdbt.h

Committer:
echo_piyo
Date:
2017-09-12
Revision:
45:a32e8091901b
Parent:
0:bf96e953cdb8

File content as of revision 45:a32e8091901b:

/*
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];
*/

#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) : SBDBT(mbed_tx,mbed_rx), pairing(pin_pairing){
    }

    void begin(long baudrate) {
        init();
        SBDBT.baud(baudrate);
        SBDBT.attach(this, &Sbdbt::data_receive, Serial::RxIrq);
    }
    
    int get_pairingState(){
        return pairing;
    }

    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() {
        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 check() {
        if (bit_test(data[1],Shikaku)) {
            shikaku = 1;
        } else {
            shikaku = 0;
        }
        if (bit_test(data[1],L1)) {
            l1 = 1;
        } else {
            l1 = 0;
        }
        if (bit_test(data[1],L2)) {
            l2 = 1;
        } else {
            l2 = 0;
        }
        if (bit_test(data[1],R1)) {
            r1 = 1;
        } else {
            r1 = 0;
        }
        if (bit_test(data[1],R2)) {
            r2 = 1;
        } else {
            r2 = 0;
        }
        if ((data[2] & Start) == Start) {
            start = 1;
            up = 0;
            down = 0;
        } else {
            start = 0;
            if (bit_test(data[2],Up)) {
                up = 1;
            } else {
                up = 0;
            }
            if (bit_test(data[2],Down)) {
                down = 1;
            } else {
                down = 0;
            }
        }
        if ((data[2] & Select) == Select) {
            select = 1;
            right = 0;
            left = 0;
        } else {
            select = 0;
            if (bit_test(data[2],Right)) {
                right = 1;
            } else {
                right = 0;
            }
            if (bit_test(data[2],Left)) {
                left = 1;
            } else {
                left = 0;
            }
        }
        if (bit_test(data[2],Sankaku)) {
            sankaku = 1;
        } else {
            sankaku = 0;
        }
        if (bit_test(data[2],Batu)) {
            batu = 1;
        } else {
            batu = 0;
        }
        if (bit_test(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 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;
            }
        }
    }
};