File content as of revision 0:8db1713e7aba:

#include "mbed.h"
#include "Serial_HL.h"

// Testprogramm um zu sehen ob die RGB-Led richtig verlötet
// ist und ob sie sich auch dimmen lässt

SerialBLK pc(USBTX, USBRX);
SvProtocol ua0(&pc);

PwmOut servo1(p21),servo2(p22),servo3(p23),servo4(p24);
int16_t stickVal1,stickVal2,stickVal3,stickVal4;

void CommandHandler();

int main()
{
    servo1.period_ms(20);
    //servo2.period_ms(20);
    //servo3.period_ms(20);
    //servo4.period_ms(20);
    
    servo1.pulsewidth_us(1000);
    servo2.pulsewidth_us(1000);
    servo3.pulsewidth_us(1000);
    servo4.pulsewidth_us(1000);
    
    
    pc.format(8,SerialBLK::None,1);pc.baud(115200);
    ua0.SvMessage("ServoTest");
    
    Timer stw; stw.start();
    while(1)
    {
        CommandHandler();
        if((stw.read_ms()>100))
        {
            stw.reset();
            if(ua0.acqON)
            {
             ua0.WriteSvI16(1, stickVal1);
             ua0.WriteSvI16(2, stickVal2);
             ua0.WriteSvI16(3, stickVal3);  
             ua0.WriteSvI16(4, stickVal4);    
            }
            }
        }
    
    return 1;
}


void CommandHandler()
{
    uint8_t cmd;
   

    // Fragen ob überhaupt etwas im RX-Reg steht
    if( !pc.IsDataAvail() )
        return;

    // wenn etwas im RX-Reg steht
    // Kommando lesen
    cmd = ua0.GetCommand();

    if( cmd==2 ) {
        // cmd2 hat 2 int16 Parameter
        stickVal1 = ua0.ReadI16();
        servo1.pulsewidth_us(1000+stickVal1);
    }
    if( cmd==3 ) {
        // cmd2 hat 2 int16 Parameter
        stickVal2 = ua0.ReadI16();
        servo2.pulsewidth_us(1000+stickVal2);
    }
    if( cmd==4 ) {
        // cmd2 hat 2 int16 Parameter
        stickVal3 = ua0.ReadI16();
        servo3.pulsewidth_us(1000+stickVal3);
    }
}