Alexander Welz
/
BERTL16_LineDrive
BERTL16 LineDrive
main.cpp@0:68b692bb433c, 2019-01-24 (annotated)
- Committer:
- WelzAlex
- Date:
- Thu Jan 24 21:06:58 2019 +0000
- Revision:
- 0:68b692bb433c
BERTL16Drive
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WelzAlex | 0:68b692bb433c | 1 | #include "mbed.h" |
WelzAlex | 0:68b692bb433c | 2 | #include "dracuDrive.h" |
WelzAlex | 0:68b692bb433c | 3 | #include "Serial_HL.h" |
WelzAlex | 0:68b692bb433c | 4 | |
WelzAlex | 0:68b692bb433c | 5 | SerialBLK pc(USBTX, USBRX); |
WelzAlex | 0:68b692bb433c | 6 | SvProtocol ua0(&pc); |
WelzAlex | 0:68b692bb433c | 7 | void CommandHandler(); |
WelzAlex | 0:68b692bb433c | 8 | BusOut boardPow(P2_13, P2_5, P2_2); // B16/17 |
WelzAlex | 0:68b692bb433c | 9 | AnalogInHL2 ls1(P1_9), ls2(P0_16), ls3(P1_3), ls4(P0_23), ls5(P0_12); |
WelzAlex | 0:68b692bb433c | 10 | DigitalIn dummy(P0_15); |
WelzAlex | 0:68b692bb433c | 11 | dracuMotor mL(P1_19,P2_15,P2_14); |
WelzAlex | 0:68b692bb433c | 12 | dracuMotor mR(P2_19,P2_20,P2_21); |
WelzAlex | 0:68b692bb433c | 13 | |
WelzAlex | 0:68b692bb433c | 14 | int GetPin(PinName aName) |
WelzAlex | 0:68b692bb433c | 15 | { |
WelzAlex | 0:68b692bb433c | 16 | return (aName >> PIN_SHIFT) & 0x0000003F; |
WelzAlex | 0:68b692bb433c | 17 | } |
WelzAlex | 0:68b692bb433c | 18 | |
WelzAlex | 0:68b692bb433c | 19 | int GetPort(PinName aName) |
WelzAlex | 0:68b692bb433c | 20 | { |
WelzAlex | 0:68b692bb433c | 21 | return (aName >> PORT_SHIFT) & 0x0000003F; |
WelzAlex | 0:68b692bb433c | 22 | } |
WelzAlex | 0:68b692bb433c | 23 | |
WelzAlex | 0:68b692bb433c | 24 | int main(void) |
WelzAlex | 0:68b692bb433c | 25 | { |
WelzAlex | 0:68b692bb433c | 26 | boardPow =3; |
WelzAlex | 0:68b692bb433c | 27 | float abweichung = 0; |
WelzAlex | 0:68b692bb433c | 28 | |
WelzAlex | 0:68b692bb433c | 29 | pc.format(8,SerialBLK::None,1); pc.baud(115200); // 115200 |
WelzAlex | 0:68b692bb433c | 30 | ua0.SvMessage("LineSensor!"); // Meldung zum PC senden |
WelzAlex | 0:68b692bb433c | 31 | |
WelzAlex | 0:68b692bb433c | 32 | float i=0.3; |
WelzAlex | 0:68b692bb433c | 33 | ua0.acqON = 1; |
WelzAlex | 0:68b692bb433c | 34 | while(1) |
WelzAlex | 0:68b692bb433c | 35 | { |
WelzAlex | 0:68b692bb433c | 36 | CommandHandler(); |
WelzAlex | 0:68b692bb433c | 37 | mL.Speed(i); |
WelzAlex | 0:68b692bb433c | 38 | mR.Speed(i); |
WelzAlex | 0:68b692bb433c | 39 | |
WelzAlex | 0:68b692bb433c | 40 | |
WelzAlex | 0:68b692bb433c | 41 | abweichung = (ls1.Read()*(-8) +ls2.Read()*(-1)+ ls1.Read() *(0) +ls4.Read()*(1) +ls5.Read()*(10))/10000.0; |
WelzAlex | 0:68b692bb433c | 42 | |
WelzAlex | 0:68b692bb433c | 43 | if(abweichung > 0.9) |
WelzAlex | 0:68b692bb433c | 44 | { |
WelzAlex | 0:68b692bb433c | 45 | mR.Speed(0); |
WelzAlex | 0:68b692bb433c | 46 | mL.Speed(0); |
WelzAlex | 0:68b692bb433c | 47 | wait_ms(250); |
WelzAlex | 0:68b692bb433c | 48 | mR.Speed(-i); |
WelzAlex | 0:68b692bb433c | 49 | mL.Speed(i); |
WelzAlex | 0:68b692bb433c | 50 | wait_ms(250); |
WelzAlex | 0:68b692bb433c | 51 | } |
WelzAlex | 0:68b692bb433c | 52 | else if(abweichung < -0.9) |
WelzAlex | 0:68b692bb433c | 53 | { |
WelzAlex | 0:68b692bb433c | 54 | mR.Speed(0); |
WelzAlex | 0:68b692bb433c | 55 | mL.Speed(0); |
WelzAlex | 0:68b692bb433c | 56 | wait_ms(250); |
WelzAlex | 0:68b692bb433c | 57 | mR.Speed(i); |
WelzAlex | 0:68b692bb433c | 58 | mL.Speed(-i); |
WelzAlex | 0:68b692bb433c | 59 | wait_ms(250); |
WelzAlex | 0:68b692bb433c | 60 | } |
WelzAlex | 0:68b692bb433c | 61 | else |
WelzAlex | 0:68b692bb433c | 62 | { |
WelzAlex | 0:68b692bb433c | 63 | mR.Speed(i - abweichung+0); |
WelzAlex | 0:68b692bb433c | 64 | mL.Speed(i + (abweichung+0)); |
WelzAlex | 0:68b692bb433c | 65 | } |
WelzAlex | 0:68b692bb433c | 66 | |
WelzAlex | 0:68b692bb433c | 67 | |
WelzAlex | 0:68b692bb433c | 68 | |
WelzAlex | 0:68b692bb433c | 69 | if( ua0.acqON ) { |
WelzAlex | 0:68b692bb433c | 70 | // nur wenn vom PC aus das Senden eingeschaltet wurde |
WelzAlex | 0:68b692bb433c | 71 | // wird auch etwas gesendet |
WelzAlex | 0:68b692bb433c | 72 | ua0.WriteSvI16(1, ls1.Read()); // Wert von ls1 auf Kanal 1 an SvVis |
WelzAlex | 0:68b692bb433c | 73 | ua0.WriteSvI16(2, ls2.Read()); |
WelzAlex | 0:68b692bb433c | 74 | ua0.WriteSvI16(3, ls3.Read()); |
WelzAlex | 0:68b692bb433c | 75 | ua0.WriteSvI16(4, ls4.Read()); |
WelzAlex | 0:68b692bb433c | 76 | ua0.WriteSvI16(5, ls5.Read()); |
WelzAlex | 0:68b692bb433c | 77 | ua0.WriteSV(6, abweichung); |
WelzAlex | 0:68b692bb433c | 78 | } |
WelzAlex | 0:68b692bb433c | 79 | } |
WelzAlex | 0:68b692bb433c | 80 | } |
WelzAlex | 0:68b692bb433c | 81 | |
WelzAlex | 0:68b692bb433c | 82 | |
WelzAlex | 0:68b692bb433c | 83 | void CommandHandler() |
WelzAlex | 0:68b692bb433c | 84 | { |
WelzAlex | 0:68b692bb433c | 85 | uint8_t cmd; |
WelzAlex | 0:68b692bb433c | 86 | int16_t idata1, idata2; |
WelzAlex | 0:68b692bb433c | 87 | // Fragen ob überhaupt etwas im RX-Reg steht |
WelzAlex | 0:68b692bb433c | 88 | if( !pc.IsDataAvail() ) |
WelzAlex | 0:68b692bb433c | 89 | return; |
WelzAlex | 0:68b692bb433c | 90 | // wenn etwas im RX-Reg steht |
WelzAlex | 0:68b692bb433c | 91 | // Kommando lesen |
WelzAlex | 0:68b692bb433c | 92 | cmd = ua0.GetCommand(); |
WelzAlex | 0:68b692bb433c | 93 | } |