AccelTest_Baseb
Dependencies: MMA7660 Serial_HL mbed
AccelTest3.cpp@1:0c47899d1aba, 2018-11-15 (annotated)
- Committer:
- martwerl
- Date:
- Thu Nov 15 17:22:07 2018 +0000
- Revision:
- 1:0c47899d1aba
AccelTest_Baseb
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
martwerl | 1:0c47899d1aba | 1 | |
martwerl | 1:0c47899d1aba | 2 | #include "mbed.h" |
martwerl | 1:0c47899d1aba | 3 | #include "MMA7660.h" |
martwerl | 1:0c47899d1aba | 4 | #include "Serial_HL.h" |
martwerl | 1:0c47899d1aba | 5 | #include "TP1Ord_18.h" |
martwerl | 1:0c47899d1aba | 6 | |
martwerl | 1:0c47899d1aba | 7 | SerialBLK pc(USBTX, USBRX); //Serial Block KLasse: reine serielle Schnitstelle (Baudrate und 2 Pinnamen einstellbar)(PutChar, GetChar, Write, Read) |
martwerl | 1:0c47899d1aba | 8 | SvProtocol ua0(&pc);//SvVis Protokoll Klasse |
martwerl | 1:0c47899d1aba | 9 | |
martwerl | 1:0c47899d1aba | 10 | void CommandHandler(); |
martwerl | 1:0c47899d1aba | 11 | |
martwerl | 1:0c47899d1aba | 12 | MMA7660 accel(p28, p27); |
martwerl | 1:0c47899d1aba | 13 | |
martwerl | 1:0c47899d1aba | 14 | void Do100Hz_Work(); |
martwerl | 1:0c47899d1aba | 15 | |
martwerl | 1:0c47899d1aba | 16 | // 3 Filter anlegen |
martwerl | 1:0c47899d1aba | 17 | TP1Ord tpx, tpy, tpz; |
martwerl | 1:0c47899d1aba | 18 | int accVal[3]; |
martwerl | 1:0c47899d1aba | 19 | |
martwerl | 1:0c47899d1aba | 20 | PwmOut servo(p21); |
martwerl | 1:0c47899d1aba | 21 | |
martwerl | 1:0c47899d1aba | 22 | int main() |
martwerl | 1:0c47899d1aba | 23 | { |
martwerl | 1:0c47899d1aba | 24 | pc.format(8,SerialBLK::None,1); |
martwerl | 1:0c47899d1aba | 25 | pc.baud(115200); |
martwerl | 1:0c47899d1aba | 26 | ua0.SvMessage("AccelTest2_2");//Meldung zum PC senden (SvVis) |
martwerl | 1:0c47899d1aba | 27 | //ua0.SvPrintf("TestIntegerwerte: %i %i", int1, int2);//Werte zum PC senden (SvVis) |
martwerl | 1:0c47899d1aba | 28 | |
martwerl | 1:0c47899d1aba | 29 | accel.setSampleRate(120); |
martwerl | 1:0c47899d1aba | 30 | Timer stw; stw.start(); |
martwerl | 1:0c47899d1aba | 31 | |
martwerl | 1:0c47899d1aba | 32 | servo.period_ms(20); |
martwerl | 1:0c47899d1aba | 33 | servo.pulsewidth_us(1000);//voll links |
martwerl | 1:0c47899d1aba | 34 | servo.pulsewidth_us(2000);//voll rechts |
martwerl | 1:0c47899d1aba | 35 | |
martwerl | 1:0c47899d1aba | 36 | while(1) |
martwerl | 1:0c47899d1aba | 37 | { |
martwerl | 1:0c47899d1aba | 38 | CommandHandler(); |
martwerl | 1:0c47899d1aba | 39 | if( stw.read_ms()>10 ) { // 100Hz |
martwerl | 1:0c47899d1aba | 40 | stw.reset(); |
martwerl | 1:0c47899d1aba | 41 | Do100Hz_Work(); |
martwerl | 1:0c47899d1aba | 42 | if( ua0.acqON ) { |
martwerl | 1:0c47899d1aba | 43 | ua0.WriteSvI16(1, accVal[0]); // X,Y,Z zum PC senden (SvVis) |
martwerl | 1:0c47899d1aba | 44 | 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) |
martwerl | 1:0c47899d1aba | 45 | //ua0.WriteSvI16(2, tpy.y); |
martwerl | 1:0c47899d1aba | 46 | //ua0.WriteSvI16(3, tpz.y); |
martwerl | 1:0c47899d1aba | 47 | |
martwerl | 1:0c47899d1aba | 48 | |
martwerl | 1:0c47899d1aba | 49 | |
martwerl | 1:0c47899d1aba | 50 | // ua0.WriteSvI16(3, accVal[1]); |
martwerl | 1:0c47899d1aba | 51 | // ua0.WriteSvI16(4, tpy.y); |
martwerl | 1:0c47899d1aba | 52 | } |
martwerl | 1:0c47899d1aba | 53 | } |
martwerl | 1:0c47899d1aba | 54 | } |
martwerl | 1:0c47899d1aba | 55 | } |
martwerl | 1:0c47899d1aba | 56 | |
martwerl | 1:0c47899d1aba | 57 | void Do100Hz_Work() |
martwerl | 1:0c47899d1aba | 58 | { |
martwerl | 1:0c47899d1aba | 59 | accel.readData(accVal); // X,Y,Z vom sensor lesen |
martwerl | 1:0c47899d1aba | 60 | tpx.CalcOneStep(accVal[0]); |
martwerl | 1:0c47899d1aba | 61 | tpy.CalcOneStep(accVal[1]); |
martwerl | 1:0c47899d1aba | 62 | tpz.CalcOneStep(accVal[2]); |
martwerl | 1:0c47899d1aba | 63 | } |
martwerl | 1:0c47899d1aba | 64 | |
martwerl | 1:0c47899d1aba | 65 | void CommandHandler() |
martwerl | 1:0c47899d1aba | 66 | { |
martwerl | 1:0c47899d1aba | 67 | uint8_t cmd; |
martwerl | 1:0c47899d1aba | 68 | if( !pc.IsDataAvail() ) |
martwerl | 1:0c47899d1aba | 69 | return; |
martwerl | 1:0c47899d1aba | 70 | cmd = ua0.GetCommand(); |
martwerl | 1:0c47899d1aba | 71 | |
martwerl | 1:0c47899d1aba | 72 | if (cmd==2) |
martwerl | 1:0c47899d1aba | 73 | { |
martwerl | 1:0c47899d1aba | 74 | float alpha = ua0.ReadF(); |
martwerl | 1:0c47899d1aba | 75 | tpx.SetAlpha(alpha); |
martwerl | 1:0c47899d1aba | 76 | tpy.SetAlpha(alpha); |
martwerl | 1:0c47899d1aba | 77 | tpz.SetAlpha(alpha); |
martwerl | 1:0c47899d1aba | 78 | } |
martwerl | 1:0c47899d1aba | 79 | |
martwerl | 1:0c47899d1aba | 80 | if (cmd==3) |
martwerl | 1:0c47899d1aba | 81 | { |
martwerl | 1:0c47899d1aba | 82 | //im SvVis3 0...1000 eingeben |
martwerl | 1:0c47899d1aba | 83 | servo.pulsewidth_us(1000 + ua0.ReadI16()); |
martwerl | 1:0c47899d1aba | 84 | } |
martwerl | 1:0c47899d1aba | 85 | |
martwerl | 1:0c47899d1aba | 86 | } |
martwerl | 1:0c47899d1aba | 87 |