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

Laboratory

Dependencies:   mbed BufferedSerial MMA8491 TSI MAG3110

main.cpp@0:79cb20d1d3d4, 2019-02-08 (annotated)

Committer:
Lukasz_K
Date:
Fri Feb 08 12:09:11 2019 +0000
Revision:
0:79cb20d1d3d4
Child:
1:3bdde2447358
XTRINSIC-SENSORS-BOARD - GIMBAL

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);
Lukasz_K 0:79cb20d1d3d4 35 BufferedSerial device(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);
Lukasz_K 0:79cb20d1d3d4 44 device.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];
Lukasz_K 0:79cb20d1d3d4 59 int xVal = mag.readVal(MAG_OUT_X_MSB);
Lukasz_K 0:79cb20d1d3d4 60 int yVal = mag.readVal(MAG_OUT_Y_MSB);
Lukasz_K 0:79cb20d1d3d4 61 float kierunek = mag.getHeading();
Lukasz_K 0:79cb20d1d3d4 62
Lukasz_K 0:79cb20d1d3d4 63 acc.acquire_MMA8491_data_g(data);
Lukasz_K 0:79cb20d1d3d4 64 redLed = 1.0 - abs((long)data[0]);
Lukasz_K 0:79cb20d1d3d4 65 greenLed = 1.0 - abs((long)data[1]);
Lukasz_K 0:79cb20d1d3d4 66 blueLed = 1.0 - abs((long)data[2]);
Lukasz_K 0:79cb20d1d3d4 67
Lukasz_K 0:79cb20d1d3d4 68 //Wskazywanie północy poprzez zaświecenie niebieskiej diody
Lukasz_K 0:79cb20d1d3d4 69 /* if (abs(kierunek) <= 22.5) {
Lukasz_K 0:79cb20d1d3d4 70 blueLed = ON;
Lukasz_K 0:79cb20d1d3d4 71 } else blueLed = OFF;*/
Lukasz_K 0:79cb20d1d3d4 72
Lukasz_K 0:79cb20d1d3d4 73 if (kierunek < 0) kierunek += 360.0;
Lukasz_K 0:79cb20d1d3d4 74
Lukasz_K 0:79cb20d1d3d4 75 float x=(float)data[0];
Lukasz_K 0:79cb20d1d3d4 76 float y=(float)data[1];
Lukasz_K 0:79cb20d1d3d4 77 float z=(float)data[2];
Lukasz_K 0:79cb20d1d3d4 78
Lukasz_K 0:79cb20d1d3d4 79 //pc.printf("%.2f %.2f %f\n\r",arc_tan_2(x,z),arc_tan_2(y,z), kierunek);
Lukasz_K 0:79cb20d1d3d4 80 //device.printf("%.2f %.2f %.2f",data[0], data[1], data[2]);
Lukasz_K 0:79cb20d1d3d4 81 //device.printf("%.2f %.2f %f\n\r",data[0], data[1], kierunek);
Lukasz_K 0:79cb20d1d3d4 82 sprintf((char*)buffer,"%.2f %.2f %f\n\r",arc_tan_2(x,z),arc_tan_2(y,z), kierunek);
Lukasz_K 0:79cb20d1d3d4 83 device.printf(buffer);
Lukasz_K 0:79cb20d1d3d4 84 //wait(0.2);
Lukasz_K 0:79cb20d1d3d4 85 }//koniec while()
Lukasz_K 0:79cb20d1d3d4 86 }//koniec main()
Lukasz_K 0:79cb20d1d3d4 87
Lukasz_K 0:79cb20d1d3d4 88 //>>>>>>>>>>>>>>>>>>>_FUNKCJA_ATAN2_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Lukasz_K 0:79cb20d1d3d4 89 double arc_tan_2(double a, double b)
Lukasz_K 0:79cb20d1d3d4 90 {
Lukasz_K 0:79cb20d1d3d4 91 double stopnie;
Lukasz_K 0:79cb20d1d3d4 92 double val = 180.0/PI;
Lukasz_K 0:79cb20d1d3d4 93 stopnie = atan2(a,b)*val;
Lukasz_K 0:79cb20d1d3d4 94 return stopnie;
Lukasz_K 0:79cb20d1d3d4 95 }
Lukasz_K 0:79cb20d1d3d4 96 //>>>>>>>>>>>>>>>>>>>_FUNKCJA_KALIBRACJI_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Lukasz_K 0:79cb20d1d3d4 97 void calXY() //kalibracja mgnetometru: znajdowanie max i min osi X, Y
Lukasz_K 0:79cb20d1d3d4 98 {
Lukasz_K 0:79cb20d1d3d4 99 int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY;
Lukasz_K 0:79cb20d1d3d4 100 redLed = ON;
Lukasz_K 0:79cb20d1d3d4 101 //Oczekiwanie na dotknięcie czujnika dotyku
Lukasz_K 0:79cb20d1d3d4 102 while( tsi.readDistance() == 0 ) {
Lukasz_K 0:79cb20d1d3d4 103 redLed = ON;
Lukasz_K 0:79cb20d1d3d4 104 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 105 redLed = OFF;
Lukasz_K 0:79cb20d1d3d4 106 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 107 }
Lukasz_K 0:79cb20d1d3d4 108 //Oczekiwanie na puszczenie czujnika dotyku
Lukasz_K 0:79cb20d1d3d4 109 while( tsi.readDistance() != 0 ) {
Lukasz_K 0:79cb20d1d3d4 110 redLed = OFF;
Lukasz_K 0:79cb20d1d3d4 111 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 112 redLed = ON;
Lukasz_K 0:79cb20d1d3d4 113 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 114 }
Lukasz_K 0:79cb20d1d3d4 115 redLed = OFF;
Lukasz_K 0:79cb20d1d3d4 116 wait(0.5);
Lukasz_K 0:79cb20d1d3d4 117 tempXmax = tempXmin = mag.readVal(MAG_OUT_X_MSB);
Lukasz_K 0:79cb20d1d3d4 118 tempYmax = tempYmin = mag.readVal(MAG_OUT_Y_MSB);
Lukasz_K 0:79cb20d1d3d4 119
Lukasz_K 0:79cb20d1d3d4 120 while(tsi.readDistance() == 0) {
Lukasz_K 0:79cb20d1d3d4 121 greenLed = ON;
Lukasz_K 0:79cb20d1d3d4 122 wait(0.1);
Lukasz_K 0:79cb20d1d3d4 123 greenLed = OFF;
Lukasz_K 0:79cb20d1d3d4 124 wait(0.1);
Lukasz_K 0:79cb20d1d3d4 125 newX = mag.readVal(MAG_OUT_X_MSB);
Lukasz_K 0:79cb20d1d3d4 126 newY = mag.readVal(MAG_OUT_Y_MSB);
Lukasz_K 0:79cb20d1d3d4 127 if (newX > tempXmax) tempXmax = newX;
Lukasz_K 0:79cb20d1d3d4 128 if (newX < tempXmin) tempXmin = newX;
Lukasz_K 0:79cb20d1d3d4 129 if (newY > tempYmax) tempYmax = newY;
Lukasz_K 0:79cb20d1d3d4 130 if (newY < tempYmin) tempYmin = newY;
Lukasz_K 0:79cb20d1d3d4 131 }
Lukasz_K 0:79cb20d1d3d4 132 mag.setCalibration( tempXmin, tempXmax, tempYmin, tempYmax );
Lukasz_K 0:79cb20d1d3d4 133
Lukasz_K 0:79cb20d1d3d4 134 // Oczekiwanie na zwolnienie nacisku
Lukasz_K 0:79cb20d1d3d4 135 while( tsi.readDistance() != 0 ) {
Lukasz_K 0:79cb20d1d3d4 136 greenLed = OFF;
Lukasz_K 0:79cb20d1d3d4 137 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 138 greenLed = ON;
Lukasz_K 0:79cb20d1d3d4 139 wait(0.2);
Lukasz_K 0:79cb20d1d3d4 140 }
Lukasz_K 0:79cb20d1d3d4 141 greenLed = OFF;
Lukasz_K 0:79cb20d1d3d4 142 wait(1.0);
Lukasz_K 0:79cb20d1d3d4 143 }