Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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 
kusano kiyoshige
sbdbt_.h
00001 /* 00002 ver1.0 00003 Sbdbt Name(tx,rx) 00004 sbdbtに接続するtx,rx 00005 00006 ver2.0 00007 Sbdbt Name(tx,rx) 00008 sbdbtに接続するtx,rx 00009 00010 begin(baudrate) 00011 sbdbtと通信するbaudrateを決定 00012 00013 ボタンデータ 0/1 00014 shikaku, l1, l2, r1, r2, start, select, 00015 up, down, right, left, sankaku, batu, maru 00016 00017 アナログスティックデータ -1 ~ 1 00018 left_x, left_y, right_x, right_y; 00019 00020 sbdbtから受信したデータ 00021 open_data[data_byte]; 00022 */ 00023 00024 #define start_byte 0b10000000 00025 #define Shikaku 0 00026 #define L1 1 00027 #define L2 2 00028 #define R1 3 00029 #define R2 4 00030 #define Start 0b00000011 00031 #define Select 0b00001100 00032 #define Up 0 00033 #define Down 1 00034 #define Right 2 00035 #define Left 3 00036 #define Sankaku 4 00037 #define Batu 5 00038 #define Maru 6 00039 #define EvenNeutral 0b01000000 00040 #define OddNeutral 0b00111111 00041 #define data_byte 8 00042 #define input_byte 7 00043 00044 class Sbdbt 00045 { 00046 public : 00047 Sbdbt(PinName mbed_tx, PinName mbed_rx, PinName pin_pairing) : SBDBT(mbed_tx,mbed_rx), pairing(pin_pairing){ 00048 } 00049 00050 void begin(long baudrate) { 00051 init(); 00052 SBDBT.baud(baudrate); 00053 SBDBT.attach(this, &Sbdbt::data_receive, Serial::RxIrq); 00054 } 00055 00056 int get_pairingState(){ 00057 return pairing; 00058 } 00059 00060 short shikaku; 00061 short l1; 00062 short l2; 00063 short r1; 00064 short r2; 00065 short start; 00066 short select; 00067 short up; 00068 short down; 00069 short right; 00070 short left; 00071 short sankaku; 00072 short batu; 00073 short maru; 00074 float left_x; 00075 float left_y; 00076 float right_x; 00077 float right_y; 00078 int open_data[data_byte]; 00079 00080 private : 00081 Serial SBDBT; 00082 DigitalIn pairing; 00083 00084 int read, data_start, checksum, byte, 00085 buffer[data_byte], data[data_byte]; 00086 00087 int Left_X, Left_Y, Right_X, Right_Y ; 00088 00089 void init() { 00090 open_data[0] = buffer[0] = data[0] = 128; 00091 open_data[1] = buffer[1] = data[1] = 0; 00092 open_data[2] = buffer[2] = data[2] = 0; 00093 open_data[3] = buffer[3] = data[3] = 64; 00094 open_data[4] = buffer[4] = data[4] = 64; 00095 open_data[5] = buffer[5] = data[5] = 64; 00096 open_data[6] = buffer[6] = data[6] = 64; 00097 open_data[7] = buffer[7] = data[7] = 0; 00098 } 00099 00100 void check() { 00101 if (bit_test(data[1],Shikaku)) { 00102 shikaku = 1; 00103 } else { 00104 shikaku = 0; 00105 } 00106 if (bit_test(data[1],L1)) { 00107 l1 = 1; 00108 } else { 00109 l1 = 0; 00110 } 00111 if (bit_test(data[1],L2)) { 00112 l2 = 1; 00113 } else { 00114 l2 = 0; 00115 } 00116 if (bit_test(data[1],R1)) { 00117 r1 = 1; 00118 } else { 00119 r1 = 0; 00120 } 00121 if (bit_test(data[1],R2)) { 00122 r2 = 1; 00123 } else { 00124 r2 = 0; 00125 } 00126 if ((data[2] & Start) == Start) { 00127 start = 1; 00128 up = 0; 00129 down = 0; 00130 } else { 00131 start = 0; 00132 if (bit_test(data[2],Up)) { 00133 up = 1; 00134 } else { 00135 up = 0; 00136 } 00137 if (bit_test(data[2],Down)) { 00138 down = 1; 00139 } else { 00140 down = 0; 00141 } 00142 } 00143 if ((data[2] & Select) == Select) { 00144 select = 1; 00145 right = 0; 00146 left = 0; 00147 } else { 00148 select = 0; 00149 if (bit_test(data[2],Right)) { 00150 right = 1; 00151 } else { 00152 right = 0; 00153 } 00154 if (bit_test(data[2],Left)) { 00155 left = 1; 00156 } else { 00157 left = 0; 00158 } 00159 } 00160 if (bit_test(data[2],Sankaku)) { 00161 sankaku = 1; 00162 } else { 00163 sankaku = 0; 00164 } 00165 if (bit_test(data[2],Batu)) { 00166 batu = 1; 00167 } else { 00168 batu = 0; 00169 } 00170 if (bit_test(data[2],Maru)) { 00171 maru = 1; 00172 } else { 00173 maru = 0; 00174 } 00175 00176 Left_X = data[3]; 00177 Left_Y = data[4]; 00178 Right_X = data[5]; 00179 Right_Y = data[6]; 00180 00181 if (Left_X <= EvenNeutral) { 00182 left_x = (float) EvenNeutral - Left_X; 00183 } else { 00184 left_x = (float) OddNeutral - Left_X; 00185 } 00186 00187 if (Left_Y <= EvenNeutral) { 00188 left_y = (float) EvenNeutral - Left_Y; 00189 } else { 00190 left_y = (float) OddNeutral - Left_Y; 00191 } 00192 00193 if (Right_X <= EvenNeutral) { 00194 right_x = (float) EvenNeutral - Right_X; 00195 } else { 00196 right_x = (float) OddNeutral - Right_X; 00197 } 00198 00199 if (Right_Y <= EvenNeutral) { 00200 right_y = (float) EvenNeutral - Right_Y; 00201 } else { 00202 right_y = (float) OddNeutral - Right_Y; 00203 } 00204 00205 left_x = -left_x / EvenNeutral; 00206 left_y = left_y / EvenNeutral; 00207 right_x = -right_x / EvenNeutral; 00208 right_y = right_y / EvenNeutral; 00209 00210 if (fabs(left_x) < 0.24f) { 00211 left_x = 0; 00212 } 00213 if (fabs(left_y) < 0.24f) { 00214 left_y = 0; 00215 } 00216 } 00217 00218 void data_receive () { 00219 read = SBDBT.getc(); 00220 if (read == start_byte) { 00221 data_start = 1; 00222 checksum = 0; 00223 byte = 1; 00224 buffer[0] = read; 00225 } else { 00226 if (data_start == 1) { 00227 buffer[byte] = read; 00228 byte++; 00229 } 00230 if (byte > input_byte) { 00231 int i; 00232 for (i = 1 ; i < input_byte; i++) { 00233 checksum = checksum + buffer[i]; 00234 } 00235 if ((checksum & 0b01111111) == buffer[7]) { 00236 for (i = 0; i < data_byte; i++) { 00237 data[i] = buffer[i]; 00238 open_data[i] = data[i]; 00239 } 00240 check(); 00241 } 00242 byte = 0; 00243 data_start = 0; 00244 } 00245 } 00246 } 00247 };
Generated on Mon Aug 8 2022 20:27:09 by
1.7.2