ABELI / Mbed 2 deprecated Beschleunigungssensor

Werte auslesen und in Winkel umrechnen

Dependencies:   mbed

main.cpp@0:d1960beb98fe, 2015-09-30 (annotated)

Committer:
Heidl
Date:
Wed Sep 30 16:08:36 2015 +0000
Revision:
0:d1960beb98fe
Werte von Beschleunigungssensor auslesen

Who changed what in which revision?

UserRevisionLine numberNew 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 0:d1960beb98fe 20 int16_t gyroXAngle;
Heidl 0:d1960beb98fe 21
Heidl 0:d1960beb98fe 22 main() {
Heidl 0:d1960beb98fe 23
Heidl 0:d1960beb98fe 24 ua0.SvMessage("MPU6050-Test"); // Meldung zum PC senden
Heidl 0:d1960beb98fe 25 mpu6050Address = setAddress(MPU6050_DEFAULT_ADDRESS);
Heidl 0:d1960beb98fe 26 MPU6050 mpu6050;
Heidl 0:d1960beb98fe 27 mpu6050.initialize(); //für Ausgabe in Terminal "//" von debug-Befehlen entfernen
Heidl 0:d1960beb98fe 28 //mpu6050.setDLPFMode(MPU6050_DLPF_BW_98);
Heidl 0:d1960beb98fe 29 //mpu6050.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
Heidl 0:d1960beb98fe 30 accelX = mpu6050.getAccelerationX();
Heidl 0:d1960beb98fe 31 //mpu6050.debugSerial.printf("Beschleunigung in x-Richtung: %d\n\r", accelX);
Heidl 0:d1960beb98fe 32 sinX = (double)accelX/16384;
Heidl 0:d1960beb98fe 33 angleX = (int16_t) asin(sinX)*57.296;
Heidl 0:d1960beb98fe 34 //mpu6050.debugSerial.printf("Neigung: %f\n\r", angleX*57.296);
Heidl 0:d1960beb98fe 35
Heidl 0:d1960beb98fe 36 Timer stw; stw.start();
Heidl 0:d1960beb98fe 37 // Timer gyro; gyro.start();
Heidl 0:d1960beb98fe 38
Heidl 0:d1960beb98fe 39 while(1)
Heidl 0:d1960beb98fe 40 {
Heidl 0:d1960beb98fe 41 CommandHandler();
Heidl 0:d1960beb98fe 42 /* if (gyro.read_us()>125)
Heidl 0:d1960beb98fe 43 {
Heidl 0:d1960beb98fe 44 gyro.reset();
Heidl 0:d1960beb98fe 45 gyroXRate = mpu6050.getRotationX();
Heidl 0:d1960beb98fe 46 gyroXRate = gyroXRate*0.125+3;
Heidl 0:d1960beb98fe 47 gyroXRate = gyroXRate*0.000125;
Heidl 0:d1960beb98fe 48 gyroXAngle = gyroXAngle + gyroXRate;
Heidl 0:d1960beb98fe 49 }
Heidl 0:d1960beb98fe 50 */ accelX = mpu6050.getAccelerationX();
Heidl 0:d1960beb98fe 51 if( (stw.read_ms()>10) ) // 100Hz
Heidl 0:d1960beb98fe 52 { // dieser Teil wird mit 100Hz aufgerufen
Heidl 0:d1960beb98fe 53 stw.reset();
Heidl 0:d1960beb98fe 54 sinX = (double)accelX/16384;
Heidl 0:d1960beb98fe 55 angleX = (float)asin(sinX)*57.296;
Heidl 0:d1960beb98fe 56 if( ua0.acqON ) {
Heidl 0:d1960beb98fe 57 // nur wenn vom PC aus das Senden eingeschaltet wurde
Heidl 0:d1960beb98fe 58 // wird auch etwas gesendet
Heidl 0:d1960beb98fe 59 ua0.WriteSvI16(1, angleX-1);
Heidl 0:d1960beb98fe 60 // ua0.WriteSvI16(2, gyroXAngle);
Heidl 0:d1960beb98fe 61 }
Heidl 0:d1960beb98fe 62 }
Heidl 0:d1960beb98fe 63 }
Heidl 0:d1960beb98fe 64 }
Heidl 0:d1960beb98fe 65
Heidl 0:d1960beb98fe 66
Heidl 0:d1960beb98fe 67 void CommandHandler()
Heidl 0:d1960beb98fe 68 {
Heidl 0:d1960beb98fe 69 uint8_t cmd;
Heidl 0:d1960beb98fe 70 int16_t idata1, idata2;
Heidl 0:d1960beb98fe 71
Heidl 0:d1960beb98fe 72 // Fragen ob überhaupt etwas im RX-Reg steht
Heidl 0:d1960beb98fe 73 if( !pc.IsDataAvail() )
Heidl 0:d1960beb98fe 74 return;
Heidl 0:d1960beb98fe 75
Heidl 0:d1960beb98fe 76 // wenn etwas im RX-Reg steht
Heidl 0:d1960beb98fe 77 // Kommando lesen
Heidl 0:d1960beb98fe 78 cmd = ua0.GetCommand();
Heidl 0:d1960beb98fe 79
Heidl 0:d1960beb98fe 80 if( cmd==3 )
Heidl 0:d1960beb98fe 81 {
Heidl 0:d1960beb98fe 82 idata1 = ua0.ReadI16();
Heidl 0:d1960beb98fe 83 leds = idata1;
Heidl 0:d1960beb98fe 84 ua0.SvMessage("Leds schalten");
Heidl 0:d1960beb98fe 85 }
Heidl 0:d1960beb98fe 86 if( cmd==4 )
Heidl 0:d1960beb98fe 87 {
Heidl 0:d1960beb98fe 88 idata1 = leds;
Heidl 0:d1960beb98fe 89 ua0.SvPrintf("LEDS %d", idata1);
Heidl 0:d1960beb98fe 90 }
Heidl 0:d1960beb98fe 91 }