Diff: main.cpp

Revision:

0:7ee4c6416e08

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 24 16:05:50 2020 +0000
@@ -0,0 +1,206 @@
+#include "mbed.h"
+#include "IRSensor.h"
+#include "Controller.h"
+#include "LowpassFilter.h"
+
+DigitalOut myled(LED1);
+
+int main()
+{
+    DigitalOut led0(PD_4);
+    DigitalOut led1(PD_3);
+    DigitalOut led2(PD_6);
+    DigitalOut led3(PD_2);
+    DigitalOut led4(PD_7);
+    DigitalOut led5(PD_5);
+
+    AnalogIn distance(PA_0); // Kreieren der Ein- und Ausgangsobjekte
+    DigitalOut enable(PG_1);
+    DigitalOut bit0(PF_0);
+    DigitalOut bit1(PF_1);
+    DigitalOut bit2(PF_2);
+
+
+
+
+    IRSensor irSensor0(distance, bit0, bit1, bit2, 0); // Objekte kreieren
+    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);
+
+    enable = 1; // schaltet die Sensoren ein
+
+    DigitalOut enableMotorDriver(PG_0);
+    DigitalIn motorDriverFault(PD_1);
+    DigitalIn motorDriverWarning(PD_0);
+    PwmOut pwmLeft(PF_9);
+    PwmOut pwmRight(PF_8);
+    
+    EncoderCounter counterLeft(PD_12, PD_13);
+    EncoderCounter counterRight(PB_4, PC_7);
+    
+    Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
+    
+    while(1) {
+        /*
+        led0 = 1; // LED is ON
+        led1 = 0;
+        led2 = 0;
+        led3 = 0;
+        led4 = 0;
+        led5 = 0;
+
+        wait(0.2); // 200 ms
+
+        led0 = 0; // LED is ON
+        led1 = 1;
+        led2 = 0;
+        led3 = 0;
+        led4 = 0;
+        led5 = 0;
+
+        wait(0.2); // 200 ms
+
+        led0 = 0; // LED is ON
+        led1 = 0;
+        led2 = 1;
+        led3 = 0;
+        led4 = 0;
+        led5 = 0;
+
+        wait(0.2); // 200 ms
+
+        led0 = 0; // LED is ON
+        led1 = 0;
+        led2 = 0;
+        led3 = 1;
+        led4 = 0;
+        led5 = 0;
+
+        wait(0.2); // 200 ms
+
+        led0 = 0; // LED is ON
+        led1 = 0;
+        led2 = 0;
+        led3 = 0;
+        led4 = 1;
+        led5 = 0;
+
+        wait(0.2); // 200 ms
+
+        led0 = 0; // LED is ON
+        led1 = 0;
+        led2 = 0;
+        led3 = 0;
+        led4 = 0;
+        led5 = 1;
+
+        wait(0.2); // 200 ms
+
+
+        AnalogIn distance(PA_0); // Kreieren der Ein- und Ausgangsobjekte
+        DigitalOut enable(PG_1);
+        DigitalOut bit0(PF_0);
+        DigitalOut bit1(PF_1);
+        DigitalOut bit2(PF_2);
+        enable = 1; // schaltet die Sensoren ein
+        bit0 = 0; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
+        bit1 = 0;
+        bit2 = 0;
+        float d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
+
+        printf("Distance hinten: %f \r \n",d);
+
+        bit0 = 1; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
+        bit1 = 0;
+        bit2 = 0;
+        d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
+
+        printf("Distance hinten-links: %f \r \n",d);
+
+        bit0 = 0; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
+        bit1 = 1;
+        bit2 = 0;
+        d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
+
+        printf("Distance vorne-links: %f \r \n",d);
+
+        bit0 = 0; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
+        bit1 = 0;
+        bit2 = 1;
+        d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
+
+        printf("Distance vorne-rechts: %f \r \n",d);
+
+        bit0 = 1; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
+        bit1 = 0;
+        bit2 = 1;
+        d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
+
+        printf("Distance hinten-rechts: %f \r \n",d);
+
+        bit0 = 1; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
+        bit1 = 1;
+        bit2 = 0;
+        d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
+
+        printf("Distance vorne: %f \r \n",d);
+        */
+      
+        
+        float distance0 = irSensor0.read(); // gibt die Distanz in [m] zurueck
+        float distance1 = irSensor1.read();
+        float distance2 = irSensor2.read();
+        float distance3 = irSensor3.read();
+        float distance4 = irSensor4.read();
+        float distance5 = irSensor5.read();
+        
+        printf("Distance hinten: %f \r \n", distance0);
+        printf("Distance hinten-links: %f \r \n", distance1);
+        printf("Distance vorne-links: %f \r \n", distance2);
+        printf("Distance vorne: %f \r \n", distance3);
+        printf("Distance vorne-rechts: %f \r \n", distance4);
+        printf("Distance hinten-rechts: %f \r \n", distance5);
+        
+        if (distance0 <= 0.1) { led0=1;
+       
+            }else {led0=0;}
+            
+        if (distance1 <= 0.1) { led1=1;
+       
+            }else {led1=0;}
+        if (distance2 <= 0.1) { led2=1;
+       
+            }else {led2=0;}
+        if (distance3 <= 0.1) { led3=1;
+       
+            }else {led3=0;}
+        if (distance4 <= 0.1) { led4=1;
+       
+            }else {led4=0;}
+        if (distance5 <= 0.1) { led5=1;
+       
+            }else {led5=0;}
+       /*     
+        pwmLeft.period(0.00005); // Setzt die Periode auf 50 μs
+        pwmRight.period(0.00005);
+        
+        pwmLeft = 0.5; // Setzt die Duty-Cycle auf 50%
+        pwmRight = 0.5;
+        */
+        enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
+
+        //pwmLeft = 0.7; // Duty-Cycle von 70%
+        //pwmRight = 0.3;
+        
+        
+        controller.setDesiredSpeedLeft(100.0); // Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(-100.0);
+        
+        wait(0.2);
+
+        }
+
+}
\ No newline at end of file