Diff: main.cpp

Revision:

0:79cb20d1d3d4

Child:

1:3bdde2447358

--- /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