Diff: main.cpp

Revision:

1:d9e840c48b1e

Parent:

0:a9fe4ef404bf

Child:

2:592f01278db4

--- a/main.cpp	Wed Mar 07 14:06:19 2018 +0000
+++ b/main.cpp	Sat Mar 31 16:45:57 2018 +0000
@@ -1,136 +1,178 @@
 #include "mbed.h"
- 
+
 #include "IRSensor.h"
 #include "EncoderCounter.h"
 #include "LowpassFilter.h"
 #include "Controller.h"
+#include "KontrastSensor.h"
+#include "Drive.h"
+#include "CheckWalls.h"
+#include "Turn.h"
+#include "Parallel.h"
 
-DigitalIn button(USER_BUTTON);
-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);
+
+//Initialisierung LEDs Blinker/Surri/Button
+DigitalIn button(USER_BUTTON);  //Moduswählknopf
+DigitalOut myled(LED1);     //Heartbeat (evt auch Anzeige für Modus, vor start
+DigitalOut blinker0(PA_4);  //Blinker links
+DigitalOut blinker1(PC_1);  //Blinker rechts
+DigitalOut surri(PC_0);     //
+
+//Initialisierung LEDs Nightrider
+PwmOut led0(PB_0);  //von links nach rechts
+PwmOut led1(PB_8);
+PwmOut led2(PB_3);
+PwmOut led3(PB_5);
+PwmOut led4(PB_4);
+PwmOut led5(PB_10);
+PwmOut led6(PB_9);
+
+
 
+//Initialisierung IR-Sensoren
+AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte
+AnalogIn distance1(PC_3);
+AnalogIn distance2(PC_5);
+AnalogIn distance3(PB_1);
+IRSensor irSensor0(distance0);    //rechts
+IRSensor irSensor1(distance1);    //vorne
+IRSensor irSensor2(distance2);    //links-vorne
+IRSensor irSensor3(distance3);    //links-hinten
+
+
+//AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte
+//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);
+
+
+//Initialisierung Kontrastsensor
+AnalogIn kontrast(PC_0);
+
+
+
+//Initialisierung Motor und Encoder
 DigitalOut enableMotorDriver(PB_2);
-DigitalIn motorDriverFault(PB_14);
-DigitalIn motorDriverWarning(PB_15);
-
-PwmOut pwmLeft(PA_8);
-PwmOut pwmRight(PA_9);
+PwmOut pwmLeft(PA_8);   //Motor links
+PwmOut pwmRight(PA_9);  //Motor rechts
+EncoderCounter counterLeft(PB_6, PB_7); //Encoder für Motor links
+EncoderCounter counterRight(PA_6, PC_7);//Encoder für Motor rechts
 
-EncoderCounter controllerLeft(PB_6, PB_7);
-EncoderCounter controllerRight(PA_6, PC_7);
-
-Controller controller(pwmLeft, pwmRight, controllerLeft, controllerRight);
- 
- 
-//AnalogIn analog_value(A0);
- 
 DigitalOut enable(PC_1);
 
+Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
 
-AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte
-DigitalOut bit0(PH_1);
-DigitalOut bit1(PC_2);
-DigitalOut bit2(PC_3);
+
+//Turn turn(); //GEHT NICHT!!!
+int state = 0;
+int mode = 1;
+
+int main()
+{
+
 
 
 
-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);
+    enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
+    enable = 1;
 
