Mechatronics
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