AccelTest_Baseb
Dependencies: MMA7660 Serial_HL mbed
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()); + } + +} +