File content as of revision 0:2271f1e56f17:

#include "mbed.h"
#include "Serial_HL.h"
#include "Bertl14.h"
#include "BertlObjects.h"
 
SerialBLK pc(USBTX, USBRX);
SvProtocol ua0(&pc);
AnalogInHL ls1(p18), ls2(p16), ls3(p19), ls4(p17);

void CommandHandler();
void followmaster();
void blinkvorne();
void blinkhinten();
void messen();
void rechts();
void links();
void gerade();

Timer t1;
unsigned int led=1;


int main(void)
{
  InitBertl();
    pex.useISR=0; leds=9;
    pex.ClearLeds();
  pc.format(8,SerialBLK::None,1); pc.baud(500000); // 115200
  
  ua0.SvMessage("MotorTest1"); // Meldung zum PC senden
  
  Timer stw; stw.start();
  while(1)
  { 
    
        
        messen(); 

     CommandHandler();
    if( (stw.read_ms()>10) ) // 100Hz
    { // dieser Teil wird mit 100Hz aufgerufen
      stw.reset();
      if( ua0.acqON ) {
        // nur wenn vom PC aus das Senden eingeschaltet wurde
        // wird auch etwas gesendet
        ua0.WriteSvI16(1, mL.encCnt);
        ua0.WriteSvI16(2, mR.encCnt);
        ua0.WriteSvI16(3, us.distCM*100.0);
        us.StartMeas(); // for next cycle
      }
    }
  }
  return 1;
}

void CommandHandler()
{
  uint8_t cmd;
  // Fragen ob überhaupt etwas im RX-Reg steht
  if( !pc.IsDataAvail() )
    return;
  // wenn etwas im RX-Reg steht
    // Kommando lesen
    cmd = ua0.GetCommand();
  
  if( cmd==2 ) // Encoder auf 0 setzen
    {
    mL.encCnt=0; mR.encCnt=0;
    ua0.SvMessage("Clear Encoder");
  }
  if( cmd==3 ) // bei beiden Motoren Gas geben
  {
    mL.SetPow(ua0.ReadF());
    mR.SetPow(ua0.ReadF());
    ua0.SvMessage("Set Power");
  }
}






void messen()
{
ls1.Read();
ls2.Read();
ls3.Read();
ls4.Read();

if (ls1.Read() > 800)
rechts();

else if (ls4.Read()> 800)
links();

else
gerade();
}


void links()
{
    mL.SetPow(0.3);
    mR.SetPow(0.0); 
}
void rechts()
{
    mL.SetPow(0.0);
    mR.SetPow(0.3); 
}
void gerade()
{
    mL.SetPow(0.3);
    mR.SetPow(0.3); 
}