Laboratory
Dependencies: mbed BufferedSerial MMA8491 TSI MAG3110
Revision 0:79cb20d1d3d4, committed 2019-02-08
- Comitter:
- Lukasz_K
- Date:
- Fri Feb 08 12:09:11 2019 +0000
- Child:
- 1:3bdde2447358
- Commit message:
- XTRINSIC-SENSORS-BOARD - GIMBAL
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BufferedSerial.lib Fri Feb 08 12:09:11 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/sam_grove/code/BufferedSerial/#a0d37088b405
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MAG3110.lib Fri Feb 08 12:09:11 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/SomeRandomBloke/code/MAG3110/#cf40601402b7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MMA8491.lib Fri Feb 08 12:09:11 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/teams/NXP/code/MMA8491/#ce9ae6fa9407
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TSI.lib Fri Feb 08 12:09:11 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/emilmont/code/TSI/#507b1f67804b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Feb 08 12:09:11 2019 +0000 @@ -0,0 +1,143 @@ + +/** + * 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(PTE22,PTE23); //TX, RX +char buffer[24]; + +void calXY(); +double arc_tan_2(double a, double b); + +int main() +{ + pc.baud(115200); + device.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(); + + acc.acquire_MMA8491_data_g(data); + redLed = 1.0 - abs((long)data[0]); + 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;*/ + + if (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.printf("%.2f %.2f %.2f",data[0], data[1], data[2]); + //device.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.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); +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Feb 08 12:09:11 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc \ No newline at end of file