DA_SteiraSchieder / Mbed 2 deprecated DA_MotorTest

Motor test DA

Dependencies:   Serial_HL mbed

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?

UserRevisionLine numberNew 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 }