File content as of revision 5:e715d157ced5:

#include <mbed.h>

#include "EncoderCounter.h"
#include "Controller.h"
#include "IRSensor.h"
#include "Fahren.h"
#include "StateMachine.h"

DigitalOut myled(LED1);

DigitalOut led0(PC_8);
DigitalOut led1(PC_6);
DigitalOut led2(PB_12);
DigitalOut led3(PA_7);
DigitalOut led4(PC_0);
DigitalOut led5(PC_9);

AnalogIn LineSensor(PB_1); // Line Sensor
DigitalOut enable(PC_1);
DigitalOut bit0(PH_1);
DigitalOut bit1(PC_2);
DigitalOut bit2(PC_3);

/*IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
IRSensor irSensor5(distance, bit0, bit1, bit2, 5);*/

AnalogIn IrRight(PC_3);                                                         //von main Vincent kopiert
AnalogIn IrLeft (PC_5);
AnalogIn IrFront(PC_2);
float disR = 0;
float disL = 0;
float disF = 0;

float dis2R = 0;
float dis2L = 0;
float dis2F = 0;
int IrR = 0;
int IrL = 0;
int IrF = 0;
int caseDrive = 0;   
double line = 0;
bool finish = 0;       
bool finishLast = 0;
int ende = 0;    
int temp = 0;                                                        //von main Vincent kopiert

IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor); 
//IRSensor bottom (IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor);           //von main Vincent kopiert
StateMachine stateMachine(IrR, IrL, IrF);                                       //von main Vincent kopiert


DigitalOut enableMotorDriver(PB_2);
DigitalIn motorDriverFault(PB_14);
DigitalIn motorDriverWarning(PB_15);

PwmOut pwmLeft(PA_8);
PwmOut pwmRight(PA_9);

EncoderCounter counterLeft(PB_6, PB_7);
EncoderCounter counterRight(PA_6, PC_7);

Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);

Fahren fahren(controller, counterLeft, counterRight);                           //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig

int main()
{

    //controller.setDesiredSpeedLeft(150.0f); // Drehzahl in [rpm]              //DEFAULT VON TOBI
    //controller.setDesiredSpeedRight(-150.0f);                                 //DEFAULT VON TOBI
    enable = 1;
    enableMotorDriver = 1;                                                      // Schaltet den Leistungstreiber ein

    while(1) {

// Test um Drehungen und geradeaus zu testen

        /*fahren.geradeaus();                                                     //Geradeausfahren aufrufen
        wait(1.0f);
        
        fahren.rechts90();
        wait(1.0f);
                
        fahren.rechts180();
        wait(1.0f);
        
        fahren.links90();
        wait(1.0f);*/
                                                                         // IR Sensoren einlesen Programm Vincen      
        float disR = iRSensor.readR(); // Distanz in mm
        float disL = iRSensor.readL();
        float disF = iRSensor.readF();
        dis2R = disR;
        dis2L = disL;
        dis2F = disF;
        int IrR = iRSensor.codeR();     // min Distanz unterschritten = 1
        int IrL = iRSensor.codeL();
        int IrF = iRSensor.codeF();
        /*int caseDrive = stateMachine.drive();*/ //entscheidung welcher Drive Case
        ende = iRSensor.get_ende();


   
  /* printf (" ende = %d\n",ende); 
    printf (" finish = %d\n",finish);
    printf (" finishLast = %d\n",finishLast);
    printf("line = %f\n", line);
        
      /*  printf ("IR Right = %d \n", IrR);
        printf("IR Left = %d\n",IrL);
        printf("IR Front = %d\n",IrF);*/
        
         if((IrR==0) && (IrL==0) && (IrF==1) && (ende == 0)){
        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen
        }   else  if ((IrR==0) && (IrL==1) && (IrF==0) && (ende == 0)){
        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen
        }   else  if ((IrR==0) && (IrL==1) && (IrF==1) && (ende == 0)){
        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen                 
        }   else  if ((IrR==1) && (IrL==0) && (IrF==0) && (ende == 0)){
        caseDrive = 1;                                                          // Folge: geradeaus fahren
        }   else  if ((IrR==1) && (IrL==0) && (IrF==1) && (ende == 0)){
        caseDrive = 3;                                                          // Folge: 270 Grad nach rechts drehen
        }   else  if ((IrR==1) && (IrL==1) && (IrF==0) && (ende == 0)){
        caseDrive = 1;                                                          // Folge: geradeaus fahren
        }   else  if ((IrR==1) && (IrL==1) && (IrF==1) && (ende == 0)){
        caseDrive = 4;                                                          // Folge: 180 Grad nach rechts drehen
        } else if ((ende == 1) && (temp == 0)){
        caseDrive = 5;
        temp = 1;
        } else {caseDrive = 0;
        }
        
        printf("caseDrive = %d\n",caseDrive);
        printf("ende = %d\n",ende);

        wait (0.5);
        

        
        
        if(caseDrive == 1){
            fahren.geradeaus();
        } else if (caseDrive == 2){
            fahren.rechts90();
            fahren.geradeaus();
        } else if (caseDrive == 3){
            fahren.links90();
            fahren.geradeaus();
        } else if (caseDrive == 4){
            fahren.rechts180();
            fahren.geradeaus();
        } else if (caseDrive == 0){
            fahren.stopp();
        } else if(caseDrive == 5){
            fahren.ziel();
            fahren.stopp();
          }  
    }
}