Diff: main.cpp

Revision:

0:d1960beb98fe

--- /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