![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
bluetooth_0110
Diff: main.cpp
- Revision:
- 0:11047e265142
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Oct 01 18:18:46 2015 +0000 @@ -0,0 +1,75 @@ + +#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); +SerialBLK blue(p9, p10); +int16_t stickVal1,stickVal2,stickVal3; + +void CommandHandler(); + +int main() +{ + servo1.period_ms(20); + servo2.period_ms(20); + servo3.period_ms(20); + + pc.format(8,SerialBLK::None,1);pc.baud(115200);blue.baud(115200); + ua0.SvMessage("ServoTest"); + + Timer stw; stw.start(); + while(1) + { + CommandHandler(); + if((stw.read_ms()>100)) + { + stw.reset(); + if(ua0.acqON) + { + ua0.SvMessage(stickVal1); + 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( !blue.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); + } +} \ No newline at end of file