![](/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
sbdbt.h
- Committer:
- echo_piyo
- Date:
- 2017-06-26
- Revision:
- 0:bf96e953cdb8
- Child:
- 45:a32e8091901b
File content as of revision 0:bf96e953cdb8:
/* 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) : SBDBT(mbed_tx,mbed_rx) { } void begin(long baudrate) { init(); SBDBT.baud(baudrate); SBDBT.attach(this, &Sbdbt::data_receive, Serial::RxIrq); } 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; 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; } } } };