File content as of revision 1:2e48f1a47d07:

#include "mbed.h"
#include "Serial_HL.h"
#include "Bertl14.h"
#include "BertlObjects.h"
 
SerialBLK pc(USBTX, USBRX);
SvProtocol ua0(&pc);

void CommandHandler();
void DriveUntilObstacle();
void Turn180();
void blinkvorne();
void blinkhinten();
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)
  { 
    t1.start();
    pex.ReadButtons(); 
    if( pex.IsButton(BTN_FLL) )
        {
        DriveUntilObstacle(); 
        }

    
       
    
    
    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 DriveUntilObstacle()
{
   
     //pex.SetLeds(LED_FL1|LED_FL2); 
     mL.SetPow(0.5);
     mR.SetPow(-0.5);  
       
     while(1)
     {
        us.StartMeas();
        wait_ms(10);
        if(led==0)
        blinkvorne();
        else
        blinkhinten();
        if ((us.distCM) < 5)
        {
         
         Turn180();
        wait_ms(10);
        } 
         
         
     }
     


}


void Turn180()

{
         if(led==0)
         led=1;
         else
         led=0;
         mL.SetPow(0.5);
         mR.SetPow(0.5);
         mL.encCnt=0;
         mR.encCnt=0;
        
         while(1)
    {
        if (mL.encCnt>45)
        mL.SetPow(0);
        if (mR.encCnt>45)
        mR.SetPow(0);
        
        if (mR.encCnt>48 && mL.encCnt>48 )
        {
            
            DriveUntilObstacle();
        }
    }          
}

void blinkvorne()
{
    if(t1.read_ms() > 200)
    {   pex.ClearLeds();
        pex.SetLeds(LED_ALL_FRONT);
        
        }
        
         if(t1.read_ms() > 200)
    {
       pex.ClearLeds();
        t1.reset();
        }
        
    }
    
void blinkhinten()
{
     if(t1.read_ms() > 200)
    {   pex.ClearLeds();
        pex.SetLeds(LED_ALL_BACK);
        
        }
        
         if(t1.read_ms() > 200)
    {
       pex.ClearLeds();
        t1.reset();
        }
    }