kusano kiyoshige / Mbed 2 deprecated 17robo_fuzi

ライブラリ化を行った後

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

headerfile_unuse/sbdbt_.h@77:17469192b53a, 2017-09-28 (annotated)

Committer:
echo_piyo
Date:
Thu Sep 28 02:25:00 2017 +0000
Revision:
77:17469192b53a
Parent:
66:1664ee92539d 
(?)????(?????)????????????????????????????????????????????

Who changed what in which revision?

UserRevisionLine numberNew 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 };