AccelTest_Baseb
Dependencies: MMA7660 Serial_HL mbed
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
diff -r c4be955ae46c -r 0c47899d1aba AccelTest3.cpp --- /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()); + } + +} +
diff -r c4be955ae46c -r 0c47899d1aba BasebAccelTest.cpp --- 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 ) { - } -}
diff -r c4be955ae46c -r 0c47899d1aba TP1Ord_18.h --- /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; +}