-int main() {
+    while(1) { // Wiederholungsschleife
 
-    
-    enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
-
-    enable = 1;
+        //Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...
     
-    while(1) {
-        
-        
-        
-        float distance0 = irSensor0.read();
-        float distance1 = irSensor1.read();
-        float distance2 = irSensor2.read();
-        float distance3 = irSensor3.read();
-        float distance4 = irSensor4.read();
-        float distance5 = irSensor5.read();
-        
-        
-        if ( distance0 < 0.1f){
-        led0 = 1;
-        controller.setDesiredSpeedLeft(0.0f); //Drehzahl in (rpm)
-        controller.setDesiredSpeedRight(0.0f); //Drehzahl in (rpm)
-        } 
-        else{
-        led0 = 0;
-        controller.setDesiredSpeedLeft(50.0f); 
-        controller.setDesiredSpeedRight(-50.0f); 
+        printf("WARTE AUF START....");
+
+        while (button) {
+            wait(1.0f); //Die Ruhe vor dem Sturm...
         }
-        
-        if ( distance1 < 0.1f){
-        led1 = 1;
-        controller.setDesiredSpeedLeft(-50.0f);
-     //   controller.setDesiredSpeedRight(0.0f);
-        } 
-        else{
-        led1 = 0;
-    //    controller.setDesiredSpeedLeft(0.0f);
-     //   controller.setDesiredSpeedRight(0.0f); 
-        }        
-        if ( distance2 < 0.1f){
-        led2 = 1;
-        //pwmLeft = 0.6; 
-        //pwmRight = 0.4;
-        } 
-        else{
-        led2 = 0;
-        //pwmLeft = 0.5; 
-        //pwmRight = 0.5;
-        }        
-        if ( distance3 < 0.1f){
-        led3 = 1;
-        //pwmLeft = 0.6; 
-        //pwmRight = 0.4;
-        } 
-        else{
-        led3 = 0;
-        //pwmLeft = 0.5; 
-        //pwmRight = 0.5;
-        }        
-        if ( distance4 < 0.1f){
-        led4 = 1;
-        //pwmLeft = 0.6; 
-        //pwmRight = 0.4;
-        } 
-        else{
-        led4 = 0;
-        //pwmLeft = 0.5; 
-        //pwmRight = 0.5;
-        }     
-                
-        if ( distance5 < 0.1f){
-        led5 = 1;
-        controller.setDesiredSpeedRight(50.0f);
-        //pwmRight = 0.4;
-        } 
-        else{
-        led5 = 0;
-        //pwmLeft = 0.5; 
-        //pwmRight = 0.5;
-        }        
-                
-        myled =! myled; // LED is ON
-        wait(0.1f); // 100 ms
+
+
+
+        while(mode==1) {
 
-        
+            //Ermittlung der Abstaende rund ums Fahrzeug
+            float distanceRight = irSensor0.read();
+            float distanceFront = irSensor1.read();
+            float distanceLeftFront = irSensor2.read();
+            float distanceLeftBack = irSensor3.read();
+             
+            int wallRight;
+            int wallFront;
+            int wallLeft; 
+            int blackLine;
+            
+            CheckWalls checkWalls(distanceRight, distanceFront, distanceLeftFront, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
+            //Decide??
+            Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft);   //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne
+            KontrastSensor kontrastSensor(kontrast, blackLine); 
+            Drive drive(kontrastSensor, counterLeft, counterRight, controller, distanceLeftFront, distanceLeftBack, distanceFront); //20cm nach vorne...
+            Parallel parallel(distanceLeftFront, distanceLeftBack);                                                 
+            
+            switch(state) { //Ausrichten
+                case 0: {
+                    
+                     
+                    
+                    state = 1;
+                    break;
+                }
+                case 1: {//Fahrzyklus
+                    
+                    //checkWalls.check(); //prueft wo Waende vorhanden sind
+                    
+                    //wallRight = 1;
+                    //wallFront = 1;
+                    //wallLeft = 1;
+                    
+                    //turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
+                    
+                    controller.setDesiredSpeedRight(0.5f);
+                    controller.setDesiredSpeedLeft(-0.5f);
+                    //drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
+
+                    
+                    blackLine = 1;
+                    if(blackLine == 1){  //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
+                    
+                        state = 2;
+                    }
+                    break;
+                }
+                case 2: { //Ziel erreicht
+                    
+                    
+                    //"Stop"
+                    //mode=2
+                    break;
+                }
+                default: { //
+                    state = 0;
+                    break;
+                }
+            }
+
+            printf("iSumLeft: %f, iSumRight: %f, counterDiff; %d , SpeedRight: %f, SpeedLeft: %f, ProportionalRight: %f, ProportionalLeft: %f \n\r", controller.getIntegralLeft(), controller.getIntegralRight(), (counterLeft.read()+counterRight.read()), controller.getSpeedRight(), controller.getSpeedLeft(), controller.getProportionalRight(), controller.getProportionalLeft());
+           
+            
+            //printf("DistanzRechts = %.0f mm\n", distanceRight);
+            //printf("DistanzVorne = %.0f mm\n", distanceFront);
+           // printf("DistanzLinksVorne = %.0f mm\n", distanceLeftFront);
+           // printf("DistanzLinksHinten = %.0f mm\n", distanceLeftBack);
+           // printf("WandRechts = %d \n", wallRight);
+           // printf("WandVorne = %d \n", wallFront);
+           // printf("WandLinks = %d \n", wallLeft);
+            
+            //printf("CountsRight = %d\n", counterRight.read());
+            //printf("CountsLeft = %d\n", counterLeft.read());  
+            //kontrastSensor.check();
+            //printf("SchwarzeLinie  =  %d", blackLine);
+            myled =! myled; // LED is ON Heartbeat
+            wait(0.5f); 
+
+        }
     }
 }