kusano kiyoshige / sbdbt

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.

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?

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