kusano kiyoshige / Mbed 2 deprecated 17robo_tokyo_kaede

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 kusano kiyoshige

sbdbt.h@0:bf96e953cdb8, 2017-06-26 (annotated)

Committer:
echo_piyo
Date:
Mon Jun 26 09:59:14 2017 +0000
Revision:
0:bf96e953cdb8
Child:
45:a32e8091901b
????????????????

Who changed what in which revision?

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