main.cpp

00001 /*Roboshark V7
00002 main.cpp
00003 Erstellt: J. Blunschi
00004 geändert: V.Ahlers
00005 V.5.18
00006 main zu Robishark V4
00007 */
00008 
00009 
00010 #include <mbed.h>
00011 
00012 #include "EncoderCounter.h"
00013 #include "Controller.h"
00014 #include "IRSensor.h"
00015 #include "Fahren.h"
00016 
00017 DigitalOut myled(LED1);
00018 
00019 DigitalOut led0(PC_8);
00020 DigitalOut led1(PC_6);
00021 DigitalOut led2(PB_12);
00022 DigitalOut led3(PA_7);
00023 DigitalOut led4(PC_0);
00024 DigitalOut led5(PC_9);
00025 
00026 AnalogIn LineSensor(PB_1); // Line Sensor
00027 DigitalOut enable(PC_1);
00028 DigitalOut bit0(PH_1);
00029 DigitalOut bit1(PC_2);
00030 DigitalOut bit2(PC_3);
00031 
00032 AnalogIn IrRight(PC_3);                                                         //von main Vincent kopiert
00033 AnalogIn IrLeft (PC_5);
00034 AnalogIn IrFront(PC_2);
00035 float disR = 0;
00036 float disL = 0;
00037 float disF = 0;
00038 
00039 float dis2R = 0;
00040 float dis2L = 0;
00041 float dis2F = 0;
00042 int IrR = 0;
00043 int IrL = 0;
00044 int IrF = 0;
00045 int caseDrive = 0;   
00046 double line = 0;
00047 bool finish = 0;       
00048 bool finishLast = 0;
00049 int ende = 0;    
00050 int temp = 0;   
00051 float SpeedR = 0;
00052 float SpeedL = 0;
00053 int reglerEin = 0;
00054                                                      //von main Vincent kopiert
00055 
00056 DigitalOut enableMotorDriver(PB_2);
00057 DigitalIn motorDriverFault(PB_14);
00058 DigitalIn motorDriverWarning(PB_15);
00059 
00060 PwmOut pwmLeft(PA_8);
00061 PwmOut pwmRight(PA_9);
00062 
00063 EncoderCounter counterLeft(PB_6, PB_7);
00064 EncoderCounter counterRight(PA_6, PC_7);
00065 
00066 IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor);          //von main Vincent kopiert
00067 
00068 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
00069 
00070 Fahren fahren(controller, counterLeft, counterRight, iRSensor);                           //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
00071 
00072 int main()
00073 {
00074 
00075     //controller.setDesiredSpeedLeft(150.0f); // Drehzahl in [rpm]              //DEFAULT VON TOBI
00076     //controller.setDesiredSpeedRight(-150.0f);                                 //DEFAULT VON TOBI
00077     enable = 1;
00078     enableMotorDriver = 1;                                                      // Schaltet den Leistungstreiber ein
00079     while(1) {
00080 
00081                                                                          // IR Sensoren einlesen
00082         float disR = iRSensor.readR(); // Distanz in mm
00083         float disL = iRSensor.readL();
00084         float disF = iRSensor.readF();
00085         dis2R = disR;
00086         dis2L = disL;
00087         dis2F = disF;
00088         int IrR = iRSensor.codeR();     // min Distanz unterschritten = 1
00089         int IrL = iRSensor.codeL();
00090         int IrF = iRSensor.codeF();
00091         ende = iRSensor.get_ende();     // Line erkennt = 1
00092 
00093 
00094    
00095    /* printf (" ende = %d\n",ende); 
00096     printf (" finish = %d\n",finish);
00097     printf (" finishLast = %d\n",finishLast);
00098     printf("line = %f\n", line);
00099         
00100         printf ("IR Right = %d \n", IrR);
00101         printf("IR Left = %d\n",IrL);
00102         printf("IR Front = %d\n",IrF);*/
00103         
00104          if((IrR==0) && (IrL==0) && (IrF==1) && (ende == 0)){
00105         caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen
00106         }   else  if ((IrR==0) && (IrL==1) && (IrF==0) && (ende == 0)){
00107         caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen
00108         }   else  if ((IrR==0) && (IrL==1) && (IrF==1) && (ende == 0)){
00109         caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen                 
00110         }   else  if ((IrR==1) && (IrL==0) && (IrF==0) && (ende == 0)){
00111         caseDrive = 1;                                                          // Folge: geradeaus fahren
00112         }   else  if ((IrR==1) && (IrL==0) && (IrF==1) && (ende == 0)){
00113         caseDrive = 3;                                                          // Folge: 90 Grad nach Links drehen
00114         }   else  if ((IrR==1) && (IrL==1) && (IrF==0) && (ende == 0)){
00115         caseDrive = 1;                                                          // Folge: geradeaus fahren
00116         }   else  if ((IrR==1) && (IrL==1) && (IrF==1) && (ende == 0)){
00117         caseDrive = 4;                                                          // Folge: 180 Grad nach rechts drehen
00118         } else if ((ende == 1) && (temp == 0)){
00119         caseDrive = 5;                                                          // Folge: Ziel erreicht
00120         temp = 1;
00121         } else {caseDrive = 0;                                                  // Folge; Stop
00122         }
00123         
00124       //  printf("caseDrive = %d\n",caseDrive);
00125         //printf("ende = %d\n",ende);
00126 
00127         wait (0.1);
00128         
00129 
00130         
00131         
00132         if(caseDrive == 1){                 // Aufrufen der verschiedenen fahr funktionen               
00133             fahren.geradeausG();
00134         } else if (caseDrive == 2){
00135             fahren.rechts90();
00136             fahren.geradeausG();
00137         } else if (caseDrive == 3){
00138             fahren.links90();
00139             fahren.geradeausG();
00140         } else if (caseDrive == 4){
00141             fahren.rechts180();
00142             fahren.geradeausG();
00143         } else if (caseDrive == 0){
00144             fahren.stopp();
00145         } else if(caseDrive == 5){
00146             fahren.ziel();
00147             fahren.stopp();
00148           }  
00149     }
00150 }