first
Dependents: 17robo_fuzi 17robo_tokyo_kaede
sbdbt.cpp@0:59256c750167, 2017-09-24 (annotated)
- Committer:
- echo_piyo
- Date:
- Sun Sep 24 05:24:33 2017 +0000
- Revision:
- 0:59256c750167
????2??
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
echo_piyo | 0:59256c750167 | 1 | |
echo_piyo | 0:59256c750167 | 2 | #include "sbdbt.h" |
echo_piyo | 0:59256c750167 | 3 | #include "mbed.h" |
echo_piyo | 0:59256c750167 | 4 | |
echo_piyo | 0:59256c750167 | 5 | Sbdbt::Sbdbt(PinName mbed_tx, PinName mbed_rx, PinName pin_pairing) : SBDBT(mbed_tx,mbed_rx), pairing(pin_pairing){ |
echo_piyo | 0:59256c750167 | 6 | } |
echo_piyo | 0:59256c750167 | 7 | |
echo_piyo | 0:59256c750167 | 8 | void Sbdbt::begin(long baudrate){ |
echo_piyo | 0:59256c750167 | 9 | init(); |
echo_piyo | 0:59256c750167 | 10 | SBDBT.baud(baudrate); |
echo_piyo | 0:59256c750167 | 11 | SBDBT.attach(this, &Sbdbt::data_receive, Serial::RxIrq); |
echo_piyo | 0:59256c750167 | 12 | } |
echo_piyo | 0:59256c750167 | 13 | |
echo_piyo | 0:59256c750167 | 14 | int Sbdbt::get_pairingState(){ |
echo_piyo | 0:59256c750167 | 15 | return pairing; |
echo_piyo | 0:59256c750167 | 16 | } |
echo_piyo | 0:59256c750167 | 17 | |
echo_piyo | 0:59256c750167 | 18 | void Sbdbt::init(){ |
echo_piyo | 0:59256c750167 | 19 | open_data[0] = buffer[0] = data[0] = 128; |
echo_piyo | 0:59256c750167 | 20 | open_data[1] = buffer[1] = data[1] = 0; |
echo_piyo | 0:59256c750167 | 21 | open_data[2] = buffer[2] = data[2] = 0; |
echo_piyo | 0:59256c750167 | 22 | open_data[3] = buffer[3] = data[3] = 64; |
echo_piyo | 0:59256c750167 | 23 | open_data[4] = buffer[4] = data[4] = 64; |
echo_piyo | 0:59256c750167 | 24 | open_data[5] = buffer[5] = data[5] = 64; |
echo_piyo | 0:59256c750167 | 25 | open_data[6] = buffer[6] = data[6] = 64; |
echo_piyo | 0:59256c750167 | 26 | open_data[7] = buffer[7] = data[7] = 0; |
echo_piyo | 0:59256c750167 | 27 | } |
echo_piyo | 0:59256c750167 | 28 | |
echo_piyo | 0:59256c750167 | 29 | void Sbdbt::check(){ |
echo_piyo | 0:59256c750167 | 30 | if (bitTest(data[1],Shikaku)) { |
echo_piyo | 0:59256c750167 | 31 | shikaku = 1; |
echo_piyo | 0:59256c750167 | 32 | } else { |
echo_piyo | 0:59256c750167 | 33 | shikaku = 0; |
echo_piyo | 0:59256c750167 | 34 | } |
echo_piyo | 0:59256c750167 | 35 | if (bitTest(data[1],L1)) { |
echo_piyo | 0:59256c750167 | 36 | l1 = 1; |
echo_piyo | 0:59256c750167 | 37 | } else { |
echo_piyo | 0:59256c750167 | 38 | l1 = 0; |
echo_piyo | 0:59256c750167 | 39 | } |
echo_piyo | 0:59256c750167 | 40 | if (bitTest(data[1],L2)) { |
echo_piyo | 0:59256c750167 | 41 | l2 = 1; |
echo_piyo | 0:59256c750167 | 42 | } else { |
echo_piyo | 0:59256c750167 | 43 | l2 = 0; |
echo_piyo | 0:59256c750167 | 44 | } |
echo_piyo | 0:59256c750167 | 45 | if (bitTest(data[1],R1)) { |
echo_piyo | 0:59256c750167 | 46 | r1 = 1; |
echo_piyo | 0:59256c750167 | 47 | } else { |
echo_piyo | 0:59256c750167 | 48 | r1 = 0; |
echo_piyo | 0:59256c750167 | 49 | } |
echo_piyo | 0:59256c750167 | 50 | if (bitTest(data[1],R2)) { |
echo_piyo | 0:59256c750167 | 51 | r2 = 1; |
echo_piyo | 0:59256c750167 | 52 | } else { |
echo_piyo | 0:59256c750167 | 53 | r2 = 0; |
echo_piyo | 0:59256c750167 | 54 | } |
echo_piyo | 0:59256c750167 | 55 | if ((data[2] & Start) == Start) { |
echo_piyo | 0:59256c750167 | 56 | start = 1; |
echo_piyo | 0:59256c750167 | 57 | up = 0; |
echo_piyo | 0:59256c750167 | 58 | down = 0; |
echo_piyo | 0:59256c750167 | 59 | } else { |
echo_piyo | 0:59256c750167 | 60 | start = 0; |
echo_piyo | 0:59256c750167 | 61 | if (bitTest(data[2],Up)) { |
echo_piyo | 0:59256c750167 | 62 | up = 1; |
echo_piyo | 0:59256c750167 | 63 | } else { |
echo_piyo | 0:59256c750167 | 64 | up = 0; |
echo_piyo | 0:59256c750167 | 65 | } |
echo_piyo | 0:59256c750167 | 66 | if (bitTest(data[2],Down)) { |
echo_piyo | 0:59256c750167 | 67 | down = 1; |
echo_piyo | 0:59256c750167 | 68 | } else { |
echo_piyo | 0:59256c750167 | 69 | down = 0; |
echo_piyo | 0:59256c750167 | 70 | } |
echo_piyo | 0:59256c750167 | 71 | } |
echo_piyo | 0:59256c750167 | 72 | if ((data[2] & Select) == Select) { |
echo_piyo | 0:59256c750167 | 73 | select = 1; |
echo_piyo | 0:59256c750167 | 74 | right = 0; |
echo_piyo | 0:59256c750167 | 75 | left = 0; |
echo_piyo | 0:59256c750167 | 76 | } else { |
echo_piyo | 0:59256c750167 | 77 | select = 0; |
echo_piyo | 0:59256c750167 | 78 | if (bitTest(data[2],Right)) { |
echo_piyo | 0:59256c750167 | 79 | right = 1; |
echo_piyo | 0:59256c750167 | 80 | } else { |
echo_piyo | 0:59256c750167 | 81 | right = 0; |
echo_piyo | 0:59256c750167 | 82 | } |
echo_piyo | 0:59256c750167 | 83 | if (bitTest(data[2],Left)) { |
echo_piyo | 0:59256c750167 | 84 | left = 1; |
echo_piyo | 0:59256c750167 | 85 | } else { |
echo_piyo | 0:59256c750167 | 86 | left = 0; |
echo_piyo | 0:59256c750167 | 87 | } |
echo_piyo | 0:59256c750167 | 88 | } |
echo_piyo | 0:59256c750167 | 89 | if (bitTest(data[2],Sankaku)) { |
echo_piyo | 0:59256c750167 | 90 | sankaku = 1; |
echo_piyo | 0:59256c750167 | 91 | } else { |
echo_piyo | 0:59256c750167 | 92 | sankaku = 0; |
echo_piyo | 0:59256c750167 | 93 | } |
echo_piyo | 0:59256c750167 | 94 | if (bitTest(data[2],Batu)) { |
echo_piyo | 0:59256c750167 | 95 | batu = 1; |
echo_piyo | 0:59256c750167 | 96 | } else { |
echo_piyo | 0:59256c750167 | 97 | batu = 0; |
echo_piyo | 0:59256c750167 | 98 | } |
echo_piyo | 0:59256c750167 | 99 | if (bitTest(data[2],Maru)) { |
echo_piyo | 0:59256c750167 | 100 | maru = 1; |
echo_piyo | 0:59256c750167 | 101 | } else { |
echo_piyo | 0:59256c750167 | 102 | maru = 0; |
echo_piyo | 0:59256c750167 | 103 | } |
echo_piyo | 0:59256c750167 | 104 | |
echo_piyo | 0:59256c750167 | 105 | Left_X = data[3]; |
echo_piyo | 0:59256c750167 | 106 | Left_Y = data[4]; |
echo_piyo | 0:59256c750167 | 107 | Right_X = data[5]; |
echo_piyo | 0:59256c750167 | 108 | Right_Y = data[6]; |
echo_piyo | 0:59256c750167 | 109 | |
echo_piyo | 0:59256c750167 | 110 | if (Left_X <= EvenNeutral) { |
echo_piyo | 0:59256c750167 | 111 | left_x = (float) EvenNeutral - Left_X; |
echo_piyo | 0:59256c750167 | 112 | } else { |
echo_piyo | 0:59256c750167 | 113 | left_x = (float) OddNeutral - Left_X; |
echo_piyo | 0:59256c750167 | 114 | } |
echo_piyo | 0:59256c750167 | 115 | |
echo_piyo | 0:59256c750167 | 116 | if (Left_Y <= EvenNeutral) { |
echo_piyo | 0:59256c750167 | 117 | left_y = (float) EvenNeutral - Left_Y; |
echo_piyo | 0:59256c750167 | 118 | } else { |
echo_piyo | 0:59256c750167 | 119 | left_y = (float) OddNeutral - Left_Y; |
echo_piyo | 0:59256c750167 | 120 | } |
echo_piyo | 0:59256c750167 | 121 | |
echo_piyo | 0:59256c750167 | 122 | if (Right_X <= EvenNeutral) { |
echo_piyo | 0:59256c750167 | 123 | right_x = (float) EvenNeutral - Right_X; |
echo_piyo | 0:59256c750167 | 124 | } else { |
echo_piyo | 0:59256c750167 | 125 | right_x = (float) OddNeutral - Right_X; |
echo_piyo | 0:59256c750167 | 126 | } |
echo_piyo | 0:59256c750167 | 127 | |
echo_piyo | 0:59256c750167 | 128 | if (Right_Y <= EvenNeutral) { |
echo_piyo | 0:59256c750167 | 129 | right_y = (float) EvenNeutral - Right_Y; |
echo_piyo | 0:59256c750167 | 130 | } else { |
echo_piyo | 0:59256c750167 | 131 | right_y = (float) OddNeutral - Right_Y; |
echo_piyo | 0:59256c750167 | 132 | } |
echo_piyo | 0:59256c750167 | 133 | |
echo_piyo | 0:59256c750167 | 134 | left_x = -left_x / EvenNeutral; |
echo_piyo | 0:59256c750167 | 135 | left_y = left_y / EvenNeutral; |
echo_piyo | 0:59256c750167 | 136 | right_x = -right_x / EvenNeutral; |
echo_piyo | 0:59256c750167 | 137 | right_y = right_y / EvenNeutral; |
echo_piyo | 0:59256c750167 | 138 | |
echo_piyo | 0:59256c750167 | 139 | if (fabs(left_x) < 0.24f) { |
echo_piyo | 0:59256c750167 | 140 | left_x = 0; |
echo_piyo | 0:59256c750167 | 141 | } |
echo_piyo | 0:59256c750167 | 142 | if (fabs(left_y) < 0.24f) { |
echo_piyo | 0:59256c750167 | 143 | left_y = 0; |
echo_piyo | 0:59256c750167 | 144 | } |
echo_piyo | 0:59256c750167 | 145 | } |
echo_piyo | 0:59256c750167 | 146 | |
echo_piyo | 0:59256c750167 | 147 | void Sbdbt::data_receive (){ |
echo_piyo | 0:59256c750167 | 148 | read = SBDBT.getc(); |
echo_piyo | 0:59256c750167 | 149 | if (read == start_byte) { |
echo_piyo | 0:59256c750167 | 150 | data_start = 1; |
echo_piyo | 0:59256c750167 | 151 | checksum = 0; |
echo_piyo | 0:59256c750167 | 152 | byte = 1; |
echo_piyo | 0:59256c750167 | 153 | buffer[0] = read; |
echo_piyo | 0:59256c750167 | 154 | } else { |
echo_piyo | 0:59256c750167 | 155 | if (data_start == 1) { |
echo_piyo | 0:59256c750167 | 156 | buffer[byte] = read; |
echo_piyo | 0:59256c750167 | 157 | byte++; |
echo_piyo | 0:59256c750167 | 158 | } |
echo_piyo | 0:59256c750167 | 159 | if (byte > input_byte) { |
echo_piyo | 0:59256c750167 | 160 | int i; |
echo_piyo | 0:59256c750167 | 161 | for (i = 1 ; i < input_byte; i++) { |
echo_piyo | 0:59256c750167 | 162 | checksum = checksum + buffer[i]; |
echo_piyo | 0:59256c750167 | 163 | } |
echo_piyo | 0:59256c750167 | 164 | if ((checksum & 0b01111111) == buffer[7]) { |
echo_piyo | 0:59256c750167 | 165 | for (i = 0; i < data_byte; i++) { |
echo_piyo | 0:59256c750167 | 166 | data[i] = buffer[i]; |
echo_piyo | 0:59256c750167 | 167 | open_data[i] = data[i]; |
echo_piyo | 0:59256c750167 | 168 | } |
echo_piyo | 0:59256c750167 | 169 | check(); |
echo_piyo | 0:59256c750167 | 170 | } |
echo_piyo | 0:59256c750167 | 171 | byte = 0; |
echo_piyo | 0:59256c750167 | 172 | data_start = 0; |
echo_piyo | 0:59256c750167 | 173 | } |
echo_piyo | 0:59256c750167 | 174 | } |
echo_piyo | 0:59256c750167 | 175 | } |