Laboratory
Dependencies: mbed BufferedSerial MMA8491 TSI MAG3110
main.cpp@1:3bdde2447358, 2019-02-13 (annotated)
- Committer:
- abm_mechatronika
- Date:
- Wed Feb 13 13:50:09 2019 +0000
- Revision:
- 1:3bdde2447358
- Parent:
- 0:79cb20d1d3d4
Po konsultacji z promotorem
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Lukasz_K | 0:79cb20d1d3d4 | 1 | |
Lukasz_K | 0:79cb20d1d3d4 | 2 | /** |
Lukasz_K | 0:79cb20d1d3d4 | 3 | * Kroki: |
Lukasz_K | 0:79cb20d1d3d4 | 4 | * 1. On startup Red LED flashes indicating calibration mode entered |
Lukasz_K | 0:79cb20d1d3d4 | 5 | * 2. Slide finger along capacitive sensor and release |
Lukasz_K | 0:79cb20d1d3d4 | 6 | * 3. Green LED flashes indicating calibration mode. |
Lukasz_K | 0:79cb20d1d3d4 | 7 | * 4. Rotate board once in horizontal plane |
Lukasz_K | 0:79cb20d1d3d4 | 8 | * 5. Tap and release capacitive sensor. Board now calibrated with min/max values |
Lukasz_K | 0:79cb20d1d3d4 | 9 | * 6. LEDs now off. Rotate board. When Blue LED lights the bottom of the board is |
Lukasz_K | 0:79cb20d1d3d4 | 10 | * pointing to approximately North (+/- 22.5') |
Lukasz_K | 0:79cb20d1d3d4 | 11 | */ |
Lukasz_K | 0:79cb20d1d3d4 | 12 | |
Lukasz_K | 0:79cb20d1d3d4 | 13 | #include "mbed.h" |
Lukasz_K | 0:79cb20d1d3d4 | 14 | #include "TSISensor.h" |
Lukasz_K | 0:79cb20d1d3d4 | 15 | #include "math.h" |
Lukasz_K | 0:79cb20d1d3d4 | 16 | #include "MAG3110.h" |
Lukasz_K | 0:79cb20d1d3d4 | 17 | #include "MMA8491.h" |
Lukasz_K | 0:79cb20d1d3d4 | 18 | #include "BufferedSerial.h" |
Lukasz_K | 0:79cb20d1d3d4 | 19 | |
Lukasz_K | 0:79cb20d1d3d4 | 20 | #define ON 0 |
Lukasz_K | 0:79cb20d1d3d4 | 21 | #define OFF 1 |
Lukasz_K | 0:79cb20d1d3d4 | 22 | |
Lukasz_K | 0:79cb20d1d3d4 | 23 | //Diody LED pokazujące status operacji |
Lukasz_K | 0:79cb20d1d3d4 | 24 | DigitalOut redLed(LED_RED); |
Lukasz_K | 0:79cb20d1d3d4 | 25 | DigitalOut greenLed(LED_GREEN); |
Lukasz_K | 0:79cb20d1d3d4 | 26 | DigitalOut blueLed(LED_BLUE); |
Lukasz_K | 0:79cb20d1d3d4 | 27 | |
Lukasz_K | 0:79cb20d1d3d4 | 28 | // Czujnik dotyku |
Lukasz_K | 0:79cb20d1d3d4 | 29 | TSISensor tsi; |
Lukasz_K | 0:79cb20d1d3d4 | 30 | |
Lukasz_K | 0:79cb20d1d3d4 | 31 | MAG3110 mag(PTE0, PTE1); //magnetometr |
Lukasz_K | 0:79cb20d1d3d4 | 32 | |
Lukasz_K | 0:79cb20d1d3d4 | 33 | //Komunikacja |
Lukasz_K | 0:79cb20d1d3d4 | 34 | BufferedSerial pc(USBTX,USBRX); |
abm_mechatronika | 1:3bdde2447358 | 35 | BufferedSerial device_zadajnik(PTE22,PTE23); //TX, RX |
Lukasz_K | 0:79cb20d1d3d4 | 36 | char buffer[24]; |
Lukasz_K | 0:79cb20d1d3d4 | 37 | |
Lukasz_K | 0:79cb20d1d3d4 | 38 | void calXY(); |
Lukasz_K | 0:79cb20d1d3d4 | 39 | double arc_tan_2(double a, double b); |
Lukasz_K | 0:79cb20d1d3d4 | 40 | |
Lukasz_K | 0:79cb20d1d3d4 | 41 | int main() |
Lukasz_K | 0:79cb20d1d3d4 | 42 | { |
Lukasz_K | 0:79cb20d1d3d4 | 43 | pc.baud(115200); |
abm_mechatronika | 1:3bdde2447358 | 44 | device_zadajnik.baud(115200); |
Lukasz_K | 0:79cb20d1d3d4 | 45 | |
Lukasz_K | 0:79cb20d1d3d4 | 46 | MMA8491 acc(I2C_SDA, I2C_SCL, PTA13); |
Lukasz_K | 0:79cb20d1d3d4 | 47 | |
Lukasz_K | 0:79cb20d1d3d4 | 48 | redLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 49 | greenLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 50 | blueLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 51 | calXY(); |
Lukasz_K | 0:79cb20d1d3d4 | 52 | redLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 53 | greenLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 54 | blueLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 55 | |
Lukasz_K | 0:79cb20d1d3d4 | 56 | while (1) { |
Lukasz_K | 0:79cb20d1d3d4 | 57 | wait(0.04); |
Lukasz_K | 0:79cb20d1d3d4 | 58 | float data[3]; |
abm_mechatronika | 1:3bdde2447358 | 59 | //int xVal = mag.readVal(MAG_OUT_X_MSB); |
abm_mechatronika | 1:3bdde2447358 | 60 | //int yVal = mag.readVal(MAG_OUT_Y_MSB); |
abm_mechatronika | 1:3bdde2447358 | 61 | float kierunek = mag.getHeading(); //kąt obrotu płytki XTRINSIC-SENSORS Board wzgledem osi północ-poludnie |
Lukasz_K | 0:79cb20d1d3d4 | 62 | |
abm_mechatronika | 1:3bdde2447358 | 63 | //odczyt przyspieszen |
Lukasz_K | 0:79cb20d1d3d4 | 64 | acc.acquire_MMA8491_data_g(data); |
abm_mechatronika | 1:3bdde2447358 | 65 | redLed = 1.0 - abs((long)data[0]); //sygnalizacja kierunku |
Lukasz_K | 0:79cb20d1d3d4 | 66 | greenLed = 1.0 - abs((long)data[1]); |
Lukasz_K | 0:79cb20d1d3d4 | 67 | blueLed = 1.0 - abs((long)data[2]); |
Lukasz_K | 0:79cb20d1d3d4 | 68 | |
Lukasz_K | 0:79cb20d1d3d4 | 69 | //Wskazywanie północy poprzez zaświecenie niebieskiej diody |
Lukasz_K | 0:79cb20d1d3d4 | 70 | /* if (abs(kierunek) <= 22.5) { |
Lukasz_K | 0:79cb20d1d3d4 | 71 | blueLed = ON; |
Lukasz_K | 0:79cb20d1d3d4 | 72 | } else blueLed = OFF;*/ |
Lukasz_K | 0:79cb20d1d3d4 | 73 | |
abm_mechatronika | 1:3bdde2447358 | 74 | // (kierunek < 0) kierunek += 360.0; //? |
Lukasz_K | 0:79cb20d1d3d4 | 75 | |
Lukasz_K | 0:79cb20d1d3d4 | 76 | float x=(float)data[0]; |
Lukasz_K | 0:79cb20d1d3d4 | 77 | float y=(float)data[1]; |
Lukasz_K | 0:79cb20d1d3d4 | 78 | float z=(float)data[2]; |
Lukasz_K | 0:79cb20d1d3d4 | 79 | |
Lukasz_K | 0:79cb20d1d3d4 | 80 | //pc.printf("%.2f %.2f %f\n\r",arc_tan_2(x,z),arc_tan_2(y,z), kierunek); |
abm_mechatronika | 1:3bdde2447358 | 81 | //device_zadajnik.printf("%.2f %.2f %.2f",data[0], data[1], data[2]); |
abm_mechatronika | 1:3bdde2447358 | 82 | //device_zadajnik.printf("%.2f %.2f %f\n\r",data[0], data[1], kierunek); |
Lukasz_K | 0:79cb20d1d3d4 | 83 | sprintf((char*)buffer,"%.2f %.2f %f\n\r",arc_tan_2(x,z),arc_tan_2(y,z), kierunek); |
abm_mechatronika | 1:3bdde2447358 | 84 | device_zadajnik.printf(buffer); //ewentualnie "pc.printf(buffer);" |
Lukasz_K | 0:79cb20d1d3d4 | 85 | //wait(0.2); |
Lukasz_K | 0:79cb20d1d3d4 | 86 | }//koniec while() |
Lukasz_K | 0:79cb20d1d3d4 | 87 | }//koniec main() |
Lukasz_K | 0:79cb20d1d3d4 | 88 | |
Lukasz_K | 0:79cb20d1d3d4 | 89 | //>>>>>>>>>>>>>>>>>>>_FUNKCJA_ATAN2_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> |
Lukasz_K | 0:79cb20d1d3d4 | 90 | double arc_tan_2(double a, double b) |
Lukasz_K | 0:79cb20d1d3d4 | 91 | { |
Lukasz_K | 0:79cb20d1d3d4 | 92 | double stopnie; |
Lukasz_K | 0:79cb20d1d3d4 | 93 | double val = 180.0/PI; |
Lukasz_K | 0:79cb20d1d3d4 | 94 | stopnie = atan2(a,b)*val; |
Lukasz_K | 0:79cb20d1d3d4 | 95 | return stopnie; |
Lukasz_K | 0:79cb20d1d3d4 | 96 | } |
Lukasz_K | 0:79cb20d1d3d4 | 97 | //>>>>>>>>>>>>>>>>>>>_FUNKCJA_KALIBRACJI_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> |
Lukasz_K | 0:79cb20d1d3d4 | 98 | void calXY() //kalibracja mgnetometru: znajdowanie max i min osi X, Y |
Lukasz_K | 0:79cb20d1d3d4 | 99 | { |
Lukasz_K | 0:79cb20d1d3d4 | 100 | int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY; |
Lukasz_K | 0:79cb20d1d3d4 | 101 | redLed = ON; |
Lukasz_K | 0:79cb20d1d3d4 | 102 | //Oczekiwanie na dotknięcie czujnika dotyku |
Lukasz_K | 0:79cb20d1d3d4 | 103 | while( tsi.readDistance() == 0 ) { |
Lukasz_K | 0:79cb20d1d3d4 | 104 | redLed = ON; |
Lukasz_K | 0:79cb20d1d3d4 | 105 | wait(0.2); |
Lukasz_K | 0:79cb20d1d3d4 | 106 | redLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 107 | wait(0.2); |
Lukasz_K | 0:79cb20d1d3d4 | 108 | } |
Lukasz_K | 0:79cb20d1d3d4 | 109 | //Oczekiwanie na puszczenie czujnika dotyku |
Lukasz_K | 0:79cb20d1d3d4 | 110 | while( tsi.readDistance() != 0 ) { |
Lukasz_K | 0:79cb20d1d3d4 | 111 | redLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 112 | wait(0.2); |
Lukasz_K | 0:79cb20d1d3d4 | 113 | redLed = ON; |
Lukasz_K | 0:79cb20d1d3d4 | 114 | wait(0.2); |
Lukasz_K | 0:79cb20d1d3d4 | 115 | } |
Lukasz_K | 0:79cb20d1d3d4 | 116 | redLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 117 | wait(0.5); |
Lukasz_K | 0:79cb20d1d3d4 | 118 | tempXmax = tempXmin = mag.readVal(MAG_OUT_X_MSB); |
Lukasz_K | 0:79cb20d1d3d4 | 119 | tempYmax = tempYmin = mag.readVal(MAG_OUT_Y_MSB); |
Lukasz_K | 0:79cb20d1d3d4 | 120 | |
Lukasz_K | 0:79cb20d1d3d4 | 121 | while(tsi.readDistance() == 0) { |
Lukasz_K | 0:79cb20d1d3d4 | 122 | greenLed = ON; |
Lukasz_K | 0:79cb20d1d3d4 | 123 | wait(0.1); |
Lukasz_K | 0:79cb20d1d3d4 | 124 | greenLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 125 | wait(0.1); |
Lukasz_K | 0:79cb20d1d3d4 | 126 | newX = mag.readVal(MAG_OUT_X_MSB); |
Lukasz_K | 0:79cb20d1d3d4 | 127 | newY = mag.readVal(MAG_OUT_Y_MSB); |
Lukasz_K | 0:79cb20d1d3d4 | 128 | if (newX > tempXmax) tempXmax = newX; |
Lukasz_K | 0:79cb20d1d3d4 | 129 | if (newX < tempXmin) tempXmin = newX; |
Lukasz_K | 0:79cb20d1d3d4 | 130 | if (newY > tempYmax) tempYmax = newY; |
Lukasz_K | 0:79cb20d1d3d4 | 131 | if (newY < tempYmin) tempYmin = newY; |
Lukasz_K | 0:79cb20d1d3d4 | 132 | } |
Lukasz_K | 0:79cb20d1d3d4 | 133 | mag.setCalibration( tempXmin, tempXmax, tempYmin, tempYmax ); |
Lukasz_K | 0:79cb20d1d3d4 | 134 | |
Lukasz_K | 0:79cb20d1d3d4 | 135 | // Oczekiwanie na zwolnienie nacisku |
Lukasz_K | 0:79cb20d1d3d4 | 136 | while( tsi.readDistance() != 0 ) { |
Lukasz_K | 0:79cb20d1d3d4 | 137 | greenLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 138 | wait(0.2); |
Lukasz_K | 0:79cb20d1d3d4 | 139 | greenLed = ON; |
Lukasz_K | 0:79cb20d1d3d4 | 140 | wait(0.2); |
Lukasz_K | 0:79cb20d1d3d4 | 141 | } |
Lukasz_K | 0:79cb20d1d3d4 | 142 | greenLed = OFF; |
Lukasz_K | 0:79cb20d1d3d4 | 143 | wait(1.0); |
Lukasz_K | 0:79cb20d1d3d4 | 144 | } |