main.cpp

00001 
00002 /**
00003  * Kroki:
00004  * 1. On startup Red LED flashes indicating calibration mode entered
00005  * 2. Slide finger along capacitive sensor and release
00006  * 3. Green LED flashes indicating calibration mode.
00007  * 4. Rotate board once in horizontal plane
00008  * 5. Tap and release capacitive sensor. Board now calibrated with min/max values
00009  * 6. LEDs now off. Rotate board. When Blue LED lights the bottom of the board is
00010  *    pointing to approximately North (+/- 22.5')
00011  */
00012 
00013 #include "mbed.h"
00014 #include "TSISensor.h"
00015 #include "math.h"
00016 #include "MAG3110.h"
00017 #include "MMA8491.h"
00018 #include "BufferedSerial.h"
00019 
00020 #define ON  0
00021 #define OFF 1
00022 
00023 //Diody LED pokazujące status operacji
00024 DigitalOut redLed(LED_RED);
00025 DigitalOut greenLed(LED_GREEN);
00026 DigitalOut blueLed(LED_BLUE);
00027 
00028 // Czujnik dotyku
00029 TSISensor tsi;
00030 
00031 MAG3110 mag(PTE0, PTE1);    //magnetometr
00032 
00033 //Komunikacja
00034 BufferedSerial pc(USBTX,USBRX);
00035 BufferedSerial device_zadajnik(PTE22,PTE23); //TX, RX
00036 char buffer[24];
00037 
00038 void calXY();
00039 double arc_tan_2(double a, double b);
00040 
00041 int main()
00042 {
00043     pc.baud(115200);
00044     device_zadajnik.baud(115200);
00045     
00046     MMA8491 acc(I2C_SDA, I2C_SCL, PTA13);
00047 
00048     redLed = OFF;
00049     greenLed = OFF;
00050     blueLed = OFF;
00051     calXY();
00052     redLed = OFF;
00053     greenLed = OFF;
00054     blueLed = OFF;
00055 
00056     while (1) {
00057         wait(0.04);
00058         float data[3];
00059         //int xVal = mag.readVal(MAG_OUT_X_MSB);
00060         //int yVal = mag.readVal(MAG_OUT_Y_MSB);
00061         float kierunek = mag.getHeading();  //kąt obrotu płytki XTRINSIC-SENSORS Board wzgledem osi północ-poludnie
00062         
00063         //odczyt przyspieszen
00064         acc.acquire_MMA8491_data_g(data);
00065         redLed = 1.0 - abs((long)data[0]);  //sygnalizacja kierunku
00066         greenLed = 1.0 - abs((long)data[1]);
00067         blueLed = 1.0 - abs((long)data[2]);
00068         
00069         //Wskazywanie północy poprzez zaświecenie niebieskiej diody
00070 /*        if (abs(kierunek) <= 22.5) {
00071             blueLed = ON;
00072         } else blueLed = OFF;*/
00073         
00074         // (kierunek < 0) kierunek += 360.0;    //?
00075         
00076         float x=(float)data[0];
00077         float y=(float)data[1];
00078         float z=(float)data[2];
00079 
00080         //pc.printf("%.2f %.2f %f\n\r",arc_tan_2(x,z),arc_tan_2(y,z), kierunek);
00081         //device_zadajnik.printf("%.2f %.2f %.2f",data[0], data[1], data[2]);
00082         //device_zadajnik.printf("%.2f %.2f %f\n\r",data[0], data[1], kierunek);
00083         sprintf((char*)buffer,"%.2f %.2f %f\n\r",arc_tan_2(x,z),arc_tan_2(y,z), kierunek);
00084         device_zadajnik.printf(buffer); //ewentualnie "pc.printf(buffer);"
00085         //wait(0.2);
00086     }//koniec while()
00087 }//koniec main()
00088 
00089 //>>>>>>>>>>>>>>>>>>>_FUNKCJA_ATAN2_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
00090 double arc_tan_2(double a, double b)
00091     {
00092         double stopnie;
00093         double val = 180.0/PI;
00094         stopnie = atan2(a,b)*val;
00095         return stopnie;
00096     }
00097 //>>>>>>>>>>>>>>>>>>>_FUNKCJA_KALIBRACJI_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
00098 void calXY() //kalibracja mgnetometru: znajdowanie max i min osi X, Y
00099 {
00100     int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY;
00101     redLed = ON;
00102     //Oczekiwanie na dotknięcie czujnika dotyku
00103     while( tsi.readDistance() == 0 ) {
00104         redLed = ON;
00105         wait(0.2);
00106         redLed = OFF;
00107         wait(0.2);
00108     }
00109     //Oczekiwanie na puszczenie czujnika dotyku
00110     while( tsi.readDistance() != 0 ) {
00111         redLed = OFF;
00112         wait(0.2);
00113         redLed = ON;
00114         wait(0.2);
00115     }
00116     redLed = OFF;
00117     wait(0.5);
00118     tempXmax = tempXmin = mag.readVal(MAG_OUT_X_MSB);
00119     tempYmax = tempYmin = mag.readVal(MAG_OUT_Y_MSB);
00120 
00121     while(tsi.readDistance() == 0) {
00122         greenLed = ON;
00123         wait(0.1);
00124         greenLed = OFF;
00125         wait(0.1);
00126         newX = mag.readVal(MAG_OUT_X_MSB);
00127         newY = mag.readVal(MAG_OUT_Y_MSB);
00128         if (newX > tempXmax) tempXmax = newX;
00129         if (newX < tempXmin) tempXmin = newX;
00130         if (newY > tempYmax) tempYmax = newY;
00131         if (newY < tempYmin) tempYmin = newY;
00132     }
00133     mag.setCalibration( tempXmin, tempXmax, tempYmin, tempYmax );
00134 
00135     // Oczekiwanie na zwolnienie nacisku
00136     while( tsi.readDistance() != 0 ) {
00137         greenLed = OFF;
00138         wait(0.2);
00139         greenLed = ON;
00140         wait(0.2);
00141     }
00142     greenLed = OFF;
00143     wait(1.0);
00144 }