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();
}