Werte auslesen und in Winkel umrechnen
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:d1960beb98fe
diff -r 000000000000 -r d1960beb98fe main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Sep 30 16:08:36 2015 +0000 @@ -0,0 +1,91 @@ +#include "MPU6050.h" +#include "Serial_HL.h" +#include "math.h" +BusOut leds(LED4,LED3,LED2,LED1); +DigitalOut mpu6050Address (p29); + +SerialBLK pc(USBTX, USBRX); +SvProtocol ua0(&pc); + +void CommandHandler(); + +int setAddress (int address) { + return address-0x68; // This function determines the address of the sensor via the AD0-pin. +} // If it should be 0x68, the AD0-pin is set to GND (0), otherwise it's set to VCC (1). + +int16_t accelX; +double sinX; +int16_t angleX; +int16_t gyroXRate; +int16_t gyroXAngle; + +main() { + + ua0.SvMessage("MPU6050-Test"); // Meldung zum PC senden + mpu6050Address = setAddress(MPU6050_DEFAULT_ADDRESS); + MPU6050 mpu6050; + mpu6050.initialize(); //für Ausgabe in Terminal "//" von debug-Befehlen entfernen + //mpu6050.setDLPFMode(MPU6050_DLPF_BW_98); + //mpu6050.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); + accelX = mpu6050.getAccelerationX(); + //mpu6050.debugSerial.printf("Beschleunigung in x-Richtung: %d\n\r", accelX); + sinX = (double)accelX/16384; + angleX = (int16_t) asin(sinX)*57.296; + //mpu6050.debugSerial.printf("Neigung: %f\n\r", angleX*57.296); + + Timer stw; stw.start(); +// Timer gyro; gyro.start(); + + while(1) + { + CommandHandler(); + /* if (gyro.read_us()>125) + { + gyro.reset(); + gyroXRate = mpu6050.getRotationX(); + gyroXRate = gyroXRate*0.125+3; + gyroXRate = gyroXRate*0.000125; + gyroXAngle = gyroXAngle + gyroXRate; + } +*/ accelX = mpu6050.getAccelerationX(); + if( (stw.read_ms()>10) ) // 100Hz + { // dieser Teil wird mit 100Hz aufgerufen + stw.reset(); + sinX = (double)accelX/16384; + angleX = (float)asin(sinX)*57.296; + if( ua0.acqON ) { + // nur wenn vom PC aus das Senden eingeschaltet wurde + // wird auch etwas gesendet + ua0.WriteSvI16(1, angleX-1); +// ua0.WriteSvI16(2, gyroXAngle); + } + } + } +} + + +void CommandHandler() +{ + uint8_t cmd; + int16_t idata1, idata2; + + // Fragen ob überhaupt etwas im RX-Reg steht + if( !pc.IsDataAvail() ) + return; + + // wenn etwas im RX-Reg steht + // Kommando lesen + cmd = ua0.GetCommand(); + + if( cmd==3 ) + { + idata1 = ua0.ReadI16(); + leds = idata1; + ua0.SvMessage("Leds schalten"); + } + if( cmd==4 ) + { + idata1 = leds; + ua0.SvPrintf("LEDS %d", idata1); + } +} \ No newline at end of file