Diff: main.cpp

Revision:

6:7bbcdd07bc2d

Parent:

4:767fd282dd9c

Child:

7:862d80e0ea2d

--- a/main.cpp	Thu Apr 26 05:58:07 2018 +0000
+++ b/main.cpp	Thu May 03 19:36:16 2018 +0000
@@ -1,3 +1,11 @@
+/*Roboshark V4
+main.cpp
+Erstellt: J. Blunschi
+geändert: V.Ahlers
+V.5.18
+main zu Robishark V4
+*/
+
 
 #include <mbed.h>
 
@@ -5,7 +13,7 @@
 #include "Controller.h"
 #include "IRSensor.h"
 #include "Fahren.h"
-#include "StateMachine.h"
+#include "Regler.h"
 
 DigitalOut myled(LED1);
 
@@ -22,13 +30,6 @@
 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);
@@ -47,12 +48,10 @@
 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
-
+int temp = 0;   
+float SpeedR = 0;
+float SpeedL = 0;
+                                                     //von main Vincent kopiert
 
 DigitalOut enableMotorDriver(PB_2);
 DigitalIn motorDriverFault(PB_14);
@@ -64,9 +63,13 @@
 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);
 
-Fahren fahren(controller, counterLeft, counterRight);                           //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
+Regler regler(IrRight, IrLeft);
+
+Fahren fahren(controller, counterLeft, counterRight, regler);                           //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
 
 int main()
 {
@@ -75,7 +78,6 @@
     //controller.setDesiredSpeedRight(-150.0f);                                 //DEFAULT VON TOBI
     enable = 1;
     enableMotorDriver = 1;                                                      // Schaltet den Leistungstreiber ein
-
     while(1) {
 
 // Test um Drehungen und geradeaus zu testen
@@ -91,7 +93,7 @@
         
         fahren.links90();
         wait(1.0f);*/
-                                                                         // IR Sensoren einlesen Programm Vincen      
+                                                                         // IR Sensoren einlesen
         float disR = iRSensor.readR(); // Distanz in mm
         float disL = iRSensor.readL();
         float disF = iRSensor.readF();
@@ -101,8 +103,7 @@
         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();
+        ende = iRSensor.get_ende();     // Line erkennt = 1
 
 
    
@@ -111,7 +112,7 @@
     printf (" finishLast = %d\n",finishLast);
     printf("line = %f\n", line);
         
-      /*  printf ("IR Right = %d \n", IrR);
+        printf ("IR Right = %d \n", IrR);
         printf("IR Left = %d\n",IrL);
         printf("IR Front = %d\n",IrF);*/
         
@@ -124,36 +125,36 @@
         }   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
+        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;
+        caseDrive = 5;                                                          // Folge: Ziel erreicht
         temp = 1;
-        } else {caseDrive = 0;
+        } else {caseDrive = 0;                                                  // Folge; Stop
         }
         
-        printf("caseDrive = %d\n",caseDrive);
-        printf("ende = %d\n",ende);
+        //printf("caseDrive = %d\n",caseDrive);
+        //printf("ende = %d\n",ende);
 
-        wait (0.5);
+        wait (0.2);
         
 
         
         
-        if(caseDrive == 1){
-            fahren.geradeaus();
+        if(caseDrive == 1){                 // Aufrufen der verschiedenen fahr funktionen
+            fahren.geradeausG();
         } else if (caseDrive == 2){
             fahren.rechts90();
-            fahren.geradeaus();
+            fahren.geradeausG();
         } else if (caseDrive == 3){
             fahren.links90();
-            fahren.geradeaus();
+            fahren.geradeausG();
         } else if (caseDrive == 4){
             fahren.rechts180();
-            fahren.geradeaus();
+            fahren.geradeausG();
         } else if (caseDrive == 0){
             fahren.stopp();
         } else if(caseDrive == 5){