File content as of revision 1:dfa0f59e8d2c:

#include "MMouse18.h"
#include "Serial_HL.h"

SerialBLK pc(USBTX, USBRX);
// SerialBLK pc(p28, p27);
SvProtocol ua0(&pc);

//Funktionen
void CommandHandler();
void CheckButton();
void ForewardUntilXing();
void NinetyDegreesLeft();
void TurnAround();
void CorrectToRight();
void CorrectToLeft();
bool CheckLeftSensor();
bool CheckLeftFrontSensor();
bool CheckRightSensor();
bool CheckRightFrontSensor();
float speed = -0.3;



int SensorLeft = 0;
int SensorFrontLeft = 0;
int SensorRight = 0;
int SensorFrontRight = 0;


Motor mL(p22,p24,p23); Motor mR(p26, p21, p25);//(Enable, Input Rückwärts, Input Vorwärts)#####255 Encoder-tics = 90 Grad Kurve
Encoder encL(p15,p16); //(Motor-Encoder Links A, Motor-Encoder Rechts A)
Encoder encR(p30,p29);//(Motor-Encoder Links B, Motor-Encoder Rechts B)

DistSens sla(p13,p20); // Sensor Links außen (Emitter, Receiver)
DistSens sli(p14,p19); // Sensor Links innen
DistSens sra(p12,p17); // Sensor rechts außen
DistSens sri(p11,p18); // Sensor rechts innen

// DigitalIn sw(p5); geht an p5 nicht

BusOut leds(LED1,LED2,LED3,LED4);

int main(void)
{

    leds=0;
    pc.format(8,SerialBLK::None,1);
    pc.baud(115200);
    mL.SetPow(0); mR.SetPow(0);
    encL.Init(ENC_A,ENC_RISE); encR.Init(ENC_A,ENC_RISE);
    
    ua0.SvMessage("MotTest7"); 
    
    Timer stw;
    stw.start();
    while(1) {
//      CheckButton();
        CommandHandler(); 
        
        
                SensorLeft = sla.Measure();
                SensorFrontLeft = sli.Measure();
                SensorRight = sra.Measure();
                SensorFrontRight = sri.Measure();
        
        
        ForewardUntilXing();    //#############################################################
        
        if( stw.read_ms()>10 ) { 
            stw.reset();
            if( ua0.acqON ) {
                
//              leds =1;
//              wait_ms(3000);
//              leds =2;
//              wait_ms(3000);
//              leds =3;
//              wait_ms(3000);
//              leds =4;
//              wait_ms(3000);
//              leds =5;
//              wait_ms(3000);
//              leds =6;
//              wait_ms(3000);
//              leds =7;
//              wait_ms(3000);
//              leds =8;
//              wait_ms(3000);
//              leds =9;
//              wait_ms(3000);
//              leds =10;
//              wait_ms(3000);
                
//              SensorLeft = sla.Measure();
//              SensorFrontLeft = sli.Measure();
//              SensorRight = sra.Measure();
//              SensorFrontRight = sri.Measure();
/*
                ua0.WriteSvI16(1, sla.Measure());//Sensor links außen in ProcVis anzeigen
                ua0.WriteSvI16(2, sli.Measure());
                ua0.WriteSvI16(3, sra.Measure());
                ua0.WriteSvI16(4, sri.Measure());
*/
                ua0.WriteSvI16(1, SensorLeft);//Sensor links außen in ProcVis anzeigen
                ua0.WriteSvI16(2, SensorFrontLeft);
                ua0.WriteSvI16(3, SensorRight);
                ua0.WriteSvI16(4, SensorFrontRight);
                ua0.WriteSvI16(5, encL.cnt);//Encoder Motor links in ProcVis anzeigen
                ua0.WriteSvI16(6, encR.cnt);

            }
        }
    }
    return 1;
}

void CommandHandler()
{
    uint8_t cmd;
    if( !pc.IsDataAvail() )
        return;
    cmd = ua0.GetCommand();
    if( cmd==2 ) {
        leds=2;
        mL.SetPow(ua0.ReadF()); mR.SetPow(ua0.ReadF());//Motor starten. Eingabe: 2 0,3 0,3
        ua0.SvMessage("cmd: 2, Set Pow");
    }
    if( cmd==3 ) {
        leds=3;
        encL.cnt=encR.cnt=0;//Encoder zurücksetzen
        ua0.SvMessage("cmd: 3, Reset Cnt");
    }
    
    if( cmd==4 ) 
    {
        leds=4;
        ua0.SvMessage("cmd: 4");
    }
/*      sla.useLED=ua0.ReadI16();
        sli.useLED=ua0.ReadI16();
        sra.useLED=ua0.ReadI16();
        sri.useLED=ua0.ReadI16();
        ua0.SvMessage("LED On/Off");
    }*/
}

void CheckButton()
{
    /* if( sw==1 )
        leds=0xF;
    else
        leds=0; */
}


void ForewardUntilXing()
{
//    while (CheckLeftFrontSensor() != 1 && CheckLeftSensor() != 1)
//    {
    
                if (SensorFrontLeft > 1000 || SensorFrontRight > 1000)
                {
                TurnAround();
//              mL.SetPow(0); mR.SetPow(0);
//              wait_ms(10000);
                }
                
        mL.SetPow(speed); mR.SetPow(speed);//geradeaus
        if(SensorLeft > 1500 /*(zu weit Links)*/)
        {
            CorrectToRight();
        }
        if(SensorRight > 1500 /* (zu weit Rechts)*/)
        {
            CorrectToLeft();
        }
                
//    }
}


void NinetyDegreesLeft()
{
    for (int i = 0; i <= 10; i++)
    {
        mL.SetPow(speed); mR.SetPow(-0.1);//Linkskurve 
        wait_ms(100);
    }

}


//bool CheckLeftSensor()
//{
//    if (SensorLeft < 1000)
//    {
//          CorrectToRight();
//      return 1;
//    }
//    else
//    {
//      return 0;//Abstand zur Wand korrekt
//    }
//}



bool CheckLeftFrontSensor()
{
    if (SensorFrontLeft > 0)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}


//bool CheckRightSensor()
//{
//  
//     if (SensorFrontRight < 1000)
//    {
//          CorrectToLeft();
//          return 1;
//    }
//    else
//    {         
//        return 0;//Abstand zur Wand korrekt
//    }
// 
//}


bool CheckRightFrontSensor()
{
    if (SensorRight > 0)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}


void CorrectToLeft()
{
        mL.SetPow(-0.1); mR.SetPow(speed);//nach links 
        wait_ms(30);
        mL.SetPow(speed); mR.SetPow(speed);//wieder geradeaus 
}


void CorrectToRight()
{
        mL.SetPow(speed); mR.SetPow(-0.1);//nach rechts
        wait_ms(30);
        mL.SetPow(speed); mR.SetPow(speed);//wieder geradeaus 
}


void TurnAround()
{
    encL.cnt=encR.cnt=0;//Encoder zurücksetzen
    while(encR.cnt<=25588)
        mL.SetPow(speed); mR.SetPow(0.3);//nach rechts

}