Revision 1:0c47899d1aba, committed 2018-11-15

Comitter:

martwerl

Date:

Thu Nov 15 17:22:07 2018 +0000

Parent:

0:c4be955ae46c

Commit message:

AccelTest_Baseb

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AccelTest3.cpp	Thu Nov 15 17:22:07 2018 +0000
@@ -0,0 +1,87 @@
+
+#include "mbed.h"
+#include "MMA7660.h" 
+#include "Serial_HL.h"
+#include "TP1Ord_18.h"
+
+SerialBLK pc(USBTX, USBRX); //Serial Block KLasse: reine serielle Schnitstelle (Baudrate und 2 Pinnamen einstellbar)(PutChar, GetChar, Write, Read)
+SvProtocol ua0(&pc);//SvVis Protokoll Klasse
+
+void CommandHandler();
+
+MMA7660 accel(p28, p27);
+
+void Do100Hz_Work();
+
+// 3 Filter anlegen
+TP1Ord tpx, tpy, tpz;
+int accVal[3];
+
+PwmOut servo(p21);
+
+int main()
+{
+  pc.format(8,SerialBLK::None,1);
+  pc.baud(115200);
+	ua0.SvMessage("AccelTest2_2");//Meldung zum PC senden (SvVis)
+	//ua0.SvPrintf("TestIntegerwerte: %i %i", int1, int2);//Werte zum PC senden (SvVis)
+	
+    accel.setSampleRate(120);
+	Timer stw; stw.start();
+	
+	servo.period_ms(20);
+	servo.pulsewidth_us(1000);//voll links
+	servo.pulsewidth_us(2000);//voll rechts
+	
+	while(1)
+  {
+    CommandHandler();
+		if( stw.read_ms()>10 ) { // 100Hz
+			stw.reset();
+			Do100Hz_Work();
+			if( ua0.acqON ) {
+				ua0.WriteSvI16(1, accVal[0]); // X,Y,Z zum PC senden (SvVis)
+				ua0.WriteSvI16(2, tpx.y);//die Achse mit Filter ans HTerm schicken (Tiefpass) Unterschied: mit Tiefpass ändern sich die Werte nicht so schnell (z.B. gegen Vibrationen)
+				//ua0.WriteSvI16(2, tpy.y);
+				//ua0.WriteSvI16(3, tpz.y);
+				
+				
+				
+				// ua0.WriteSvI16(3, accVal[1]);
+				// ua0.WriteSvI16(4, tpy.y);
+			}
+    }
+  }
+}
+
+void Do100Hz_Work()
+{
+	accel.readData(accVal); // X,Y,Z vom sensor lesen
+	tpx.CalcOneStep(accVal[0]);
+	tpy.CalcOneStep(accVal[1]);
+	tpz.CalcOneStep(accVal[2]);
+}
+
+void CommandHandler()
+{
+  uint8_t cmd;
+  if( !pc.IsDataAvail() )
+    return;
+  cmd = ua0.GetCommand();
+  
+  if (cmd==2)
+  {
+  	float alpha = ua0.ReadF();
+  	tpx.SetAlpha(alpha); 
+  	tpy.SetAlpha(alpha);
+  	tpz.SetAlpha(alpha); 
+  }	
+  
+    if (cmd==3)
+  {
+  	//im SvVis3 0...1000 eingeben
+  	servo.pulsewidth_us(1000 + ua0.ReadI16());
+  }	
+  
+}
+

--- a/BasebAccelTest.cpp	Mon Dec 05 15:46:02 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,50 +0,0 @@
-#include "mbed.h"
-#include "MMA7660.h" 
-#include "Serial_HL.h"
-
-SerialBLK pc(USBTX, USBRX);
-SvProtocol ua0(&pc);
-
-void CommandHandler();
-
-MMA7660 accel(p28, p27); 
-
-int main()
-{
-  pc.format(8,SerialBLK::None,1);
-  pc.baud(115200);
-
-  ua0.SvMessage("AccelTest2_2");
-  
-  accel.setSampleRate(120);
-  
-  Timer stw; stw.start();
-  
-  int val2[3];
-  
-  while(1)
-  {
-    CommandHandler();
-    if( ua0.acqON && (stw.read_ms()>10) ) // 100Hz
-    {
-            stw.reset();
-      accel.readData(val2); // X,Y,Z vom sensor lesen
-      ua0.WriteSvI16(1, val2[0]); // X,Y,Z zum PC senden
-      ua0.WriteSvI16(2, val2[1]);
-      ua0.WriteSvI16(3, val2[2]);
-    }
-  }
-}
-
-
-void CommandHandler()
-{
-  uint8_t cmd;
-  if( !pc.IsDataAvail() )
-    return;
-  cmd = ua0.GetCommand();
-  if( cmd==2 ) {
-  }
-  if( cmd==3 ) {
-  }
-}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TP1Ord_18.h	Thu Nov 15 17:22:07 2018 +0000
@@ -0,0 +1,40 @@
+
+
+// Digitaler Tiefpass 1er Ordnung
+
+class TP1Ord {
+	private:
+		float alpha; // Filter-Constante
+	public:
+		float yn_1;
+		float y; // momentaner Ausgangswert des Filters
+	public:
+		// Konstruktor wird automatisch aufgerufen wenn eine
+		// Variable der Klasse TP1Ord angelegt wird.
+		// interne Variablen sinnvoll initialisieren
+		TP1Ord(); 
+	
+	  // einen Abtastschritt des Filters rechnen
+		// es entsteht ein neues y
+		void CalcOneStep(float aX);
+	
+		void SetAlpha(float aAlpha);
+};
+
+TP1Ord::TP1Ord()
+{
+	y=0; yn_1=0;
+	SetAlpha(0.1); // sinnvalles Alpha einstellen
+}
+
+void TP1Ord::CalcOneStep(float aX)
+{
+	y = alpha*aX + (1-alpha)*yn_1;
+	yn_1 = y; // y einmal merken ( zwischenspeichern )
+}
+
+void TP1Ord::SetAlpha(float aAlpha)
+{ // 0..1 absichern
+	if( aAlpha>=0 && aAlpha<=1 )
+		alpha = aAlpha;
+}