main.cpp

00001 
00002 #include "mbed.h"
00003 #include "Serial_HL.h"
00004 
00005 // Testprogramm um zu sehen ob die RGB-Led richtig verlötet
00006 // ist und ob sie sich auch dimmen lässt
00007 
00008 SerialBLK pc(USBTX, USBRX);
00009 SvProtocol ua0(&pc);
00010 
00011 PwmOut servo1(p21),servo2(p22),servo3(p23);
00012 int16_t stickVal1,stickVal2,stickVal3;
00013 
00014 void CommandHandler();
00015 
00016 int main()
00017 {
00018     servo1.period_ms(20);
00019     servo2.period_ms(20);
00020     servo3.period_ms(20);
00021     
00022     pc.format(8,SerialBLK::None,1);pc.baud(500000);
00023     ua0.SvMessage("ServoTest");
00024     
00025     Timer stw; stw.start();
00026     while(1)
00027     {
00028         CommandHandler();
00029         if((stw.read_ms()>100))
00030         {
00031             stw.reset();
00032             if(ua0.acqON)
00033             {
00034              ua0.WriteSvI16(1, stickVal1);
00035              ua0.WriteSvI16(2, stickVal2);
00036              ua0.WriteSvI16(3, stickVal3);   
00037                 }
00038             }
00039         }
00040     
00041     return 1;
00042 }
00043 
00044 
00045 void CommandHandler()
00046 {
00047     uint8_t cmd;
00048    
00049 
00050     // Fragen ob überhaupt etwas im RX-Reg steht
00051     if( !pc.IsDataAvail() )
00052         return;
00053 
00054     // wenn etwas im RX-Reg steht
00055     // Kommando lesen
00056     cmd = ua0.GetCommand();
00057 
00058     if( cmd==2 ) {
00059         // cmd2 hat 2 int16 Parameter
00060         stickVal1 = ua0.ReadI16();
00061         servo1.pulsewidth_us(1000+stickVal1);
00062     }
00063     if( cmd==3 ) {
00064         // cmd2 hat 2 int16 Parameter
00065         stickVal2 = ua0.ReadI16();
00066         servo2.pulsewidth_us(1000+stickVal2);
00067     }
00068     if( cmd==4 ) {
00069         // cmd2 hat 2 int16 Parameter
00070         stickVal3 = ua0.ReadI16();
00071         servo3.pulsewidth_us(1000+stickVal3);
00072     }
00073 }