![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sampleProgram
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_fuzi by
Diff: headerfile_unuse/sbdbt_.h
- Revision:
- 66:1664ee92539d
- Parent:
- 45:a32e8091901b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/headerfile_unuse/sbdbt_.h Sun Sep 24 05:25:03 2017 +0000 @@ -0,0 +1,247 @@ +/* +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; + } + } + } +}; \ No newline at end of file