File content as of revision 6:7bbcdd07bc2d:

/*Roboshark V4
main.cpp
Erstellt: J. Blunschi
geändert: V.Ahlers
V.5.18
main zu Robishark V4
*/


#include <mbed.h>

#include "EncoderCounter.h"
#include "Controller.h"
#include "IRSensor.h"
#include "Fahren.h"
#include "Regler.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);

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;   
float SpeedR = 0;
float SpeedL = 0;
                                                     //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);

IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor);          //von main Vincent kopiert

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

Regler regler(IrRight, IrLeft);

Fahren fahren(controller, counterLeft, counterRight, regler);                           //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
        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();
        ende = iRSensor.get_ende();     // Line erkennt = 1


   
  /* 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: 90 Grad nach Links 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;                                                          // Folge: Ziel erreicht
        temp = 1;
        } else {caseDrive = 0;                                                  // Folge; Stop
        }
        
        //printf("caseDrive = %d\n",caseDrive);
        //printf("ende = %d\n",ende);

        wait (0.2);
        

        
        
        if(caseDrive == 1){                 // Aufrufen der verschiedenen fahr funktionen
            fahren.geradeausG();
        } else if (caseDrive == 2){
            fahren.rechts90();
            fahren.geradeausG();
        } else if (caseDrive == 3){
            fahren.links90();
            fahren.geradeausG();
        } else if (caseDrive == 4){
            fahren.rechts180();
            fahren.geradeausG();
        } else if (caseDrive == 0){
            fahren.stopp();
        } else if(caseDrive == 5){
            fahren.ziel();
            fahren.stopp();
          }  
    }
}