// Ein Programm von Florian Reisinger, 1CHEL für den Bulme BERTL 2016.
// Ziel ist es, dass der Bertl einer schwarzen Linie auf weißem Untergrund nachfährt.





#include "mbed.h"

// ************ DEKLARATIONEN **************
DigitalOut Von (P2_13);        
DigitalOut MotorL_EN(P1_19);          
DigitalOut MotorL_FORWARD(P2_15);   
DigitalOut MotorL_REVERSE(P2_14);  
DigitalOut MotorR_EN(P2_19);      
DigitalOut MotorR_FORWARD(P2_20); 
DigitalOut MotorR_REVERSE(P2_21);

// ************ LineSensor **************
DigitalOut LineON (P2_5);   
DigitalOut Incr (P2_2);      
DigitalIn ISO1 (P1_9);          // ganz links
DigitalIn ISO2 (P0_16);         // links
DigitalIn ISO3 (P0_23);         // rechts
DigitalIn ISO4 (P0_15);         // ganz rechts
DigitalIn ISO5 (P1_3);          // mitte

// *********** LEDs ****************
DigitalOut LedD1 (P1_10);       // Fernlicht links
DigitalOut LedD2 (P1_11);       // Blinker vorne links
DigitalOut LedD4 (P1_12);       // Fernlicht rechts
DigitalOut LedD5 (P1_13);       // Blinker vorne rechts
DigitalOut LedD6 (P1_14);       // Blinker hinten links
DigitalOut LedD7 (P1_15);       // Rücklicht links
DigitalOut LedD8 (P1_16);       // Blinker hinten rechts
DigitalOut LedD9 (P1_17);       // Rücklicht rechts
DigitalOut LedD10 (P1_18);      // 4er ganz links
DigitalOut LedD11 (P2_16);      // 4er links
DigitalOut LedD12 (P1_20);      // 4er rechts
DigitalOut LedD13 (P1_21);      // 4er ganz rechts

// *********** Taster ************
DigitalIn TA1 (P1_23);          // vorne mitte 
DigitalIn TA2 (P1_24);          // vorne rechts
DigitalIn TA3 (P1_25);          // vorne links
DigitalIn TA4 (P1_26);          // hinten mitte
DigitalIn TA5 (P1_27);          // hinten links
DigitalIn TA6 (P1_28);          // hinten rechts
DigitalIn TA7 (P1_30);          // vorne ganz rechts
DigitalIn TA8 (P1_31);          // vorne ganz linkis

// *************  Hauptprogramm ************
int main() {                    
    Von=1;
    MotorR_EN=MotorL_EN=1;             
    LineON=1;
    Incr=1;


//***Fernlicht***
LedD1=0;
LedD4=0;
LedD7=0;
LedD9=0;

//***Blinker***
LedD2=1;
LedD5=1;
LedD6=1;
LedD8=1;

//***4er***
LedD10=1;
LedD11=1;
LedD12=1;
LedD13=1;

int a, b=0;



//***********************LINIE****************************** 
  
        while(1)
        {


        if (ISO5 == 1)         // MITTE
           {
                LedD10=1;
                LedD11=1;
                LedD12=1;
                LedD13=1;
                
                LedD2=1;
                LedD5=1;
                LedD6=1;
                LedD8=1;

                MotorR_FORWARD=1;
                MotorL_FORWARD=1;
           }

        else if (ISO3 == 1)         // RECHTS
           {
                LedD10=1;
                LedD11=1;
                LedD12=1;
                LedD13=0;

                LedD2=1;
                LedD5=0;
                LedD6=1;
                LedD8=0;
                
                a++;
                
                if(a&1) // ungerade 
                {
                    MotorR_FORWARD=0;
                    MotorL_FORWARD=1;   
                }
                else // gerade
                {
                    MotorR_FORWARD=1;
                    MotorL_FORWARD=1;
                }
           }
           
        else if (ISO2 == 1)         // LINKS
           {
                LedD10=1;
                LedD11=0;
                LedD12=1;
                LedD13=1;                

                LedD2=0;
                LedD5=1;
                LedD6=0;
                LedD8=1;
                
                b++;
                
                if(b&1) // ungerade 
                {
                    MotorR_FORWARD=1;
                    MotorL_FORWARD=0;   
                }
                else // gerade
                {
                    MotorR_FORWARD=1;
                    MotorL_FORWARD=1;
                }
           }
           
        else if (ISO4 == 1)         // GANZ RECHTS
           {
                LedD10=1;
                LedD11=1;
                LedD12=0;
                LedD13=1;

                LedD2=1;
                LedD5=0;
                LedD6=1;
                LedD8=0;

                MotorR_FORWARD=0;
                MotorL_FORWARD=1;  
           }
           
        else if (ISO1 == 1 )        // GANZ LINKS
           {
                LedD10=0;
                LedD11=1;
                LedD12=1;
                LedD13=1;

                LedD2=0;
                LedD5=1;
                LedD6=0;
                LedD8=1;
                
                MotorR_FORWARD=1;
                MotorL_FORWARD=0;
           } 
             
       
        if (a >= 10 or b >= 10)
            {
                a=0;
                b=0;
            }

        
    }

}
// **************     ENDE     *************