![](/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@45:a32e8091901b, 2017-09-12 (annotated)
- Committer:
- echo_piyo
- Date:
- Tue Sep 12 08:29:39 2017 +0000
- Revision:
- 45:a32e8091901b
- Parent:
- 0:bf96e953cdb8
(?)SBDBT??????????????get_pairingState()??????; SBDBT??????????????Nucleo?Output????if????; ?????????????0.0?????Nucleo????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
echo_piyo | 0:bf96e953cdb8 | 1 | /* |
echo_piyo | 45:a32e8091901b | 2 | ver1.0 |
echo_piyo | 45:a32e8091901b | 3 | Sbdbt Name(tx,rx) |
echo_piyo | 45:a32e8091901b | 4 | sbdbtに接続するtx,rx |
echo_piyo | 45:a32e8091901b | 5 | |
echo_piyo | 45:a32e8091901b | 6 | ver2.0 |
echo_piyo | 0:bf96e953cdb8 | 7 | Sbdbt Name(tx,rx) |
echo_piyo | 0:bf96e953cdb8 | 8 | sbdbtに接続するtx,rx |
echo_piyo | 0:bf96e953cdb8 | 9 | |
echo_piyo | 0:bf96e953cdb8 | 10 | begin(baudrate) |
echo_piyo | 0:bf96e953cdb8 | 11 | sbdbtと通信するbaudrateを決定 |
echo_piyo | 0:bf96e953cdb8 | 12 | |
echo_piyo | 0:bf96e953cdb8 | 13 | ボタンデータ 0/1 |
echo_piyo | 0:bf96e953cdb8 | 14 | shikaku, l1, l2, r1, r2, start, select, |
echo_piyo | 0:bf96e953cdb8 | 15 | up, down, right, left, sankaku, batu, maru |
echo_piyo | 0:bf96e953cdb8 | 16 | |
echo_piyo | 0:bf96e953cdb8 | 17 | アナログスティックデータ -1 ~ 1 |
echo_piyo | 0:bf96e953cdb8 | 18 | left_x, left_y, right_x, right_y; |
echo_piyo | 0:bf96e953cdb8 | 19 | |
echo_piyo | 0:bf96e953cdb8 | 20 | sbdbtから受信したデータ |
echo_piyo | 0:bf96e953cdb8 | 21 | open_data[data_byte]; |
echo_piyo | 0:bf96e953cdb8 | 22 | */ |
echo_piyo | 0:bf96e953cdb8 | 23 | |
echo_piyo | 0:bf96e953cdb8 | 24 | #define start_byte 0b10000000 |
echo_piyo | 0:bf96e953cdb8 | 25 | #define Shikaku 0 |
echo_piyo | 0:bf96e953cdb8 | 26 | #define L1 1 |
echo_piyo | 0:bf96e953cdb8 | 27 | #define L2 2 |
echo_piyo | 0:bf96e953cdb8 | 28 | #define R1 3 |
echo_piyo | 0:bf96e953cdb8 | 29 | #define R2 4 |
echo_piyo | 0:bf96e953cdb8 | 30 | #define Start 0b00000011 |
echo_piyo | 0:bf96e953cdb8 | 31 | #define Select 0b00001100 |
echo_piyo | 0:bf96e953cdb8 | 32 | #define Up 0 |
echo_piyo | 0:bf96e953cdb8 | 33 | #define Down 1 |
echo_piyo | 0:bf96e953cdb8 | 34 | #define Right 2 |
echo_piyo | 0:bf96e953cdb8 | 35 | #define Left 3 |
echo_piyo | 0:bf96e953cdb8 | 36 | #define Sankaku 4 |
echo_piyo | 0:bf96e953cdb8 | 37 | #define Batu 5 |
echo_piyo | 0:bf96e953cdb8 | 38 | #define Maru 6 |
echo_piyo | 0:bf96e953cdb8 | 39 | #define EvenNeutral 0b01000000 |
echo_piyo | 0:bf96e953cdb8 | 40 | #define OddNeutral 0b00111111 |
echo_piyo | 0:bf96e953cdb8 | 41 | #define data_byte 8 |
echo_piyo | 0:bf96e953cdb8 | 42 | #define input_byte 7 |
echo_piyo | 0:bf96e953cdb8 | 43 | |
echo_piyo | 0:bf96e953cdb8 | 44 | class Sbdbt |
echo_piyo | 0:bf96e953cdb8 | 45 | { |
echo_piyo | 0:bf96e953cdb8 | 46 | public : |
echo_piyo | 45:a32e8091901b | 47 | Sbdbt(PinName mbed_tx, PinName mbed_rx, PinName pin_pairing) : SBDBT(mbed_tx,mbed_rx), pairing(pin_pairing){ |
echo_piyo | 0:bf96e953cdb8 | 48 | } |
echo_piyo | 0:bf96e953cdb8 | 49 | |
echo_piyo | 0:bf96e953cdb8 | 50 | void begin(long baudrate) { |
echo_piyo | 0:bf96e953cdb8 | 51 | init(); |
echo_piyo | 0:bf96e953cdb8 | 52 | SBDBT.baud(baudrate); |
echo_piyo | 0:bf96e953cdb8 | 53 | SBDBT.attach(this, &Sbdbt::data_receive, Serial::RxIrq); |
echo_piyo | 0:bf96e953cdb8 | 54 | } |
echo_piyo | 45:a32e8091901b | 55 | |
echo_piyo | 45:a32e8091901b | 56 | int get_pairingState(){ |
echo_piyo | 45:a32e8091901b | 57 | return pairing; |
echo_piyo | 45:a32e8091901b | 58 | } |
echo_piyo | 0:bf96e953cdb8 | 59 | |
echo_piyo | 0:bf96e953cdb8 | 60 | short shikaku; |
echo_piyo | 0:bf96e953cdb8 | 61 | short l1; |
echo_piyo | 0:bf96e953cdb8 | 62 | short l2; |
echo_piyo | 0:bf96e953cdb8 | 63 | short r1; |
echo_piyo | 0:bf96e953cdb8 | 64 | short r2; |
echo_piyo | 0:bf96e953cdb8 | 65 | short start; |
echo_piyo | 0:bf96e953cdb8 | 66 | short select; |
echo_piyo | 0:bf96e953cdb8 | 67 | short up; |
echo_piyo | 0:bf96e953cdb8 | 68 | short down; |
echo_piyo | 0:bf96e953cdb8 | 69 | short right; |
echo_piyo | 0:bf96e953cdb8 | 70 | short left; |
echo_piyo | 0:bf96e953cdb8 | 71 | short sankaku; |
echo_piyo | 0:bf96e953cdb8 | 72 | short batu; |
echo_piyo | 0:bf96e953cdb8 | 73 | short maru; |
echo_piyo | 0:bf96e953cdb8 | 74 | float left_x; |
echo_piyo | 0:bf96e953cdb8 | 75 | float left_y; |
echo_piyo | 0:bf96e953cdb8 | 76 | float right_x; |
echo_piyo | 0:bf96e953cdb8 | 77 | float right_y; |
echo_piyo | 0:bf96e953cdb8 | 78 | int open_data[data_byte]; |
echo_piyo | 0:bf96e953cdb8 | 79 | |
echo_piyo | 0:bf96e953cdb8 | 80 | private : |
echo_piyo | 0:bf96e953cdb8 | 81 | Serial SBDBT; |
echo_piyo | 45:a32e8091901b | 82 | DigitalIn pairing; |
echo_piyo | 0:bf96e953cdb8 | 83 | |
echo_piyo | 0:bf96e953cdb8 | 84 | int read, data_start, checksum, byte, |
echo_piyo | 0:bf96e953cdb8 | 85 | buffer[data_byte], data[data_byte]; |
echo_piyo | 0:bf96e953cdb8 | 86 | |
echo_piyo | 0:bf96e953cdb8 | 87 | int Left_X, Left_Y, Right_X, Right_Y ; |
echo_piyo | 45:a32e8091901b | 88 | |
echo_piyo | 0:bf96e953cdb8 | 89 | void init() { |
echo_piyo | 0:bf96e953cdb8 | 90 | open_data[0] = buffer[0] = data[0] = 128; |
echo_piyo | 0:bf96e953cdb8 | 91 | open_data[1] = buffer[1] = data[1] = 0; |
echo_piyo | 0:bf96e953cdb8 | 92 | open_data[2] = buffer[2] = data[2] = 0; |
echo_piyo | 0:bf96e953cdb8 | 93 | open_data[3] = buffer[3] = data[3] = 64; |
echo_piyo | 0:bf96e953cdb8 | 94 | open_data[4] = buffer[4] = data[4] = 64; |
echo_piyo | 0:bf96e953cdb8 | 95 | open_data[5] = buffer[5] = data[5] = 64; |
echo_piyo | 0:bf96e953cdb8 | 96 | open_data[6] = buffer[6] = data[6] = 64; |
echo_piyo | 0:bf96e953cdb8 | 97 | open_data[7] = buffer[7] = data[7] = 0; |
echo_piyo | 0:bf96e953cdb8 | 98 | } |
echo_piyo | 0:bf96e953cdb8 | 99 | |
echo_piyo | 0:bf96e953cdb8 | 100 | void check() { |
echo_piyo | 0:bf96e953cdb8 | 101 | if (bit_test(data[1],Shikaku)) { |
echo_piyo | 0:bf96e953cdb8 | 102 | shikaku = 1; |
echo_piyo | 0:bf96e953cdb8 | 103 | } else { |
echo_piyo | 0:bf96e953cdb8 | 104 | shikaku = 0; |
echo_piyo | 0:bf96e953cdb8 | 105 | } |
echo_piyo | 0:bf96e953cdb8 | 106 | if (bit_test(data[1],L1)) { |
echo_piyo | 0:bf96e953cdb8 | 107 | l1 = 1; |
echo_piyo | 0:bf96e953cdb8 | 108 | } else { |
echo_piyo | 0:bf96e953cdb8 | 109 | l1 = 0; |
echo_piyo | 0:bf96e953cdb8 | 110 | } |
echo_piyo | 0:bf96e953cdb8 | 111 | if (bit_test(data[1],L2)) { |
echo_piyo | 0:bf96e953cdb8 | 112 | l2 = 1; |
echo_piyo | 0:bf96e953cdb8 | 113 | } else { |
echo_piyo | 0:bf96e953cdb8 | 114 | l2 = 0; |
echo_piyo | 0:bf96e953cdb8 | 115 | } |
echo_piyo | 0:bf96e953cdb8 | 116 | if (bit_test(data[1],R1)) { |
echo_piyo | 0:bf96e953cdb8 | 117 | r1 = 1; |
echo_piyo | 0:bf96e953cdb8 | 118 | } else { |
echo_piyo | 0:bf96e953cdb8 | 119 | r1 = 0; |
echo_piyo | 0:bf96e953cdb8 | 120 | } |
echo_piyo | 0:bf96e953cdb8 | 121 | if (bit_test(data[1],R2)) { |
echo_piyo | 0:bf96e953cdb8 | 122 | r2 = 1; |
echo_piyo | 0:bf96e953cdb8 | 123 | } else { |
echo_piyo | 0:bf96e953cdb8 | 124 | r2 = 0; |
echo_piyo | 0:bf96e953cdb8 | 125 | } |
echo_piyo | 0:bf96e953cdb8 | 126 | if ((data[2] & Start) == Start) { |
echo_piyo | 0:bf96e953cdb8 | 127 | start = 1; |
echo_piyo | 0:bf96e953cdb8 | 128 | up = 0; |
echo_piyo | 0:bf96e953cdb8 | 129 | down = 0; |
echo_piyo | 0:bf96e953cdb8 | 130 | } else { |
echo_piyo | 0:bf96e953cdb8 | 131 | start = 0; |
echo_piyo | 0:bf96e953cdb8 | 132 | if (bit_test(data[2],Up)) { |
echo_piyo | 0:bf96e953cdb8 | 133 | up = 1; |
echo_piyo | 0:bf96e953cdb8 | 134 | } else { |
echo_piyo | 0:bf96e953cdb8 | 135 | up = 0; |
echo_piyo | 0:bf96e953cdb8 | 136 | } |
echo_piyo | 0:bf96e953cdb8 | 137 | if (bit_test(data[2],Down)) { |
echo_piyo | 0:bf96e953cdb8 | 138 | down = 1; |
echo_piyo | 0:bf96e953cdb8 | 139 | } else { |
echo_piyo | 0:bf96e953cdb8 | 140 | down = 0; |
echo_piyo | 0:bf96e953cdb8 | 141 | } |
echo_piyo | 0:bf96e953cdb8 | 142 | } |
echo_piyo | 0:bf96e953cdb8 | 143 | if ((data[2] & Select) == Select) { |
echo_piyo | 0:bf96e953cdb8 | 144 | select = 1; |
echo_piyo | 0:bf96e953cdb8 | 145 | right = 0; |
echo_piyo | 0:bf96e953cdb8 | 146 | left = 0; |
echo_piyo | 0:bf96e953cdb8 | 147 | } else { |
echo_piyo | 0:bf96e953cdb8 | 148 | select = 0; |
echo_piyo | 0:bf96e953cdb8 | 149 | if (bit_test(data[2],Right)) { |
echo_piyo | 0:bf96e953cdb8 | 150 | right = 1; |
echo_piyo | 0:bf96e953cdb8 | 151 | } else { |
echo_piyo | 0:bf96e953cdb8 | 152 | right = 0; |
echo_piyo | 0:bf96e953cdb8 | 153 | } |
echo_piyo | 0:bf96e953cdb8 | 154 | if (bit_test(data[2],Left)) { |
echo_piyo | 0:bf96e953cdb8 | 155 | left = 1; |
echo_piyo | 0:bf96e953cdb8 | 156 | } else { |
echo_piyo | 0:bf96e953cdb8 | 157 | left = 0; |
echo_piyo | 0:bf96e953cdb8 | 158 | } |
echo_piyo | 0:bf96e953cdb8 | 159 | } |
echo_piyo | 0:bf96e953cdb8 | 160 | if (bit_test(data[2],Sankaku)) { |
echo_piyo | 0:bf96e953cdb8 | 161 | sankaku = 1; |
echo_piyo | 0:bf96e953cdb8 | 162 | } else { |
echo_piyo | 0:bf96e953cdb8 | 163 | sankaku = 0; |
echo_piyo | 0:bf96e953cdb8 | 164 | } |
echo_piyo | 0:bf96e953cdb8 | 165 | if (bit_test(data[2],Batu)) { |
echo_piyo | 0:bf96e953cdb8 | 166 | batu = 1; |
echo_piyo | 0:bf96e953cdb8 | 167 | } else { |
echo_piyo | 0:bf96e953cdb8 | 168 | batu = 0; |
echo_piyo | 0:bf96e953cdb8 | 169 | } |
echo_piyo | 0:bf96e953cdb8 | 170 | if (bit_test(data[2],Maru)) { |
echo_piyo | 0:bf96e953cdb8 | 171 | maru = 1; |
echo_piyo | 0:bf96e953cdb8 | 172 | } else { |
echo_piyo | 0:bf96e953cdb8 | 173 | maru = 0; |
echo_piyo | 0:bf96e953cdb8 | 174 | } |
echo_piyo | 0:bf96e953cdb8 | 175 | |
echo_piyo | 0:bf96e953cdb8 | 176 | Left_X = data[3]; |
echo_piyo | 0:bf96e953cdb8 | 177 | Left_Y = data[4]; |
echo_piyo | 0:bf96e953cdb8 | 178 | Right_X = data[5]; |
echo_piyo | 0:bf96e953cdb8 | 179 | Right_Y = data[6]; |
echo_piyo | 0:bf96e953cdb8 | 180 | |
echo_piyo | 0:bf96e953cdb8 | 181 | if (Left_X <= EvenNeutral) { |
echo_piyo | 0:bf96e953cdb8 | 182 | left_x = (float) EvenNeutral - Left_X; |
echo_piyo | 0:bf96e953cdb8 | 183 | } else { |
echo_piyo | 0:bf96e953cdb8 | 184 | left_x = (float) OddNeutral - Left_X; |
echo_piyo | 0:bf96e953cdb8 | 185 | } |
echo_piyo | 0:bf96e953cdb8 | 186 | |
echo_piyo | 0:bf96e953cdb8 | 187 | if (Left_Y <= EvenNeutral) { |
echo_piyo | 0:bf96e953cdb8 | 188 | left_y = (float) EvenNeutral - Left_Y; |
echo_piyo | 0:bf96e953cdb8 | 189 | } else { |
echo_piyo | 0:bf96e953cdb8 | 190 | left_y = (float) OddNeutral - Left_Y; |
echo_piyo | 0:bf96e953cdb8 | 191 | } |
echo_piyo | 0:bf96e953cdb8 | 192 | |
echo_piyo | 0:bf96e953cdb8 | 193 | if (Right_X <= EvenNeutral) { |
echo_piyo | 0:bf96e953cdb8 | 194 | right_x = (float) EvenNeutral - Right_X; |
echo_piyo | 0:bf96e953cdb8 | 195 | } else { |
echo_piyo | 0:bf96e953cdb8 | 196 | right_x = (float) OddNeutral - Right_X; |
echo_piyo | 0:bf96e953cdb8 | 197 | } |
echo_piyo | 0:bf96e953cdb8 | 198 | |
echo_piyo | 0:bf96e953cdb8 | 199 | if (Right_Y <= EvenNeutral) { |
echo_piyo | 0:bf96e953cdb8 | 200 | right_y = (float) EvenNeutral - Right_Y; |
echo_piyo | 0:bf96e953cdb8 | 201 | } else { |
echo_piyo | 0:bf96e953cdb8 | 202 | right_y = (float) OddNeutral - Right_Y; |
echo_piyo | 0:bf96e953cdb8 | 203 | } |
echo_piyo | 0:bf96e953cdb8 | 204 | |
echo_piyo | 0:bf96e953cdb8 | 205 | left_x = -left_x / EvenNeutral; |
echo_piyo | 0:bf96e953cdb8 | 206 | left_y = left_y / EvenNeutral; |
echo_piyo | 0:bf96e953cdb8 | 207 | right_x = -right_x / EvenNeutral; |
echo_piyo | 0:bf96e953cdb8 | 208 | right_y = right_y / EvenNeutral; |
echo_piyo | 0:bf96e953cdb8 | 209 | |
echo_piyo | 0:bf96e953cdb8 | 210 | if (fabs(left_x) < 0.24f) { |
echo_piyo | 0:bf96e953cdb8 | 211 | left_x = 0; |
echo_piyo | 0:bf96e953cdb8 | 212 | } |
echo_piyo | 0:bf96e953cdb8 | 213 | if (fabs(left_y) < 0.24f) { |
echo_piyo | 0:bf96e953cdb8 | 214 | left_y = 0; |
echo_piyo | 0:bf96e953cdb8 | 215 | } |
echo_piyo | 0:bf96e953cdb8 | 216 | } |
echo_piyo | 0:bf96e953cdb8 | 217 | |
echo_piyo | 0:bf96e953cdb8 | 218 | void data_receive () { |
echo_piyo | 0:bf96e953cdb8 | 219 | read = SBDBT.getc(); |
echo_piyo | 0:bf96e953cdb8 | 220 | if (read == start_byte) { |
echo_piyo | 0:bf96e953cdb8 | 221 | data_start = 1; |
echo_piyo | 0:bf96e953cdb8 | 222 | checksum = 0; |
echo_piyo | 0:bf96e953cdb8 | 223 | byte = 1; |
echo_piyo | 0:bf96e953cdb8 | 224 | buffer[0] = read; |
echo_piyo | 0:bf96e953cdb8 | 225 | } else { |
echo_piyo | 0:bf96e953cdb8 | 226 | if (data_start == 1) { |
echo_piyo | 0:bf96e953cdb8 | 227 | buffer[byte] = read; |
echo_piyo | 0:bf96e953cdb8 | 228 | byte++; |
echo_piyo | 0:bf96e953cdb8 | 229 | } |
echo_piyo | 0:bf96e953cdb8 | 230 | if (byte > input_byte) { |
echo_piyo | 0:bf96e953cdb8 | 231 | int i; |
echo_piyo | 0:bf96e953cdb8 | 232 | for (i = 1 ; i < input_byte; i++) { |
echo_piyo | 0:bf96e953cdb8 | 233 | checksum = checksum + buffer[i]; |
echo_piyo | 0:bf96e953cdb8 | 234 | } |
echo_piyo | 0:bf96e953cdb8 | 235 | if ((checksum & 0b01111111) == buffer[7]) { |
echo_piyo | 0:bf96e953cdb8 | 236 | for (i = 0; i < data_byte; i++) { |
echo_piyo | 0:bf96e953cdb8 | 237 | data[i] = buffer[i]; |
echo_piyo | 0:bf96e953cdb8 | 238 | open_data[i] = data[i]; |
echo_piyo | 0:bf96e953cdb8 | 239 | } |
echo_piyo | 0:bf96e953cdb8 | 240 | check(); |
echo_piyo | 0:bf96e953cdb8 | 241 | } |
echo_piyo | 0:bf96e953cdb8 | 242 | byte = 0; |
echo_piyo | 0:bf96e953cdb8 | 243 | data_start = 0; |
echo_piyo | 0:bf96e953cdb8 | 244 | } |
echo_piyo | 0:bf96e953cdb8 | 245 | } |
echo_piyo | 0:bf96e953cdb8 | 246 | } |
echo_piyo | 0:bf96e953cdb8 | 247 | }; |