Werte auslesen und in Winkel umrechnen
Fork of Beschleunigungssensor by
main.cpp@1:f8c0ebd6096c, 2015-10-08 (annotated)
- Committer:
- Heidl
- Date:
- Thu Oct 08 19:25:13 2015 +0000
- Revision:
- 1:f8c0ebd6096c
- Parent:
- 0:d1960beb98fe
Werte auslesen und in Winkel umrechnen
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Heidl | 0:d1960beb98fe | 1 | #include "MPU6050.h" |
Heidl | 0:d1960beb98fe | 2 | #include "Serial_HL.h" |
Heidl | 0:d1960beb98fe | 3 | #include "math.h" |
Heidl | 0:d1960beb98fe | 4 | BusOut leds(LED4,LED3,LED2,LED1); |
Heidl | 0:d1960beb98fe | 5 | DigitalOut mpu6050Address (p29); |
Heidl | 0:d1960beb98fe | 6 | |
Heidl | 0:d1960beb98fe | 7 | SerialBLK pc(USBTX, USBRX); |
Heidl | 0:d1960beb98fe | 8 | SvProtocol ua0(&pc); |
Heidl | 0:d1960beb98fe | 9 | |
Heidl | 0:d1960beb98fe | 10 | void CommandHandler(); |
Heidl | 0:d1960beb98fe | 11 | |
Heidl | 0:d1960beb98fe | 12 | int setAddress (int address) { |
Heidl | 0:d1960beb98fe | 13 | return address-0x68; // This function determines the address of the sensor via the AD0-pin. |
Heidl | 0:d1960beb98fe | 14 | } // If it should be 0x68, the AD0-pin is set to GND (0), otherwise it's set to VCC (1). |
Heidl | 0:d1960beb98fe | 15 | |
Heidl | 0:d1960beb98fe | 16 | int16_t accelX; |
Heidl | 0:d1960beb98fe | 17 | double sinX; |
Heidl | 0:d1960beb98fe | 18 | int16_t angleX; |
Heidl | 0:d1960beb98fe | 19 | int16_t gyroXRate; |
Heidl | 1:f8c0ebd6096c | 20 | int16_t gyroXAngleTemp; |
Heidl | 1:f8c0ebd6096c | 21 | float gyroXAngle; |
Heidl | 0:d1960beb98fe | 22 | |
Heidl | 0:d1960beb98fe | 23 | main() { |
Heidl | 0:d1960beb98fe | 24 | |
Heidl | 0:d1960beb98fe | 25 | ua0.SvMessage("MPU6050-Test"); // Meldung zum PC senden |
Heidl | 0:d1960beb98fe | 26 | mpu6050Address = setAddress(MPU6050_DEFAULT_ADDRESS); |
Heidl | 0:d1960beb98fe | 27 | MPU6050 mpu6050; |
Heidl | 0:d1960beb98fe | 28 | mpu6050.initialize(); //für Ausgabe in Terminal "//" von debug-Befehlen entfernen |
Heidl | 1:f8c0ebd6096c | 29 | mpu6050.debugSerial.printf("DeviceID: %d", mpu6050.getDeviceID()); |
Heidl | 0:d1960beb98fe | 30 | //mpu6050.setDLPFMode(MPU6050_DLPF_BW_98); |
Heidl | 1:f8c0ebd6096c | 31 | mpu6050.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); |
Heidl | 0:d1960beb98fe | 32 | accelX = mpu6050.getAccelerationX(); |
Heidl | 0:d1960beb98fe | 33 | //mpu6050.debugSerial.printf("Beschleunigung in x-Richtung: %d\n\r", accelX); |
Heidl | 0:d1960beb98fe | 34 | sinX = (double)accelX/16384; |
Heidl | 0:d1960beb98fe | 35 | angleX = (int16_t) asin(sinX)*57.296; |
Heidl | 0:d1960beb98fe | 36 | //mpu6050.debugSerial.printf("Neigung: %f\n\r", angleX*57.296); |
Heidl | 0:d1960beb98fe | 37 | |
Heidl | 0:d1960beb98fe | 38 | Timer stw; stw.start(); |
Heidl | 1:f8c0ebd6096c | 39 | Timer gyro; gyro.start(); |
Heidl | 0:d1960beb98fe | 40 | |
Heidl | 0:d1960beb98fe | 41 | while(1) |
Heidl | 0:d1960beb98fe | 42 | { |
Heidl | 0:d1960beb98fe | 43 | CommandHandler(); |
Heidl | 1:f8c0ebd6096c | 44 | if (gyro.read_us()>125) |
Heidl | 0:d1960beb98fe | 45 | { |
Heidl | 0:d1960beb98fe | 46 | gyro.reset(); |
Heidl | 0:d1960beb98fe | 47 | gyroXRate = mpu6050.getRotationX(); |
Heidl | 0:d1960beb98fe | 48 | gyroXRate = gyroXRate*0.125+3; |
Heidl | 1:f8c0ebd6096c | 49 | gyroXRate = (float)gyroXRate; |
Heidl | 1:f8c0ebd6096c | 50 | gyroXAngleTemp = (float)gyroXRate*0.000125*100; |
Heidl | 1:f8c0ebd6096c | 51 | gyroXAngle = gyroXAngle + gyroXAngleTemp; |
Heidl | 0:d1960beb98fe | 52 | } |
Heidl | 1:f8c0ebd6096c | 53 | accelX = mpu6050.getAccelerationX(); |
Heidl | 0:d1960beb98fe | 54 | if( (stw.read_ms()>10) ) // 100Hz |
Heidl | 0:d1960beb98fe | 55 | { // dieser Teil wird mit 100Hz aufgerufen |
Heidl | 0:d1960beb98fe | 56 | stw.reset(); |
Heidl | 0:d1960beb98fe | 57 | sinX = (double)accelX/16384; |
Heidl | 0:d1960beb98fe | 58 | angleX = (float)asin(sinX)*57.296; |
Heidl | 0:d1960beb98fe | 59 | if( ua0.acqON ) { |
Heidl | 0:d1960beb98fe | 60 | // nur wenn vom PC aus das Senden eingeschaltet wurde |
Heidl | 0:d1960beb98fe | 61 | // wird auch etwas gesendet |
Heidl | 0:d1960beb98fe | 62 | ua0.WriteSvI16(1, angleX-1); |
Heidl | 1:f8c0ebd6096c | 63 | ua0.WriteSvI16(2, gyroXAngle); |
Heidl | 0:d1960beb98fe | 64 | } |
Heidl | 0:d1960beb98fe | 65 | } |
Heidl | 0:d1960beb98fe | 66 | } |
Heidl | 0:d1960beb98fe | 67 | } |
Heidl | 0:d1960beb98fe | 68 | |
Heidl | 0:d1960beb98fe | 69 | |
Heidl | 0:d1960beb98fe | 70 | void CommandHandler() |
Heidl | 0:d1960beb98fe | 71 | { |
Heidl | 0:d1960beb98fe | 72 | uint8_t cmd; |
Heidl | 0:d1960beb98fe | 73 | int16_t idata1, idata2; |
Heidl | 0:d1960beb98fe | 74 | |
Heidl | 0:d1960beb98fe | 75 | // Fragen ob überhaupt etwas im RX-Reg steht |
Heidl | 0:d1960beb98fe | 76 | if( !pc.IsDataAvail() ) |
Heidl | 0:d1960beb98fe | 77 | return; |
Heidl | 0:d1960beb98fe | 78 | |
Heidl | 0:d1960beb98fe | 79 | // wenn etwas im RX-Reg steht |
Heidl | 0:d1960beb98fe | 80 | // Kommando lesen |
Heidl | 0:d1960beb98fe | 81 | cmd = ua0.GetCommand(); |
Heidl | 0:d1960beb98fe | 82 | |
Heidl | 0:d1960beb98fe | 83 | if( cmd==3 ) |
Heidl | 0:d1960beb98fe | 84 | { |
Heidl | 0:d1960beb98fe | 85 | idata1 = ua0.ReadI16(); |
Heidl | 0:d1960beb98fe | 86 | leds = idata1; |
Heidl | 0:d1960beb98fe | 87 | ua0.SvMessage("Leds schalten"); |
Heidl | 0:d1960beb98fe | 88 | } |
Heidl | 0:d1960beb98fe | 89 | if( cmd==4 ) |
Heidl | 0:d1960beb98fe | 90 | { |
Heidl | 0:d1960beb98fe | 91 | idata1 = leds; |
Heidl | 0:d1960beb98fe | 92 | ua0.SvPrintf("LEDS %d", idata1); |
Heidl | 0:d1960beb98fe | 93 | } |
Heidl | 0:d1960beb98fe | 94 | } |