BertlTemplate2_Linienfahren

Dependencies:   BertlLib mbed

BertlTemplate_Linienfahren.cpp

Committer:
martwerl
Date:
2018-11-15
Revision:
5:b1a4eed391a6

File content as of revision 5:b1a4eed391a6:


#include "mbed.h"
#include "Serial_HL.h"
#include "Bertl14.h"
#include "BertlObjects.h"

//              main=2^0  LS    ENC 2^2
BusOut boardPow(p30,      P1_6, P1_7);

// ls5 nur beim Betrl15
// AnalogInHL ls1(p18), ls2(p16), ls3(p19), ls4(p17); // B14

AnalogInHL ls1(p18), ls2(p16), ls3(p20), ls4(p19), ls5(p17); // B15

int main(void)
{
    boardPow=3; wait_ms(10);
    InitBertl();
    pex.useISR=0; leds=9;
    pex.ClearLeds();
    
    bool rechts = 0;//flag, welche kurve zuletzt dran war
    bool links = 0;
     
    while(1)
    {
        wait_ms(10);
        
       if(ls1.Read()>600)
        {
                    mL.SetPow(0.3); mR.SetPow(0.1);//rechtskrve 
                    rechts = 1; 
        }
        else if(ls4.Read()>600)
        {
                    mL.SetPow(0.1); mR.SetPow(0.3);//linkskurve  
                    links = 1; 
        }
        else if(ls1.Read()<200 && ls4.Read()<200 && ls2.Read()>200 && ls3.Read()>200)
        {
                    mL.SetPow(0.3); mR.SetPow(0.3);//geradeaus  
                    rechts = 0;
                    links = 0; 
        }        
        else if(ls1.Read()<200 && ls4.Read()<200 && ls2.Read()<200 && ls3.Read()<200  && ls5.Read()<200)//wenn alle unter 200, je nachdem was die letzte kurve war --> andere richtung
        {
                    if(links == 1)
                    {
                        mL.SetPow(0.1); mR.SetPow(0.3);//linkskurve  
                        rechts = 0;//zurücksetzen
                    }
                    else if (rechts == 1)
                    {
                        mL.SetPow(0.3); mR.SetPow(0.1);//rechtskurve  
                        links = 0;
                    }
                    else
                    {
                        mL.SetPow(0.3); mR.SetPow(0.3);//geradeaus 
                    }   
        }       
    } 
    return 1;
}