File content as of revision 0:79cb20d1d3d4:

/**
 * 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);
}