Martin Werluschnig / Mbed 2 deprecated AccelTest_Baseb

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?

UserRevisionLine numberNew 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