AccelTest3.cpp

00001 
00002 #include "mbed.h"
00003 #include "MMA7660.h" 
00004 #include "Serial_HL.h"
00005 #include "TP1Ord_18.h"
00006 
00007 SerialBLK pc(USBTX, USBRX); //Serial Block KLasse: reine serielle Schnitstelle (Baudrate und 2 Pinnamen einstellbar)(PutChar, GetChar, Write, Read)
00008 SvProtocol ua0(&pc);//SvVis Protokoll Klasse
00009 
00010 void CommandHandler();
00011 
00012 MMA7660 accel(p28, p27);
00013 
00014 void Do100Hz_Work();
00015 
00016 // 3 Filter anlegen
00017 TP1Ord tpx, tpy, tpz;
00018 int accVal[3];
00019 
00020 PwmOut servo(p21);
00021 
00022 int main()
00023 {
00024   pc.format(8,SerialBLK::None,1);
00025   pc.baud(115200);
00026     ua0.SvMessage("AccelTest2_2");//Meldung zum PC senden (SvVis)
00027     //ua0.SvPrintf("TestIntegerwerte: %i %i", int1, int2);//Werte zum PC senden (SvVis)
00028     
00029     accel.setSampleRate(120);
00030     Timer stw; stw.start();
00031     
00032     servo.period_ms(20);
00033     servo.pulsewidth_us(1000);//voll links
00034     servo.pulsewidth_us(2000);//voll rechts
00035     
00036     while(1)
00037   {
00038     CommandHandler();
00039         if( stw.read_ms()>10 ) { // 100Hz
00040             stw.reset();
00041             Do100Hz_Work();
00042             if( ua0.acqON ) {
00043                 ua0.WriteSvI16(1, accVal[0]); // X,Y,Z zum PC senden (SvVis)
00044                 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)
00045                 //ua0.WriteSvI16(2, tpy.y);
00046                 //ua0.WriteSvI16(3, tpz.y);
00047                 
00048                 
00049                 
00050                 // ua0.WriteSvI16(3, accVal[1]);
00051                 // ua0.WriteSvI16(4, tpy.y);
00052             }
00053     }
00054   }
00055 }
00056 
00057 void Do100Hz_Work()
00058 {
00059     accel.readData(accVal); // X,Y,Z vom sensor lesen
00060     tpx.CalcOneStep(accVal[0]);
00061     tpy.CalcOneStep(accVal[1]);
00062     tpz.CalcOneStep(accVal[2]);
00063 }
00064 
00065 void CommandHandler()
00066 {
00067   uint8_t cmd;
00068   if( !pc.IsDataAvail() )
00069     return;
00070   cmd = ua0.GetCommand();
00071   
00072   if (cmd==2)
00073   {
00074     float alpha = ua0.ReadF();
00075     tpx.SetAlpha(alpha); 
00076     tpy.SetAlpha(alpha);
00077     tpz.SetAlpha(alpha); 
00078   } 
00079   
00080     if (cmd==3)
00081   {
00082     //im SvVis3 0...1000 eingeben
00083     servo.pulsewidth_us(1000 + ua0.ReadI16());
00084   } 
00085   
00086 }
00087