![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Motor test DA
main.cpp@0:8db1713e7aba, 2015-10-13 (annotated)
- Committer:
- thesteira
- Date:
- Tue Oct 13 19:47:40 2015 +0000
- Revision:
- 0:8db1713e7aba
Motor test DA
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
thesteira | 0:8db1713e7aba | 1 | |
thesteira | 0:8db1713e7aba | 2 | #include "mbed.h" |
thesteira | 0:8db1713e7aba | 3 | #include "Serial_HL.h" |
thesteira | 0:8db1713e7aba | 4 | |
thesteira | 0:8db1713e7aba | 5 | // Testprogramm um zu sehen ob die RGB-Led richtig verlötet |
thesteira | 0:8db1713e7aba | 6 | // ist und ob sie sich auch dimmen lässt |
thesteira | 0:8db1713e7aba | 7 | |
thesteira | 0:8db1713e7aba | 8 | SerialBLK pc(USBTX, USBRX); |
thesteira | 0:8db1713e7aba | 9 | SvProtocol ua0(&pc); |
thesteira | 0:8db1713e7aba | 10 | |
thesteira | 0:8db1713e7aba | 11 | PwmOut servo1(p21),servo2(p22),servo3(p23),servo4(p24); |
thesteira | 0:8db1713e7aba | 12 | int16_t stickVal1,stickVal2,stickVal3,stickVal4; |
thesteira | 0:8db1713e7aba | 13 | |
thesteira | 0:8db1713e7aba | 14 | void CommandHandler(); |
thesteira | 0:8db1713e7aba | 15 | |
thesteira | 0:8db1713e7aba | 16 | int main() |
thesteira | 0:8db1713e7aba | 17 | { |
thesteira | 0:8db1713e7aba | 18 | servo1.period_ms(20); |
thesteira | 0:8db1713e7aba | 19 | //servo2.period_ms(20); |
thesteira | 0:8db1713e7aba | 20 | //servo3.period_ms(20); |
thesteira | 0:8db1713e7aba | 21 | //servo4.period_ms(20); |
thesteira | 0:8db1713e7aba | 22 | |
thesteira | 0:8db1713e7aba | 23 | servo1.pulsewidth_us(1000); |
thesteira | 0:8db1713e7aba | 24 | servo2.pulsewidth_us(1000); |
thesteira | 0:8db1713e7aba | 25 | servo3.pulsewidth_us(1000); |
thesteira | 0:8db1713e7aba | 26 | servo4.pulsewidth_us(1000); |
thesteira | 0:8db1713e7aba | 27 | |
thesteira | 0:8db1713e7aba | 28 | |
thesteira | 0:8db1713e7aba | 29 | pc.format(8,SerialBLK::None,1);pc.baud(115200); |
thesteira | 0:8db1713e7aba | 30 | ua0.SvMessage("ServoTest"); |
thesteira | 0:8db1713e7aba | 31 | |
thesteira | 0:8db1713e7aba | 32 | Timer stw; stw.start(); |
thesteira | 0:8db1713e7aba | 33 | while(1) |
thesteira | 0:8db1713e7aba | 34 | { |
thesteira | 0:8db1713e7aba | 35 | CommandHandler(); |
thesteira | 0:8db1713e7aba | 36 | if((stw.read_ms()>100)) |
thesteira | 0:8db1713e7aba | 37 | { |
thesteira | 0:8db1713e7aba | 38 | stw.reset(); |
thesteira | 0:8db1713e7aba | 39 | if(ua0.acqON) |
thesteira | 0:8db1713e7aba | 40 | { |
thesteira | 0:8db1713e7aba | 41 | ua0.WriteSvI16(1, stickVal1); |
thesteira | 0:8db1713e7aba | 42 | ua0.WriteSvI16(2, stickVal2); |
thesteira | 0:8db1713e7aba | 43 | ua0.WriteSvI16(3, stickVal3); |
thesteira | 0:8db1713e7aba | 44 | ua0.WriteSvI16(4, stickVal4); |
thesteira | 0:8db1713e7aba | 45 | } |
thesteira | 0:8db1713e7aba | 46 | } |
thesteira | 0:8db1713e7aba | 47 | } |
thesteira | 0:8db1713e7aba | 48 | |
thesteira | 0:8db1713e7aba | 49 | return 1; |
thesteira | 0:8db1713e7aba | 50 | } |
thesteira | 0:8db1713e7aba | 51 | |
thesteira | 0:8db1713e7aba | 52 | |
thesteira | 0:8db1713e7aba | 53 | void CommandHandler() |
thesteira | 0:8db1713e7aba | 54 | { |
thesteira | 0:8db1713e7aba | 55 | uint8_t cmd; |
thesteira | 0:8db1713e7aba | 56 | |
thesteira | 0:8db1713e7aba | 57 | |
thesteira | 0:8db1713e7aba | 58 | // Fragen ob überhaupt etwas im RX-Reg steht |
thesteira | 0:8db1713e7aba | 59 | if( !pc.IsDataAvail() ) |
thesteira | 0:8db1713e7aba | 60 | return; |
thesteira | 0:8db1713e7aba | 61 | |
thesteira | 0:8db1713e7aba | 62 | // wenn etwas im RX-Reg steht |
thesteira | 0:8db1713e7aba | 63 | // Kommando lesen |
thesteira | 0:8db1713e7aba | 64 | cmd = ua0.GetCommand(); |
thesteira | 0:8db1713e7aba | 65 | |
thesteira | 0:8db1713e7aba | 66 | if( cmd==2 ) { |
thesteira | 0:8db1713e7aba | 67 | // cmd2 hat 2 int16 Parameter |
thesteira | 0:8db1713e7aba | 68 | stickVal1 = ua0.ReadI16(); |
thesteira | 0:8db1713e7aba | 69 | servo1.pulsewidth_us(1000+stickVal1); |
thesteira | 0:8db1713e7aba | 70 | } |
thesteira | 0:8db1713e7aba | 71 | if( cmd==3 ) { |
thesteira | 0:8db1713e7aba | 72 | // cmd2 hat 2 int16 Parameter |
thesteira | 0:8db1713e7aba | 73 | stickVal2 = ua0.ReadI16(); |
thesteira | 0:8db1713e7aba | 74 | servo2.pulsewidth_us(1000+stickVal2); |
thesteira | 0:8db1713e7aba | 75 | } |
thesteira | 0:8db1713e7aba | 76 | if( cmd==4 ) { |
thesteira | 0:8db1713e7aba | 77 | // cmd2 hat 2 int16 Parameter |
thesteira | 0:8db1713e7aba | 78 | stickVal3 = ua0.ReadI16(); |
thesteira | 0:8db1713e7aba | 79 | servo3.pulsewidth_us(1000+stickVal3); |
thesteira | 0:8db1713e7aba | 80 | } |
thesteira | 0:8db1713e7aba | 81 | } |