Diff: main_TTS.cpp

Revision:

2:d4aaf6997c1a

Parent:

0:c1875ecb8b79

diff -r a75f33f74f30 -r d4aaf6997c1a main_TTS.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_TTS.cpp	Wed Oct 13 11:45:26 2021 +0000
@@ -0,0 +1,86 @@
+/* ******************************************************************* */
+/* ***** HUMER TRAP TRAINING (TTS) SYSTEM Q1/2021 Version BETA ******* */
+/* ***** Erstellung des Rohgerüstes, 8.2.2021 ************************ */
+/* ***** Erstellung der Messdaten und digitales Filter, 10.2.21 ****** */
+/* ***** Firma: shotkam.com / Auftrag 257-23A ************************ */
+/* ******************************************************************* */
+#include "mbed.h"
+#include "MPU6050.h"
+Serial pc(USBTX, USBRX);     // Deklaration serielle Schn. für NUC-F303K8
+Timer t;
+float zeitintervall = 0.2;
+float ax, ay, az, gx, gy, gz, temp, Temp; // gefilterte Werte für MPU6050
+MPU6050 mpu(D4,D5);         // Deklaration mpu6050: (SDA, SCL)
+int count;              //
+char fcount;           // Zähler für Array, digitales Filter
+char debugser=1, i=0;             // Debug serielle Schnittstelle 1=on, 0=0ff
+char filterflag=0;           // 1 = Digitales Filter, Berechnung
+Ticker mtick;               // Ticker 25ms
+float adata[3],gdata[3];              // Temporäre Daten bezüglich MPU6050
+float fadata[3][3],fgdata[3][3],ftemp[3];
+
+/* ***************** Prototypendeklaration *************************** */
+void init_board();
+void init_mpu6050();
+void debugausgabe();
+void filter();
+
+/* *********** Interruptfunktionen *********************************** */
+void tickerbase (void)      // Interruptfuktion, Ticker
+{   filterflag=1;  
+}   //end filterbase, Tickerfunktion
+
+/* ***************** Hauptprogramm ******************************* */
+int main()
+{       init_board(); init_mpu6050();
+    while(1) 
+    {
+        if(filterflag==1)
+        {   count++;    filter();   filterflag=0;
+        }   //endif
+        
+    }   //end while()
+}       //end main()
+
+/* *************************** Funktionsprogramme **************** */
+void debugausgabe()
+{   if (debugser==1)
+    {   pc.printf("%4d; ax=%5.1f; ay=%5.1f; az=%5.1f",count,ax,ay,az);
+        pc.printf(" gx=%5.1f; gy=%5.1f; gz=%5.1f",gx,gy,gz);
+        pc.printf(" Temp:= %6.2f \r", Temp);
+    }
+} 
+void filter(void)
+{   // Digitales Kammfilter
+            mpu.getAccelero(fadata[fcount]); //Einlesen Beschleunigung
+            mpu.getGyro(fgdata[fcount]);    //Einlesen Gyrator
+            ftemp[fcount]=mpu.getTemp();      // Sensorzeit Summe: 2,3ms
+            ax=(fadata[0][0]+fadata[1][0]+fadata[2][0])/3.0; //Filter
+            ay=(fadata[0][1]+fadata[1][1]+fadata[2][1])/3.0;
+            az=(fadata[0][2]+fadata[1][2]+fadata[2][2])/3.0;
+            gx=(fgdata[0][0]+fgdata[1][1]+fgdata[2][0])*5.0;
+            gy=(fgdata[0][1]+fgdata[1][1]+fgdata[2][1])*5.0;
+            gz=(fgdata[0][2]+fgdata[1][2]+fgdata[2][2])*5.0;
+            Temp=(ftemp[0]+ftemp[1]+ftemp[2])/3.0;    
+            fcount++; if(fcount==3) fcount=0;
+            if(debugser==1) debugausgabe();              // Ausgabezeit: 6,6ms
+}
+
+void init_mpu6050()                 // Funktion init_mpu6050 Initialisierung
+{
+    bool ok ;
+    ok = mpu.testConnection();
+    if(ok) 
+    {
+        if (debugser==1) pc.printf("Test is ok!\n");
+        mpu.setSleepMode(0); //THIS WILL GET THE SENSOR OUT OF SLEEP MODE.
+    } else
+        if (debugser==1) pc.printf("Test failed!\n");
+}
+
+void init_board(void)                   // Funktion init_board
+{
+    pc.baud(115900);
+    wait(10);
+    mtick.attach(&tickerbase, zeitintervall); // Int. Interrupt Ticker
+}
\ No newline at end of file