Diff: main.cpp

Revision:

0:6d0671ae4648

Child:

1:a95ba1510438

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,150 @@
+
+#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 distance(PB_1);
+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;                                                              //von main Vincent kopiert
+
+IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F);               //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.rechts270();
+        wait(1.0f);*/
+        
+                                                                                // IR Sensoren einlesen Programm Vincent
+        
+                    
+        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
+        
+        
+        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)){
+        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen
+        }   else  if ((IrR==0) && (IrL==1) && (IrF==0)){
+        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen
+        }   else  if ((IrR==0) && (IrL==1) && (IrF==1)){
+        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen                 
+        }   else  if ((IrR==1) && (IrL==0) && (IrF==0)){
+        caseDrive = 1;                                                          // Folge: geradeaus fahren
+        }   else  if ((IrR==1) && (IrL==0) && (IrF==1)){
+        caseDrive = 3;                                                          // Folge: 270 Grad nach rechts drehen
+        }   else  if ((IrR==1) && (IrL==1) && (IrF==0)){
+        caseDrive = 1;                                                          // Folge: geradeaus fahren
+        }   else  if ((IrR==1) && (IrL==1) && (IrF==1)){
+        caseDrive = 4;                                                          // Folge: 180 Grad nach rechts drehen
+        }   else{ caseDrive=0;
+        }
+        
+        printf("caseDrive = %d\n",caseDrive);
+
+        wait (1.0);
+        
+        /*switch (caseDrive){
+            caseDrive '2': 
+                buttonNow = button->read();
+                if (buttonNow && !buttonBefore){*/
+        
+        
+        if(caseDrive == 1){
+            fahren.geradeaus();
+        } else if (caseDrive == 2){
+            fahren.rechts90();
+        } else if (caseDrive == 3){
+            fahren.rechts270();
+        } else if (caseDrive == 4){
+            fahren.rechts180();
+        }
+                
+            
+    }
+}
+
+
+