Diff: AccelTest3.cpp

Revision:

1:0c47899d1aba

--- /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());
+  }	
+  
+}
+