FRA221-2018 / Mbed 2 deprecated PJ12_glove

Code glove

Dependencies:   mbed MPU6050

main.cpp@0:b8ed5a8d189c, 2018-12-19 (annotated)

Committer:
pingnpp
Date:
Wed Dec 19 08:11:56 2018 +0000
Revision:
0:b8ed5a8d189c
PJ12_glove

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pingnpp 0:b8ed5a8d189c 1 #include "mbed.h"
pingnpp 0:b8ed5a8d189c 2 #include "MPU6050.h"
pingnpp 0:b8ed5a8d189c 3
pingnpp 0:b8ed5a8d189c 4 Serial pc(PA_2, PA_3);
pingnpp 0:b8ed5a8d189c 5 Serial bt(PA_9, PA_10);
pingnpp 0:b8ed5a8d189c 6
pingnpp 0:b8ed5a8d189c 7 AnalogIn force(PA_7);
pingnpp 0:b8ed5a8d189c 8 MPU6050 AccGyro(PB_7, PB_6);
pingnpp 0:b8ed5a8d189c 9 AnalogIn flex[] = {(PA_0), (PA_1), (PA_4), (PA_5), (PA_6)};
pingnpp 0:b8ed5a8d189c 10 DigitalOut LED(LED1);
pingnpp 0:b8ed5a8d189c 11 float min[] = {1.000, 1.000, 1.000, 1.000, 1.000};
pingnpp 0:b8ed5a8d189c 12 float max[] = {0.000, 0.000, 0.000, 0.000, 0.000};
pingnpp 0:b8ed5a8d189c 13 float avg[] = {0.000, 0.000, 0.000, 0.000, 0.000};
pingnpp 0:b8ed5a8d189c 14 double Ax, Ay, Az, readd;
pingnpp 0:b8ed5a8d189c 15 double Gx, Gy, Gz;
pingnpp 0:b8ed5a8d189c 16 int memoryTime;
pingnpp 0:b8ed5a8d189c 17 uint16_t AccelReadings[3] = {0, 0, 0};
pingnpp 0:b8ed5a8d189c 18 uint16_t GyroReadings[3] = {0, 0, 0};
pingnpp 0:b8ed5a8d189c 19 short int step, step1, step2, step3, step4, step5, memm, stepOn = 0, memOn = 0, chargeOn = 0, stepOff = 0, memOff = 0, chargeOff = 0, num = 0, dataFinger, charge = 0;
pingnpp 0:b8ed5a8d189c 20 uint8_t DevId, elec = 0, sendData, mem = 0;
pingnpp 0:b8ed5a8d189c 21 bool shutDown;
pingnpp 0:b8ed5a8d189c 22 Timer t, minn, timeOn, timeOff, see;
pingnpp 0:b8ed5a8d189c 23
pingnpp 0:b8ed5a8d189c 24 short int boolFlex(int a){
pingnpp 0:b8ed5a8d189c 25 return (float)flex[a].read() >= avg[a];
pingnpp 0:b8ed5a8d189c 26 }
pingnpp 0:b8ed5a8d189c 27 float readFlex(int a){
pingnpp 0:b8ed5a8d189c 28 return (float)flex[a].read();
pingnpp 0:b8ed5a8d189c 29 }
pingnpp 0:b8ed5a8d189c 30 short int finger(){
pingnpp 0:b8ed5a8d189c 31 short int data = 0;
pingnpp 0:b8ed5a8d189c 32 short int pos = 1;
pingnpp 0:b8ed5a8d189c 33 for(int i = 4; i >= 0; i--){
pingnpp 0:b8ed5a8d189c 34 data += boolFlex(i)*pos;
pingnpp 0:b8ed5a8d189c 35 pos *= 2;
pingnpp 0:b8ed5a8d189c 36 }
pingnpp 0:b8ed5a8d189c 37 return data;
pingnpp 0:b8ed5a8d189c 38 }
pingnpp 0:b8ed5a8d189c 39 void updateIMU(){
pingnpp 0:b8ed5a8d189c 40 AccGyro.readAccel(AccelReadings);
pingnpp 0:b8ed5a8d189c 41 AccGyro.readGyro(GyroReadings);
pingnpp 0:b8ed5a8d189c 42 Ax = (double)(int16_t)AccelReadings[0];
pingnpp 0:b8ed5a8d189c 43 Ay = (double)(int16_t)AccelReadings[1];
pingnpp 0:b8ed5a8d189c 44 Az = (double)(int16_t)AccelReadings[2];
pingnpp 0:b8ed5a8d189c 45 Gx = (double)(int16_t)GyroReadings[0] / 131.0;
pingnpp 0:b8ed5a8d189c 46 Gy = (double)(int16_t)GyroReadings[1] / 131.0;
pingnpp 0:b8ed5a8d189c 47 Gz = (double)(int16_t)GyroReadings[2] / 131.0;
pingnpp 0:b8ed5a8d189c 48 }
pingnpp 0:b8ed5a8d189c 49 bool range(int a, int b, int c){
pingnpp 0:b8ed5a8d189c 50 return a > b && a < c;
pingnpp 0:b8ed5a8d189c 51 }
pingnpp 0:b8ed5a8d189c 52 void ping(){
pingnpp 0:b8ed5a8d189c 53 if(shutDown) shutDown = false;
pingnpp 0:b8ed5a8d189c 54 else shutDown = true;
pingnpp 0:b8ed5a8d189c 55 }
pingnpp 0:b8ed5a8d189c 56
pingnpp 0:b8ed5a8d189c 57
pingnpp 0:b8ed5a8d189c 58
pingnpp 0:b8ed5a8d189c 59 bool memFan = false, memAir = false, memKettle = false, memTv = false, memRadio = false;
pingnpp 0:b8ed5a8d189c 60
pingnpp 0:b8ed5a8d189c 61
pingnpp 0:b8ed5a8d189c 62
pingnpp 0:b8ed5a8d189c 63 void onOff(uint8_t on, uint8_t off){
pingnpp 0:b8ed5a8d189c 64 dataFinger = finger();
pingnpp 0:b8ed5a8d189c 65 if(Ax > 1400){
pingnpp 0:b8ed5a8d189c 66 if(dataFinger == 31 && stepOn != 1) stepOn = 1;
pingnpp 0:b8ed5a8d189c 67 else if(dataFinger != 31 && dataFinger != 16 && stepOn == 1){
pingnpp 0:b8ed5a8d189c 68 if(memOn == 0){
pingnpp 0:b8ed5a8d189c 69 timeOn.reset();
pingnpp 0:b8ed5a8d189c 70 memOn = 1;
pingnpp 0:b8ed5a8d189c 71 }
pingnpp 0:b8ed5a8d189c 72 else if(timeOn.read_ms() >= 1000){
pingnpp 0:b8ed5a8d189c 73 stepOn = 0;
pingnpp 0:b8ed5a8d189c 74 memOn = 0;
pingnpp 0:b8ed5a8d189c 75 chargeOn = 0;
pingnpp 0:b8ed5a8d189c 76 }
pingnpp 0:b8ed5a8d189c 77 }
pingnpp 0:b8ed5a8d189c 78 else if(dataFinger == 16 && stepOn == 1){
pingnpp 0:b8ed5a8d189c 79 chargeOn += 1;
pingnpp 0:b8ed5a8d189c 80 stepOn = 0;
pingnpp 0:b8ed5a8d189c 81 mem = 0;
pingnpp 0:b8ed5a8d189c 82 }
pingnpp 0:b8ed5a8d189c 83 }
pingnpp 0:b8ed5a8d189c 84 else if(timeOn.read_ms() > 2000){
pingnpp 0:b8ed5a8d189c 85 stepOn = 0;
pingnpp 0:b8ed5a8d189c 86 memOn = 0;
pingnpp 0:b8ed5a8d189c 87 chargeOn = 0;
pingnpp 0:b8ed5a8d189c 88 }
pingnpp 0:b8ed5a8d189c 89 if(chargeOn >= 2){
pingnpp 0:b8ed5a8d189c 90 bt.printf("%c", on);
pingnpp 0:b8ed5a8d189c 91 pc.printf("%c\n", on);
pingnpp 0:b8ed5a8d189c 92 chargeOn = 0;
pingnpp 0:b8ed5a8d189c 93 }
pingnpp 0:b8ed5a8d189c 94 if(dataFinger == 31){
pingnpp 0:b8ed5a8d189c 95 if(Gz < -20) stepOff = 1;
pingnpp 0:b8ed5a8d189c 96 else if(Gz > -20.00 && Gz < 20.00 && stepOff == 1){
pingnpp 0:b8ed5a8d189c 97 if(memOff == 0){
pingnpp 0:b8ed5a8d189c 98 timeOff.reset();
pingnpp 0:b8ed5a8d189c 99 memOff = 1;
pingnpp 0:b8ed5a8d189c 100 }
pingnpp 0:b8ed5a8d189c 101 else if(timeOff.read_ms() >= 1000){
pingnpp 0:b8ed5a8d189c 102 stepOff = 0;
pingnpp 0:b8ed5a8d189c 103 memOff = 0;
pingnpp 0:b8ed5a8d189c 104 chargeOff = 0;
pingnpp 0:b8ed5a8d189c 105 }
pingnpp 0:b8ed5a8d189c 106 }
pingnpp 0:b8ed5a8d189c 107 else if(Gz > 20 && stepOff == 1){
pingnpp 0:b8ed5a8d189c 108 chargeOff += 1;
pingnpp 0:b8ed5a8d189c 109 stepOff = 0;
pingnpp 0:b8ed5a8d189c 110 memOff = 0;
pingnpp 0:b8ed5a8d189c 111 }
pingnpp 0:b8ed5a8d189c 112 else if(timeOff.read_ms() > 2000){
pingnpp 0:b8ed5a8d189c 113 stepOff = 0;
pingnpp 0:b8ed5a8d189c 114 memOff = 0;
pingnpp 0:b8ed5a8d189c 115 chargeOff = 0;
pingnpp 0:b8ed5a8d189c 116 }
pingnpp 0:b8ed5a8d189c 117 if(chargeOff >= 4){
pingnpp 0:b8ed5a8d189c 118 bt.printf("%c", off);
pingnpp 0:b8ed5a8d189c 119 pc.printf("%c\n", off);
pingnpp 0:b8ed5a8d189c 120 chargeOff = 0;
pingnpp 0:b8ed5a8d189c 121 }
pingnpp 0:b8ed5a8d189c 122 }
pingnpp 0:b8ed5a8d189c 123 }
pingnpp 0:b8ed5a8d189c 124
pingnpp 0:b8ed5a8d189c 125 void set0(){
pingnpp 0:b8ed5a8d189c 126 stepOn = 0;
pingnpp 0:b8ed5a8d189c 127 memOn = 0;
pingnpp 0:b8ed5a8d189c 128 chargeOn = 0;
pingnpp 0:b8ed5a8d189c 129 stepOff = 0;
pingnpp 0:b8ed5a8d189c 130 memOff = 0;
pingnpp 0:b8ed5a8d189c 131 chargeOff = 0;
pingnpp 0:b8ed5a8d189c 132 step1 = 0;
pingnpp 0:b8ed5a8d189c 133 step2 = 0;
pingnpp 0:b8ed5a8d189c 134 step3 = 0;
pingnpp 0:b8ed5a8d189c 135 step4 = 0;
pingnpp 0:b8ed5a8d189c 136 // step5 = 0;
pingnpp 0:b8ed5a8d189c 137 }
pingnpp 0:b8ed5a8d189c 138
pingnpp 0:b8ed5a8d189c 139
pingnpp 0:b8ed5a8d189c 140
pingnpp 0:b8ed5a8d189c 141 int main() {
pingnpp 0:b8ed5a8d189c 142 pc.baud(38400);
pingnpp 0:b8ed5a8d189c 143 bt.baud(38400);
pingnpp 0:b8ed5a8d189c 144 while((float)force.read() <= 0.8){ //calibrate flex
pingnpp 0:b8ed5a8d189c 145 if(num >= 5) num = 0;
pingnpp 0:b8ed5a8d189c 146 readd = readFlex(num);
pingnpp 0:b8ed5a8d189c 147 if(readd <= min[num]) min[num] = readd;
pingnpp 0:b8ed5a8d189c 148 if(readd >= max[num]) max[num] = readd;
pingnpp 0:b8ed5a8d189c 149 num += 1;
pingnpp 0:b8ed5a8d189c 150 }
pingnpp 0:b8ed5a8d189c 151 for(int i = 0; i < 5; i++) avg[i] = (min[i] + max[i])/2;
pingnpp 0:b8ed5a8d189c 152 pc.printf("\n\n\n%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n", avg[0], avg[1], avg[2], avg[3], avg[4]);
pingnpp 0:b8ed5a8d189c 153 DevId = AccGyro.getWhoAmI();
pingnpp 0:b8ed5a8d189c 154 while(DevId != 0x71) DevId = AccGyro.getWhoAmI();
pingnpp 0:b8ed5a8d189c 155 LED = 1;
pingnpp 0:b8ed5a8d189c 156 AccGyro.setPowerCtl_1(0x00, 0x00, 0x00, 0x00, INT_8MHz_OSC); // Disable sleep mode
pingnpp 0:b8ed5a8d189c 157 wait_ms(1);
pingnpp 0:b8ed5a8d189c 158 AccGyro.setGyroConfig(GYRO_ST_OFF, GFS_2000dps); // Accelerometer elf-test trigger off.
pingnpp 0:b8ed5a8d189c 159 wait_ms(1);
pingnpp 0:b8ed5a8d189c 160 AccGyro.setAccelConfig(ACC_ST_OFF, AFS_16g); // Gyroscope self-test trigger off.
pingnpp 0:b8ed5a8d189c 161 wait_ms(1);
pingnpp 0:b8ed5a8d189c 162 InterruptIn force(PA_7);
pingnpp 0:b8ed5a8d189c 163 shutDown = false;
pingnpp 0:b8ed5a8d189c 164 minn.start();
pingnpp 0:b8ed5a8d189c 165 see.start();
pingnpp 0:b8ed5a8d189c 166 force.rise(&ping);
pingnpp 0:b8ed5a8d189c 167 // while(1){
pingnpp 0:b8ed5a8d189c 168 // updateIMU();
pingnpp 0:b8ed5a8d189c 169 // pc.printf("flex : %d\tAccel\tX: %.3f\tY: %.3f\tZ: %.3f\tGyro\tX: %.3f\tY: %.3f\tZ: %.3f\n", finger(), Ax, Ay, Az, Gx, Gy, Gz);
pingnpp 0:b8ed5a8d189c 170 // }
pingnpp 0:b8ed5a8d189c 171 // while(1){
pingnpp 0:b8ed5a8d189c 172 // updateIMU();
pingnpp 0:b8ed5a8d189c 173 // onOff('A', 'B');
pingnpp 0:b8ed5a8d189c 174 // pc.printf("step : %d\tmem : %d\tcharge : %d\tstep : %d\tmem : %d\tcharge : %d\n", stepOn, memOn, chargeOn, stepOff, memOff, chargeOff);
pingnpp 0:b8ed5a8d189c 175 // }
pingnpp 0:b8ed5a8d189c 176 while(1){
pingnpp 0:b8ed5a8d189c 177 if(!shutDown){
pingnpp 0:b8ed5a8d189c 178 if(bt.readable()){
pingnpp 0:b8ed5a8d189c 179 elec = (uint8_t)bt.getc()-48;
pingnpp 0:b8ed5a8d189c 180 pc.printf("\t\t\t%d\n", elec);
pingnpp 0:b8ed5a8d189c 181 }
pingnpp 0:b8ed5a8d189c 182 updateIMU();
pingnpp 0:b8ed5a8d189c 183 /*if(minn.read_ms() >= 60000 && elec != 0){
pingnpp 0:b8ed5a8d189c 184 bt.printf("%c", elec + 83 S);
pingnpp 0:b8ed5a8d189c 185 elec = 0;
pingnpp 0:b8ed5a8d189c 186 }*/
pingnpp 0:b8ed5a8d189c 187 while(elec == 0){
pingnpp 0:b8ed5a8d189c 188 if(bt.readable()){
pingnpp 0:b8ed5a8d189c 189 elec = (uint8_t)bt.getc()-48;
pingnpp 0:b8ed5a8d189c 190 pc.printf("\t%d\n", elec);
pingnpp 0:b8ed5a8d189c 191 }
pingnpp 0:b8ed5a8d189c 192 updateIMU();
pingnpp 0:b8ed5a8d189c 193 if(Ax > 1400){
pingnpp 0:b8ed5a8d189c 194 if(step != 1) step = 1;
pingnpp 0:b8ed5a8d189c 195 dataFinger = finger();
pingnpp 0:b8ed5a8d189c 196 if(dataFinger == 8) sendData = 1;
pingnpp 0:b8ed5a8d189c 197 else if(dataFinger == 12) sendData = 3;
pingnpp 0:b8ed5a8d189c 198 else if(dataFinger == 14) sendData = 5;
pingnpp 0:b8ed5a8d189c 199 else if(dataFinger == 15) sendData = 4;
pingnpp 0:b8ed5a8d189c 200 else if(dataFinger == 0) sendData = 2;
pingnpp 0:b8ed5a8d189c 201 else sendData = 6;
pingnpp 0:b8ed5a8d189c 202 if(minn.read_ms() >= 10){
pingnpp 0:b8ed5a8d189c 203 bt.printf("%c", sendData+48);
pingnpp 0:b8ed5a8d189c 204 pc.printf("%d\n", sendData);
pingnpp 0:b8ed5a8d189c 205 minn.reset();
pingnpp 0:b8ed5a8d189c 206 }
pingnpp 0:b8ed5a8d189c 207 }
pingnpp 0:b8ed5a8d189c 208 else if(range(Ay, -1000, 1000) && Gy > 0 && sendData < 6){
pingnpp 0:b8ed5a8d189c 209 if(step == 1) step = 2;
pingnpp 0:b8ed5a8d189c 210 }
pingnpp 0:b8ed5a8d189c 211 else if(Az > 1800 && step == 2){
pingnpp 0:b8ed5a8d189c 212 while(minn.read_ms() < 10);
pingnpp 0:b8ed5a8d189c 213 bt.printf("%c", 'a'+sendData);
pingnpp 0:b8ed5a8d189c 214 pc.printf("%c\n", 'a'+sendData);
pingnpp 0:b8ed5a8d189c 215 minn.reset();
pingnpp 0:b8ed5a8d189c 216 elec = sendData;
pingnpp 0:b8ed5a8d189c 217 step = 0;
pingnpp 0:b8ed5a8d189c 218 sendData = 6;
pingnpp 0:b8ed5a8d189c 219 set0();
pingnpp 0:b8ed5a8d189c 220 while(1){
pingnpp 0:b8ed5a8d189c 221 if(bt.readable()) elec = (uint8_t)bt.getc()-48;
pingnpp 0:b8ed5a8d189c 222 if(elec != 0) break;
pingnpp 0:b8ed5a8d189c 223 }
pingnpp 0:b8ed5a8d189c 224 break;
pingnpp 0:b8ed5a8d189c 225 pc.printf("yeah");
pingnpp 0:b8ed5a8d189c 226 }
pingnpp 0:b8ed5a8d189c 227 else{
pingnpp 0:b8ed5a8d189c 228 if(step != 0){
pingnpp 0:b8ed5a8d189c 229 step = 0;
pingnpp 0:b8ed5a8d189c 230 }
pingnpp 0:b8ed5a8d189c 231 }
pingnpp 0:b8ed5a8d189c 232 if(step == 0 && minn.read_ms() >= 10){
pingnpp 0:b8ed5a8d189c 233 sendData = 0;
pingnpp 0:b8ed5a8d189c 234 bt.printf("%c", 'a');
pingnpp 0:b8ed5a8d189c 235 pc.printf("%d\n", sendData);
pingnpp 0:b8ed5a8d189c 236 minn.reset();
pingnpp 0:b8ed5a8d189c 237 }
pingnpp 0:b8ed5a8d189c 238 }
pingnpp 0:b8ed5a8d189c 239 updateIMU();
pingnpp 0:b8ed5a8d189c 240 if(elec == 1){ //fan
pingnpp 0:b8ed5a8d189c 241 dataFinger = finger();
pingnpp 0:b8ed5a8d189c 242 onOff('A', 'B');
pingnpp 0:b8ed5a8d189c 243 if(Az < -800){
pingnpp 0:b8ed5a8d189c 244 if(dataFinger == 31 && step1 != 1) step1 = 1;
pingnpp 0:b8ed5a8d189c 245 else if(dataFinger == 16 && step1 == 1){
pingnpp 0:b8ed5a8d189c 246 bt.printf("%c", 'D');
pingnpp 0:b8ed5a8d189c 247 pc.printf("%c\n", 'D');
pingnpp 0:b8ed5a8d189c 248 step1 = 2;
pingnpp 0:b8ed5a8d189c 249 }
pingnpp 0:b8ed5a8d189c 250 }
pingnpp 0:b8ed5a8d189c 251 else if(Az > 1500){
pingnpp 0:b8ed5a8d189c 252 if(dataFinger == 31 && step1 != 3) step1 = 3;
pingnpp 0:b8ed5a8d189c 253 else if(dataFinger == 16 && step1 == 3){
pingnpp 0:b8ed5a8d189c 254 bt.printf("%c", 'C');
pingnpp 0:b8ed5a8d189c 255 pc.printf("%c\n", 'C');
pingnpp 0:b8ed5a8d189c 256 step1 = 4;
pingnpp 0:b8ed5a8d189c 257 }
pingnpp 0:b8ed5a8d189c 258 }
pingnpp 0:b8ed5a8d189c 259 else step1 = 0;
pingnpp 0:b8ed5a8d189c 260 }
pingnpp 0:b8ed5a8d189c 261 else if(elec == 2){ //air
pingnpp 0:b8ed5a8d189c 262 dataFinger = finger();
pingnpp 0:b8ed5a8d189c 263 onOff('E', 'F');
pingnpp 0:b8ed5a8d189c 264 if(dataFinger == 28){
pingnpp 0:b8ed5a8d189c 265 if(Gz > 50 && step2 !=1) step2 = 1;
pingnpp 0:b8ed5a8d189c 266 else if(Gz < 10 && step2 == 1){
pingnpp 0:b8ed5a8d189c 267 bt.printf("%c", 'H');
pingnpp 0:b8ed5a8d189c 268 pc.printf("%c\n", 'H');
pingnpp 0:b8ed5a8d189c 269 step2 = 2;
pingnpp 0:b8ed5a8d189c 270 }
pingnpp 0:b8ed5a8d189c 271 }
pingnpp 0:b8ed5a8d189c 272 else if(dataFinger == 24){
pingnpp 0:b8ed5a8d189c 273 if(Gz > 20 && step2 !=4) step2 = 4;
pingnpp 0:b8ed5a8d189c 274 else if(Gz < 20 && step2 == 4){
pingnpp 0:b8ed5a8d189c 275 bt.printf("%c", 'G');
pingnpp 0:b8ed5a8d189c 276 pc.printf("%c\n", 'G');
pingnpp 0:b8ed5a8d189c 277 step2 = 5;
pingnpp 0:b8ed5a8d189c 278 }
pingnpp 0:b8ed5a8d189c 279 }
pingnpp 0:b8ed5a8d189c 280 else step2 = 0;
pingnpp 0:b8ed5a8d189c 281 }
pingnpp 0:b8ed5a8d189c 282 else if(elec == 3){ //kettle
pingnpp 0:b8ed5a8d189c 283 dataFinger = finger();
pingnpp 0:b8ed5a8d189c 284 onOff('I', 'J');
pingnpp 0:b8ed5a8d189c 285 }
pingnpp 0:b8ed5a8d189c 286 else if(elec == 4){ //tv
pingnpp 0:b8ed5a8d189c 287 dataFinger = finger();
pingnpp 0:b8ed5a8d189c 288 onOff('K', 'L');
pingnpp 0:b8ed5a8d189c 289 if(dataFinger == 16){
pingnpp 0:b8ed5a8d189c 290 if(Ay < -800 && step4 != 3) step4 = 3;
pingnpp 0:b8ed5a8d189c 291 else if(Ay > 1000 && step4 == 3){
pingnpp 0:b8ed5a8d189c 292 bt.printf("%c", 'M');
pingnpp 0:b8ed5a8d189c 293 pc.printf("%c\n", 'M');
pingnpp 0:b8ed5a8d189c 294 step4 = 4;
pingnpp 0:b8ed5a8d189c 295 }
pingnpp 0:b8ed5a8d189c 296 if(Ay > 1400 && step4 != 5) step4 = 5;
pingnpp 0:b8ed5a8d189c 297 else if(Ay < -300 && step4 == 5){
pingnpp 0:b8ed5a8d189c 298 bt.printf("%c", 'N');
pingnpp 0:b8ed5a8d189c 299 pc.printf("%c\n", 'N');
pingnpp 0:b8ed5a8d189c 300 step4 = 6;
pingnpp 0:b8ed5a8d189c 301 }
pingnpp 0:b8ed5a8d189c 302 }
pingnpp 0:b8ed5a8d189c 303 }
pingnpp 0:b8ed5a8d189c 304 else if(elec == 5){ //radio
pingnpp 0:b8ed5a8d189c 305 dataFinger = finger();
pingnpp 0:b8ed5a8d189c 306 onOff('O', 'P');
pingnpp 0:b8ed5a8d189c 307 if(dataFinger == 16){
pingnpp 0:b8ed5a8d189c 308 if(Gx > 20 && step5 != 1){
pingnpp 0:b8ed5a8d189c 309 bt.printf("%c", 'U');
pingnpp 0:b8ed5a8d189c 310 pc.printf("%c\n", 'U');
pingnpp 0:b8ed5a8d189c 311 step5 = 1;
pingnpp 0:b8ed5a8d189c 312 }
pingnpp 0:b8ed5a8d189c 313
pingnpp 0:b8ed5a8d189c 314 if(Ay < -800 && step5 != 3) step5 = 3;
pingnpp 0:b8ed5a8d189c 315 else if(Ay > 1000 && step5 == 3){
pingnpp 0:b8ed5a8d189c 316 bt.printf("%c", 'Q');
pingnpp 0:b8ed5a8d189c 317 pc.printf("%c\n", 'Q');
pingnpp 0:b8ed5a8d189c 318 step5 = 4;
pingnpp 0:b8ed5a8d189c 319 }
pingnpp 0:b8ed5a8d189c 320 if(Ay > 1400 && step5 != 5) step5 = 5;
pingnpp 0:b8ed5a8d189c 321 else if(Ay < -300 && step5 == 5){
pingnpp 0:b8ed5a8d189c 322 bt.printf("%c", 'R');
pingnpp 0:b8ed5a8d189c 323 pc.printf("%c\n", 'R');
pingnpp 0:b8ed5a8d189c 324 step5 = 6;
pingnpp 0:b8ed5a8d189c 325 }
pingnpp 0:b8ed5a8d189c 326 }
pingnpp 0:b8ed5a8d189c 327 if(dataFinger == 24){
pingnpp 0:b8ed5a8d189c 328 if(Gx < -20&& step5 != 2){
pingnpp 0:b8ed5a8d189c 329 bt.printf("%c", 'T');
pingnpp 0:b8ed5a8d189c 330 pc.printf("%c\n", 'T');
pingnpp 0:b8ed5a8d189c 331 step5 = 2;
pingnpp 0:b8ed5a8d189c 332 }
pingnpp 0:b8ed5a8d189c 333 }
pingnpp 0:b8ed5a8d189c 334 }
pingnpp 0:b8ed5a8d189c 335 }
pingnpp 0:b8ed5a8d189c 336 }
pingnpp 0:b8ed5a8d189c 337 }