Alexander Welz
/
BERTL16_LineDrive
BERTL16 LineDrive
main.cpp
- Committer:
- WelzAlex
- Date:
- 2019-01-24
- Revision:
- 0:68b692bb433c
File content as of revision 0:68b692bb433c:
#include "mbed.h" #include "dracuDrive.h" #include "Serial_HL.h" SerialBLK pc(USBTX, USBRX); SvProtocol ua0(&pc); void CommandHandler(); BusOut boardPow(P2_13, P2_5, P2_2); // B16/17 AnalogInHL2 ls1(P1_9), ls2(P0_16), ls3(P1_3), ls4(P0_23), ls5(P0_12); DigitalIn dummy(P0_15); dracuMotor mL(P1_19,P2_15,P2_14); dracuMotor mR(P2_19,P2_20,P2_21); int GetPin(PinName aName) { return (aName >> PIN_SHIFT) & 0x0000003F; } int GetPort(PinName aName) { return (aName >> PORT_SHIFT) & 0x0000003F; } int main(void) { boardPow =3; float abweichung = 0; pc.format(8,SerialBLK::None,1); pc.baud(115200); // 115200 ua0.SvMessage("LineSensor!"); // Meldung zum PC senden float i=0.3; ua0.acqON = 1; while(1) { CommandHandler(); mL.Speed(i); mR.Speed(i); abweichung = (ls1.Read()*(-8) +ls2.Read()*(-1)+ ls1.Read() *(0) +ls4.Read()*(1) +ls5.Read()*(10))/10000.0; if(abweichung > 0.9) { mR.Speed(0); mL.Speed(0); wait_ms(250); mR.Speed(-i); mL.Speed(i); wait_ms(250); } else if(abweichung < -0.9) { mR.Speed(0); mL.Speed(0); wait_ms(250); mR.Speed(i); mL.Speed(-i); wait_ms(250); } else { mR.Speed(i - abweichung+0); mL.Speed(i + (abweichung+0)); } if( ua0.acqON ) { // nur wenn vom PC aus das Senden eingeschaltet wurde // wird auch etwas gesendet ua0.WriteSvI16(1, ls1.Read()); // Wert von ls1 auf Kanal 1 an SvVis ua0.WriteSvI16(2, ls2.Read()); ua0.WriteSvI16(3, ls3.Read()); ua0.WriteSvI16(4, ls4.Read()); ua0.WriteSvI16(5, ls5.Read()); ua0.WriteSV(6, abweichung); } } } void CommandHandler() { uint8_t cmd; int16_t idata1, idata2; // Fragen ob überhaupt etwas im RX-Reg steht if( !pc.IsDataAvail() ) return; // wenn etwas im RX-Reg steht // Kommando lesen cmd = ua0.GetCommand(); }