File content as of revision 1:353c3b1ad238:

 
#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(p13,p14);
SvProtocol ua0(&pc);
DigitalOut led1(LED1);
Ticker t1;
 
PwmOut servo1(p21),servo2(p22),servo3(p23);
int16_t stickVal1,stickVal2,stickVal3;
int speed=1000;
 
void CommandHandler();

void morespeed() {
    if (stickVal1 == 1)
    {   if(speed < 2000)
            {
            speed = speed + 50;
            servo1.pulsewidth_us(speed);
            servo2.pulsewidth_us(speed);
            ua0.SvMessage("+100");
            }
        }
    if (stickVal1 == 2)
    {
        if(speed > 1000)
            {
            speed = speed - 50;
            servo1.pulsewidth_us(speed);
            servo2.pulsewidth_us(speed);
            ua0.SvMessage("-100");
            }
        }
}
 
int main()
{   
    servo1.period_ms(20);
    servo2.period_ms(20);
    servo3.period_ms(20);
    t1.attach(&morespeed, 3.0);
    
     pc.format(8,SerialBLK::None,1);pc.baud(9600);
    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);   
                }
            }
        }
    
    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
        
        led1 = 1;
        stickVal1 = ua0.ReadI16();
        //servo1.pulsewidth_us(1000+stickVal1);
        //servo2.pulsewidth_us(1000+stickVal1);
        //ua0.SvMessage("Gerade");
    }
    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);
        if(stickVal3 == 1)
            {servo3.pulsewidth_us(1000);
            ua0.SvMessage("Links");
            }
        if(stickVal3 == 2)
            {servo3.pulsewidth_us(1500);
            ua0.SvMessage("Gerade");
            }
        if(stickVal3 == 3)
            {servo3.pulsewidth_us(1650);
            ua0.SvMessage("Rechts");
            }
        
    }
}