Mechatronics / Mbed 2 deprecated 1-FRDM-Gimbal-LAB

Laboratory

Dependencies:   mbed BufferedSerial MMA8491 TSI MAG3110

main.cpp@1:3bdde2447358, 2019-02-13 (annotated)

Committer:
abm_mechatronika
Date:
Wed Feb 13 13:50:09 2019 +0000
Revision:
1:3bdde2447358
Parent:
0:79cb20d1d3d4 
Po konsultacji z promotorem

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Lukasz_K 0:79cb20d1d3d4 1
Lukasz_K 0:79cb20d1d3d4 2 /**
Lukasz_K 0:79cb20d1d3d4 3 * Kroki:
Lukasz_K 0:79cb20d1d3d4 4 * 1. On startup Red LED flashes indicating calibration mode entered
Lukasz_K 0:79cb20d1d3d4 5 * 2. Slide finger along capacitive sensor and release
Lukasz_K 0:79cb20d1d3d4 6 * 3. Green LED flashes indicating calibration mode.
Lukasz_K 0:79cb20d1d3d4 7 * 4. Rotate board once in horizontal plane
Lukasz_K 0:79cb20d1d3d4 8 * 5. Tap and release capacitive sensor. Board now calibrated with min/max values
Lukasz_K 0:79cb20d1d3d4 9 * 6. LEDs now off. Rotate board. When Blue LED lights the bottom of the board is
Lukasz_K 0:79cb20d1d3d4 10 * pointing to approximately North (+/- 22.5')
Lukasz_K 0:79cb20d1d3d4 11 */
Lukasz_K 0:79cb20d1d3d4 12
Lukasz_K 0:79cb20d1d3d4 13 #include "mbed.h"
Lukasz_K 0:79cb20d1d3d4 14 #include "TSISensor.h"
Lukasz_K 0:79cb20d1d3d4 15 #include "math.h"
Lukasz_K 0:79cb20d1d3d4 16 #include "MAG3110.h"
Lukasz_K 0:79cb20d1d3d4 17 #include "MMA8491.h"
Lukasz_K 0:79cb20d1d3d4 18 #include "BufferedSerial.h"
Lukasz_K 0:79cb20d1d3d4 19
Lukasz_K 0:79cb20d1d3d4 20 #define ON 0
Lukasz_K 0:79cb20d1d3d4 21 #define OFF 1
Lukasz_K 0:79cb20d1d3d4 22
Lukasz_K 0:79cb20d1d3d4 23 //Diody LED pokazujące status operacji
Lukasz_K 0:79cb20d1d3d4 24 DigitalOut redLed(LED_RED);
Lukasz_K 0:79cb20d1d3d4 25 DigitalOut greenLed(LED_GREEN);
Lukasz_K 0:79cb20d1d3d4 26 DigitalOut blueLed(LED_BLUE);
Lukasz_K 0:79cb20d1d3d4 27
Lukasz_K 0:79cb20d1d3d4 28 // Czujnik dotyku
Lukasz_K 0:79cb20d1d3d4 29 TSISensor tsi;
Lukasz_K 0:79cb20d1d3d4 30
Lukasz_K 0:79cb20d1d3d4 31 MAG3110 mag(PTE0, PTE1); //magnetometr
Lukasz_K 0:79cb20d1d3d4 32
Lukasz_K 0:79cb20d1d3d4 33 //Komunikacja
Lukasz_K 0:79cb20d1d3d4 34 BufferedSerial pc(USBTX,USBRX);
abm_mechatronika 1:3bdde2447358 35 BufferedSerial device_zadajnik(PTE22,PTE23); //TX, RX
Lukasz_K 0:79cb20d1d3d4 36 char buffer[24];
Lukasz_K 0:79cb20d1d3d4 37
Lukasz_K 0:79cb20d1d3d4 38 void calXY();
Lukasz_K 0:79cb20d1d3d4 39 double arc_tan_2(double a, double b);
Lukasz_K 0:79cb20d1d3d4 40
Lukasz_K 0:79cb20d1d3d4 41 int main()
Lukasz_K 0:79cb20d1d3d4 42 {
Lukasz_K 0:79cb20d1d3d4 43 pc.baud(115200);
abm_mechatronika 1:3bdde2447358 44 device_zadajnik.baud(115200);
Lukasz_K 0:79cb20d1d3d4 45
Lukasz_K 0:79cb20d1d3d4 46 MMA8491 acc(I2C_SDA, I2C_SCL, PTA13);
Lukasz_K 0:79cb20d1d3d4 47
Lukasz_K 0:79cb20d1d3d4 48 redLed = OFF;
Lukasz_K 0:79cb20d1d3d4 49 greenLed = OFF;
Lukasz_K 0:79cb20d1d3d4 50 blueLed = OFF;
Lukasz_K 0:79cb20d1d3d4 51 calXY();
Lukasz_K 0:79cb20d1d3d4 52 redLed = OFF;
Lukasz_K 0:79cb20d1d3d4 53 greenLed = OFF;
Lukasz_K 0:79cb20d1d3d4 54 blueLed = OFF;
Lukasz_K 0:79cb20d1d3d4 55
Lukasz_K 0:79cb20d1d3d4 56 while (1) {
Lukasz_K 0:79cb20d1d3d4 57 wait(0.04);
Lukasz_K 0:79cb20d1d3d4 58 float data[3];
abm_mechatronika 1:3bdde2447358 59 //int xVal = mag.readVal(MAG_OUT_X_MSB);
abm_mechatronika 1:3bdde2447358 60 //int yVal = mag.readVal(MAG_OUT_Y_MSB);
abm_mechatronika 1:3bdde2447358 61 float kierunek = mag.getHeading(); //kąt obrotu płytki XTRINSIC-SENSORS Board wzgledem osi północ-poludnie
Lukasz_K 0:79cb20d1d3d4 62
abm_mechatronika 1:3bdde2447358 63 //odczyt przyspieszen
Lukasz_K 0:79cb20d1d3d4 64 acc.acquire_MMA8491_data_g(data);
abm_mechatronika 1:3bdde2447358 65 redLed = 1.0 - abs((long)data[0]); //sygnalizacja kierunku
Lukasz_K 0:79cb20d1d3d4 66 greenLed = 1.0 - abs((long)data[1]);
Lukasz_K 0:79cb20d1d3d4 67 blueLed = 1.0 - abs((long)data[2]);
Lukasz_K 0:79cb20d1d3d4 68
Lukasz_K 0:79cb20d1d3d4 69 //Wskazywanie północy poprzez zaświecenie niebieskiej diody
Lukasz_K 0:79cb20d1d3d4 70 /* if (abs(kierunek) <= 22.5) {
Lukasz_K 0:79cb20d1d3d4 71 blueLed = ON;
Lukasz_K 0:79cb20d1d3d4 72 } else blueLed = OFF;*/
Lukasz_K 0:79cb20d1d3d4 73
abm_mechatronika 1:3bdde2447358 74 // (kierunek < 0) kierunek += 360.0; //?
Lukasz_K 0:79cb20d1d3d4 75
Lukasz_K 0:79cb20d1d3d4 76 float x=(float)data[0];
Lukasz_K 0:79cb20d1d3d4 77 float y=(float)data[1];
Lukasz_K 0:79cb20d1d3d4 78 float z=(float)data[2];
Lukasz_K 0:79cb20d1d3d4 79
Lukasz_K 0:79cb20d1d3d4 80 //pc.printf("%.2f %.2f %f\n\r",arc_tan_2(x,z),arc_tan_2(y,z), kierunek);
abm_mechatronika 1:3bdde2447358 81 //device_zadajnik.printf("%.2f %.2f %.2f",data[0], data[1], data[2]);
abm_mechatronika 1:3bdde2447358 82 //device_zadajnik.printf("%.2f %.2f %f\n\r",data[0], data[1], kierunek);
Lukasz_K 0:79cb20d1d3d4 83 sprintf((char*)buffer,"%.2f %.2f %f\n\r",arc_tan_2(x,z),arc_tan_2(y,z), kierunek);
abm_mechatronika 1:3bdde2447358 84 device_zadajnik.printf(buffer); //ewentualnie "pc.printf(buffer);"
Lukasz_K 0:79cb20d1d3d4 85 //wait(0.2);
Lukasz_K 0:79cb20d1d3d4 86 }//koniec while()
Lukasz_K 0:79cb20d1d3d4 87 }//koniec main()
Lukasz_K 0:79cb20d1d3d4 88
Lukasz_K 0:79cb20d1d3d4 89 //>>>>>>>>>>>>>>>>>>>_FUNKCJA_ATAN2_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Lukasz_K 0:79cb20d1d3d4 90 double arc_tan_2(double a, double b)
Lukasz_K 0:79cb20d1d3d4 91 {
Lukasz_K 0:79cb20d1d3d4 92 double stopnie;
Lukasz_K 0:79cb20d1d3d4 93 double val = 180.0/PI;
Lukasz_K 0:79cb20d1d3d4 94 stopnie = atan2(a,b)*val;
Lukasz_K 0:79cb20d1d3d4 95 return stopnie;
Lukasz_K 0:79cb20d1d3d4 96 }
Lukasz_K 0:79cb20d1d3d4 97 //>>>>>>>>>>>>>>>>>>>_FUNKCJA_KALIBRACJI_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Lukasz_K 0:79cb20d1d3d4 98 void calXY() //kalibracja mgnetometru: znajdowanie max i min osi X, Y
Lukasz_K 0:79cb20d1d3d4 99 {
Lukasz_K 0:79cb20d1d3d4 100 int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY;
Lukasz_K 0:79cb20d1d3d4 101 redLed = ON;
Lukasz_K 0:79cb20d1d3d4 102 //Oczekiwanie na dotknięcie czujnika dotyku
Lukasz_K 0:79cb20d1d3d4 103 while( tsi.readDistance() == 0 ) {
Lukasz_K 0:79cb20d1d3d4 104 redLed = ON;
Lukasz_K 0:79cb20d1d3d4 105 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 106 redLed = OFF;
Lukasz_K 0:79cb20d1d3d4 107 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 108 }
Lukasz_K 0:79cb20d1d3d4 109 //Oczekiwanie na puszczenie czujnika dotyku
Lukasz_K 0:79cb20d1d3d4 110 while( tsi.readDistance() != 0 ) {
Lukasz_K 0:79cb20d1d3d4 111 redLed = OFF;
Lukasz_K 0:79cb20d1d3d4 112 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 113 redLed = ON;
Lukasz_K 0:79cb20d1d3d4 114 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 115 }
Lukasz_K 0:79cb20d1d3d4 116 redLed = OFF;
Lukasz_K 0:79cb20d1d3d4 117 wait(0.5);
Lukasz_K 0:79cb20d1d3d4 118 tempXmax = tempXmin = mag.readVal(MAG_OUT_X_MSB);
Lukasz_K 0:79cb20d1d3d4 119 tempYmax = tempYmin = mag.readVal(MAG_OUT_Y_MSB);
Lukasz_K 0:79cb20d1d3d4 120
Lukasz_K 0:79cb20d1d3d4 121 while(tsi.readDistance() == 0) {
Lukasz_K 0:79cb20d1d3d4 122 greenLed = ON;
Lukasz_K 0:79cb20d1d3d4 123 wait(0.1);
Lukasz_K 0:79cb20d1d3d4 124 greenLed = OFF;
Lukasz_K 0:79cb20d1d3d4 125 wait(0.1);
Lukasz_K 0:79cb20d1d3d4 126 newX = mag.readVal(MAG_OUT_X_MSB);
Lukasz_K 0:79cb20d1d3d4 127 newY = mag.readVal(MAG_OUT_Y_MSB);
Lukasz_K 0:79cb20d1d3d4 128 if (newX > tempXmax) tempXmax = newX;
Lukasz_K 0:79cb20d1d3d4 129 if (newX < tempXmin) tempXmin = newX;
Lukasz_K 0:79cb20d1d3d4 130 if (newY > tempYmax) tempYmax = newY;
Lukasz_K 0:79cb20d1d3d4 131 if (newY < tempYmin) tempYmin = newY;
Lukasz_K 0:79cb20d1d3d4 132 }
Lukasz_K 0:79cb20d1d3d4 133 mag.setCalibration( tempXmin, tempXmax, tempYmin, tempYmax );
Lukasz_K 0:79cb20d1d3d4 134
Lukasz_K 0:79cb20d1d3d4 135 // Oczekiwanie na zwolnienie nacisku
Lukasz_K 0:79cb20d1d3d4 136 while( tsi.readDistance() != 0 ) {
Lukasz_K 0:79cb20d1d3d4 137 greenLed = OFF;
Lukasz_K 0:79cb20d1d3d4 138 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 139 greenLed = ON;
Lukasz_K 0:79cb20d1d3d4 140 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 141 }
Lukasz_K 0:79cb20d1d3d4 142 greenLed = OFF;
Lukasz_K 0:79cb20d1d3d4 143 wait(1.0);
Lukasz_K 0:79cb20d1d3d4 144 }