FRA221-2018
/
PJ12_glove
Code glove
Diff: main.cpp
- Revision:
- 0:b8ed5a8d189c
diff -r 000000000000 -r b8ed5a8d189c main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Dec 19 08:11:56 2018 +0000 @@ -0,0 +1,337 @@ +#include "mbed.h" +#include "MPU6050.h" + +Serial pc(PA_2, PA_3); +Serial bt(PA_9, PA_10); + +AnalogIn force(PA_7); +MPU6050 AccGyro(PB_7, PB_6); +AnalogIn flex[] = {(PA_0), (PA_1), (PA_4), (PA_5), (PA_6)}; +DigitalOut LED(LED1); +float min[] = {1.000, 1.000, 1.000, 1.000, 1.000}; +float max[] = {0.000, 0.000, 0.000, 0.000, 0.000}; +float avg[] = {0.000, 0.000, 0.000, 0.000, 0.000}; +double Ax, Ay, Az, readd; +double Gx, Gy, Gz; +int memoryTime; +uint16_t AccelReadings[3] = {0, 0, 0}; +uint16_t GyroReadings[3] = {0, 0, 0}; +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; +uint8_t DevId, elec = 0, sendData, mem = 0; +bool shutDown; +Timer t, minn, timeOn, timeOff, see; + +short int boolFlex(int a){ + return (float)flex[a].read() >= avg[a]; +} +float readFlex(int a){ + return (float)flex[a].read(); +} +short int finger(){ + short int data = 0; + short int pos = 1; + for(int i = 4; i >= 0; i--){ + data += boolFlex(i)*pos; + pos *= 2; + } + return data; +} +void updateIMU(){ + AccGyro.readAccel(AccelReadings); + AccGyro.readGyro(GyroReadings); + Ax = (double)(int16_t)AccelReadings[0]; + Ay = (double)(int16_t)AccelReadings[1]; + Az = (double)(int16_t)AccelReadings[2]; + Gx = (double)(int16_t)GyroReadings[0] / 131.0; + Gy = (double)(int16_t)GyroReadings[1] / 131.0; + Gz = (double)(int16_t)GyroReadings[2] / 131.0; +} +bool range(int a, int b, int c){ + return a > b && a < c; +} +void ping(){ + if(shutDown) shutDown = false; + else shutDown = true; +} + + + +bool memFan = false, memAir = false, memKettle = false, memTv = false, memRadio = false; + + + +void onOff(uint8_t on, uint8_t off){ + dataFinger = finger(); + if(Ax > 1400){ + if(dataFinger == 31 && stepOn != 1) stepOn = 1; + else if(dataFinger != 31 && dataFinger != 16 && stepOn == 1){ + if(memOn == 0){ + timeOn.reset(); + memOn = 1; + } + else if(timeOn.read_ms() >= 1000){ + stepOn = 0; + memOn = 0; + chargeOn = 0; + } + } + else if(dataFinger == 16 && stepOn == 1){ + chargeOn += 1; + stepOn = 0; + mem = 0; + } + } + else if(timeOn.read_ms() > 2000){ + stepOn = 0; + memOn = 0; + chargeOn = 0; + } + if(chargeOn >= 2){ + bt.printf("%c", on); + pc.printf("%c\n", on); + chargeOn = 0; + } + if(dataFinger == 31){ + if(Gz < -20) stepOff = 1; + else if(Gz > -20.00 && Gz < 20.00 && stepOff == 1){ + if(memOff == 0){ + timeOff.reset(); + memOff = 1; + } + else if(timeOff.read_ms() >= 1000){ + stepOff = 0; + memOff = 0; + chargeOff = 0; + } + } + else if(Gz > 20 && stepOff == 1){ + chargeOff += 1; + stepOff = 0; + memOff = 0; + } + else if(timeOff.read_ms() > 2000){ + stepOff = 0; + memOff = 0; + chargeOff = 0; + } + if(chargeOff >= 4){ + bt.printf("%c", off); + pc.printf("%c\n", off); + chargeOff = 0; + } + } +} + +void set0(){ + stepOn = 0; + memOn = 0; + chargeOn = 0; + stepOff = 0; + memOff = 0; + chargeOff = 0; + step1 = 0; + step2 = 0; + step3 = 0; + step4 = 0; +// step5 = 0; +} + + + +int main() { + pc.baud(38400); + bt.baud(38400); + while((float)force.read() <= 0.8){ //calibrate flex + if(num >= 5) num = 0; + readd = readFlex(num); + if(readd <= min[num]) min[num] = readd; + if(readd >= max[num]) max[num] = readd; + num += 1; + } + for(int i = 0; i < 5; i++) avg[i] = (min[i] + max[i])/2; + 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]); + DevId = AccGyro.getWhoAmI(); + while(DevId != 0x71) DevId = AccGyro.getWhoAmI(); + LED = 1; + AccGyro.setPowerCtl_1(0x00, 0x00, 0x00, 0x00, INT_8MHz_OSC); // Disable sleep mode + wait_ms(1); + AccGyro.setGyroConfig(GYRO_ST_OFF, GFS_2000dps); // Accelerometer elf-test trigger off. + wait_ms(1); + AccGyro.setAccelConfig(ACC_ST_OFF, AFS_16g); // Gyroscope self-test trigger off. + wait_ms(1); + InterruptIn force(PA_7); + shutDown = false; + minn.start(); + see.start(); + force.rise(&ping); +// while(1){ +// updateIMU(); +// 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); +// } +// while(1){ +// updateIMU(); +// onOff('A', 'B'); +// pc.printf("step : %d\tmem : %d\tcharge : %d\tstep : %d\tmem : %d\tcharge : %d\n", stepOn, memOn, chargeOn, stepOff, memOff, chargeOff); +// } + while(1){ + if(!shutDown){ + if(bt.readable()){ + elec = (uint8_t)bt.getc()-48; + pc.printf("\t\t\t%d\n", elec); + } + updateIMU(); + /*if(minn.read_ms() >= 60000 && elec != 0){ + bt.printf("%c", elec + 83 S); + elec = 0; + }*/ + while(elec == 0){ + if(bt.readable()){ + elec = (uint8_t)bt.getc()-48; + pc.printf("\t%d\n", elec); + } + updateIMU(); + if(Ax > 1400){ + if(step != 1) step = 1; + dataFinger = finger(); + if(dataFinger == 8) sendData = 1; + else if(dataFinger == 12) sendData = 3; + else if(dataFinger == 14) sendData = 5; + else if(dataFinger == 15) sendData = 4; + else if(dataFinger == 0) sendData = 2; + else sendData = 6; + if(minn.read_ms() >= 10){ + bt.printf("%c", sendData+48); + pc.printf("%d\n", sendData); + minn.reset(); + } + } + else if(range(Ay, -1000, 1000) && Gy > 0 && sendData < 6){ + if(step == 1) step = 2; + } + else if(Az > 1800 && step == 2){ + while(minn.read_ms() < 10); + bt.printf("%c", 'a'+sendData); + pc.printf("%c\n", 'a'+sendData); + minn.reset(); + elec = sendData; + step = 0; + sendData = 6; + set0(); + while(1){ + if(bt.readable()) elec = (uint8_t)bt.getc()-48; + if(elec != 0) break; + } + break; + pc.printf("yeah"); + } + else{ + if(step != 0){ + step = 0; + } + } + if(step == 0 && minn.read_ms() >= 10){ + sendData = 0; + bt.printf("%c", 'a'); + pc.printf("%d\n", sendData); + minn.reset(); + } + } + updateIMU(); + if(elec == 1){ //fan + dataFinger = finger(); + onOff('A', 'B'); + if(Az < -800){ + if(dataFinger == 31 && step1 != 1) step1 = 1; + else if(dataFinger == 16 && step1 == 1){ + bt.printf("%c", 'D'); + pc.printf("%c\n", 'D'); + step1 = 2; + } + } + else if(Az > 1500){ + if(dataFinger == 31 && step1 != 3) step1 = 3; + else if(dataFinger == 16 && step1 == 3){ + bt.printf("%c", 'C'); + pc.printf("%c\n", 'C'); + step1 = 4; + } + } + else step1 = 0; + } + else if(elec == 2){ //air + dataFinger = finger(); + onOff('E', 'F'); + if(dataFinger == 28){ + if(Gz > 50 && step2 !=1) step2 = 1; + else if(Gz < 10 && step2 == 1){ + bt.printf("%c", 'H'); + pc.printf("%c\n", 'H'); + step2 = 2; + } + } + else if(dataFinger == 24){ + if(Gz > 20 && step2 !=4) step2 = 4; + else if(Gz < 20 && step2 == 4){ + bt.printf("%c", 'G'); + pc.printf("%c\n", 'G'); + step2 = 5; + } + } + else step2 = 0; + } + else if(elec == 3){ //kettle + dataFinger = finger(); + onOff('I', 'J'); + } + else if(elec == 4){ //tv + dataFinger = finger(); + onOff('K', 'L'); + if(dataFinger == 16){ + if(Ay < -800 && step4 != 3) step4 = 3; + else if(Ay > 1000 && step4 == 3){ + bt.printf("%c", 'M'); + pc.printf("%c\n", 'M'); + step4 = 4; + } + if(Ay > 1400 && step4 != 5) step4 = 5; + else if(Ay < -300 && step4 == 5){ + bt.printf("%c", 'N'); + pc.printf("%c\n", 'N'); + step4 = 6; + } + } + } + else if(elec == 5){ //radio + dataFinger = finger(); + onOff('O', 'P'); + if(dataFinger == 16){ + if(Gx > 20 && step5 != 1){ + bt.printf("%c", 'U'); + pc.printf("%c\n", 'U'); + step5 = 1; + } + + if(Ay < -800 && step5 != 3) step5 = 3; + else if(Ay > 1000 && step5 == 3){ + bt.printf("%c", 'Q'); + pc.printf("%c\n", 'Q'); + step5 = 4; + } + if(Ay > 1400 && step5 != 5) step5 = 5; + else if(Ay < -300 && step5 == 5){ + bt.printf("%c", 'R'); + pc.printf("%c\n", 'R'); + step5 = 6; + } + } + if(dataFinger == 24){ + if(Gx < -20&& step5 != 2){ + bt.printf("%c", 'T'); + pc.printf("%c\n", 'T'); + step5 = 2; + } + } + } + } + } +} \ No newline at end of file