Alexander Welz / Mbed 2 deprecated BERTL16_LineDrive

BERTL16 LineDrive

Dependencies:   Serial_HL mbed

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?

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