Laboratory
Dependencies: mbed BufferedSerial MMA8491 TSI MAG3110
main.cpp
- Committer:
- abm_mechatronika
- Date:
- 2019-02-13
- Revision:
- 1:3bdde2447358
- Parent:
- 0:79cb20d1d3d4
File content as of revision 1:3bdde2447358:
/** * Kroki: * 1. On startup Red LED flashes indicating calibration mode entered * 2. Slide finger along capacitive sensor and release * 3. Green LED flashes indicating calibration mode. * 4. Rotate board once in horizontal plane * 5. Tap and release capacitive sensor. Board now calibrated with min/max values * 6. LEDs now off. Rotate board. When Blue LED lights the bottom of the board is * pointing to approximately North (+/- 22.5') */ #include "mbed.h" #include "TSISensor.h" #include "math.h" #include "MAG3110.h" #include "MMA8491.h" #include "BufferedSerial.h" #define ON 0 #define OFF 1 //Diody LED pokazujące status operacji DigitalOut redLed(LED_RED); DigitalOut greenLed(LED_GREEN); DigitalOut blueLed(LED_BLUE); // Czujnik dotyku TSISensor tsi; MAG3110 mag(PTE0, PTE1); //magnetometr //Komunikacja BufferedSerial pc(USBTX,USBRX); BufferedSerial device_zadajnik(PTE22,PTE23); //TX, RX char buffer[24]; void calXY(); double arc_tan_2(double a, double b); int main() { pc.baud(115200); device_zadajnik.baud(115200); MMA8491 acc(I2C_SDA, I2C_SCL, PTA13); redLed = OFF; greenLed = OFF; blueLed = OFF; calXY(); redLed = OFF; greenLed = OFF; blueLed = OFF; while (1) { wait(0.04); float data[3]; //int xVal = mag.readVal(MAG_OUT_X_MSB); //int yVal = mag.readVal(MAG_OUT_Y_MSB); float kierunek = mag.getHeading(); //kąt obrotu płytki XTRINSIC-SENSORS Board wzgledem osi północ-poludnie //odczyt przyspieszen acc.acquire_MMA8491_data_g(data); redLed = 1.0 - abs((long)data[0]); //sygnalizacja kierunku greenLed = 1.0 - abs((long)data[1]); blueLed = 1.0 - abs((long)data[2]); //Wskazywanie północy poprzez zaświecenie niebieskiej diody /* if (abs(kierunek) <= 22.5) { blueLed = ON; } else blueLed = OFF;*/ // (kierunek < 0) kierunek += 360.0; //? float x=(float)data[0]; float y=(float)data[1]; float z=(float)data[2]; //pc.printf("%.2f %.2f %f\n\r",arc_tan_2(x,z),arc_tan_2(y,z), kierunek); //device_zadajnik.printf("%.2f %.2f %.2f",data[0], data[1], data[2]); //device_zadajnik.printf("%.2f %.2f %f\n\r",data[0], data[1], kierunek); sprintf((char*)buffer,"%.2f %.2f %f\n\r",arc_tan_2(x,z),arc_tan_2(y,z), kierunek); device_zadajnik.printf(buffer); //ewentualnie "pc.printf(buffer);" //wait(0.2); }//koniec while() }//koniec main() //>>>>>>>>>>>>>>>>>>>_FUNKCJA_ATAN2_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> double arc_tan_2(double a, double b) { double stopnie; double val = 180.0/PI; stopnie = atan2(a,b)*val; return stopnie; } //>>>>>>>>>>>>>>>>>>>_FUNKCJA_KALIBRACJI_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> void calXY() //kalibracja mgnetometru: znajdowanie max i min osi X, Y { int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY; redLed = ON; //Oczekiwanie na dotknięcie czujnika dotyku while( tsi.readDistance() == 0 ) { redLed = ON; wait(0.2); redLed = OFF; wait(0.2); } //Oczekiwanie na puszczenie czujnika dotyku while( tsi.readDistance() != 0 ) { redLed = OFF; wait(0.2); redLed = ON; wait(0.2); } redLed = OFF; wait(0.5); tempXmax = tempXmin = mag.readVal(MAG_OUT_X_MSB); tempYmax = tempYmin = mag.readVal(MAG_OUT_Y_MSB); while(tsi.readDistance() == 0) { greenLed = ON; wait(0.1); greenLed = OFF; wait(0.1); newX = mag.readVal(MAG_OUT_X_MSB); newY = mag.readVal(MAG_OUT_Y_MSB); if (newX > tempXmax) tempXmax = newX; if (newX < tempXmin) tempXmin = newX; if (newY > tempYmax) tempYmax = newY; if (newY < tempYmin) tempYmin = newY; } mag.setCalibration( tempXmin, tempXmax, tempYmin, tempYmax ); // Oczekiwanie na zwolnienie nacisku while( tsi.readDistance() != 0 ) { greenLed = OFF; wait(0.2); greenLed = ON; wait(0.2); } greenLed = OFF; wait(1.0); }