Replace bits-bare-minimum-zyklus-1.ino

bits-bare-minimum-zyklus-1.ino

-/***************************************************
- ________  ___  _________  ________    _______     
-|\   __  \|\  \|\___   ___\\   ____\  /  ___  \    
-\ \  \|\ /\ \  \|___ \  \_\ \  \___|_/__/|_/  /|   
- \ \   __  \ \  \   \ \  \ \ \_____  \__|//  / /   
-  \ \  \|\  \ \  \   \ \  \ \|____|\  \  /  /_/__  
-   \ \_______\ \__\   \ \__\  ____\_\  \|\________\
-    \|_______|\|__|    \|__| |\_________\\|_______|
-                             \|_________|          
-                                                        
-  Name:
-  <Hier kommt der Name des Projekts>
-
-  Description:
-  <Hier kommt was dieses Skript macht>
-  
-
-  Author:
-  <Hier Namen einfügen>    
-
-**************************************************/
-#define DEBUG_SERIAL
-// #define WIFI_COMMUNICATION
-
-#ifdef DEBUG_SERIAL
-#define _println_(x) Serial.println(x)
-#define _print_(x) Serial.print(x)
-#else
-#ifdef WIFI_COMMUNICATION
-#define _println_(x) Serial3.println(x)
-#define _print_(x) Serial3.print(x)
-#else
-#define _println_(x)
-#define _print_(x)
-#endif
-#endif
-
-#include "config.h"
-#include "basic_functions.h"
-#include "custom_config.h"
-#include "custom_functions.h"
-
-void setup(){
-    // Hier wird entschieden wie die Serielle Kommunikation vom Mega erfolgen soll. Weiteres siehe in config.h
-    #ifdef DEBUG_SERIAL
-    Serial.begin(115200);
-    #endif
-    #ifdef WIFI_COMMUNICATION
-    Serial3.begin(115200);
-    #endif
-
-    // Initialisierung der optischen Time-of-Flight Sensoren
-    tof_controller.init();
-
-    // Starten der I2C Kommunikation
-    Wire.begin();
-    Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
-
-    // Initialisierung der LED-Streifen
-    front_strip.init();
-    back_strip.init();
-    left_strip.init();
-    right_strip.init();
-
-    // Initialisierung der IMU inkl. Lichtspiel
-    imu.init();
-    start = millis();
-    _println_("Waiting for IMU getting ready. Please do not move the car!");
-    _print_("Waiting");
-    set_led_color(255,0,0);
-    for (int k=0; k<13; k++){
-        for (int i=0; i<FRONT_LED_NUM; i++){
-            front_strip.set_pixel_color(i,thk_LedStripController::Color(red, green, blue));
-            front_strip.show();
-            delay(500/FRONT_LED_NUM);
-            _print_(".");
-        }
-        for (int i=0; i<FRONT_LED_NUM; i++){
-            front_strip.set_pixel_color(i,thk_LedStripController::Color(0, 0, 0));
-            front_strip.show();
-            delay(500/FRONT_LED_NUM);
-            _print_(".");
-        }
-        red -= 20;  
-        green += 20;   
-    }
-
-    // Pin deklaration der Ultraschallsensoren
-    pinMode(TRIGF, OUTPUT);
-    pinMode(TRIGB, OUTPUT);
-    pinMode(ECHOF, INPUT);
-    pinMode(ECHOB, INPUT);
-
-    // Initialisierung des Motors
-    Motor.init();    
-
-    // Initialisierung der Servomotoren
-    ServoSteer.begin();
-    ServoSearch.begin();
-
-    // Initialisierung abgeschlossen
-    _println_("Ready!");
-    buzzer_signal(350, 2, 100);
-    
-    set_led_color(0,100,0);
-    all_led_blink(2,200);
-}
-
-void loop(){
-
-}
+/***************************************************
+ ________  ___  _________  ________    _______     
+|\   __  \|\  \|\___   ___\\   ____\  /  ___  \    
+\ \  \|\ /\ \  \|___ \  \_\ \  \___|_/__/|_/  /|   
+ \ \   __  \ \  \   \ \  \ \ \_____  \__|//  / /   
+  \ \  \|\  \ \  \   \ \  \ \|____|\  \  /  /_/__  
+   \ \_______\ \__\   \ \__\  ____\_\  \|\________\
+    \|_______|\|__|    \|__| |\_________\\|_______|
+                             \|_________|          
+                                                        
+  Name:
+  <Hier kommt der Name des Projekts>
+
+  Description:
+  <Hier kommt was dieses Skript macht>
+  
+
+  Author:
+  <Hier Namen einfügen>    
+
+**************************************************/
+#define DEBUG_SERIAL
+// #define WIFI_COMMUNICATION
+
+#ifdef DEBUG_SERIAL
+#define _println_(x) Serial.println(x)
+#define _print_(x) Serial.print(x)
+#else
+#ifdef WIFI_COMMUNICATION
+#define _println_(x) Serial3.println(x)
+#define _print_(x) Serial3.print(x)
+#else
+#define _println_(x)
+#define _print_(x)
+#endif
+#endif
+
+#include "config.h"
+#include "basic_functions.h"
+#include "custom_config.h"
+#include "custom_functions.h"
+
+void setup(){
+    // Hier wird entschieden wie die Serielle Kommunikation vom Mega erfolgen soll. Weiteres siehe in config.h
+    #ifdef DEBUG_SERIAL
+    Serial.begin(115200);
+    #endif
+    #ifdef WIFI_COMMUNICATION
+    Serial3.begin(115200);
+    #endif
+
+    // Initialisierung der optischen Time-of-Flight Sensoren
+    tof_controller.init();
+
+    // Starten der I2C Kommunikation
+    Wire.begin();
+    Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
+
+    // Initialisierung der LED-Streifen
+    front_strip.init();
+    back_strip.init();
+    left_strip.init();
+    right_strip.init();
+
+    // Initialisierung der IMU inkl. Lichtspiel
+    imu.init();
+    start = millis();
+    _println_("Waiting for IMU getting ready. Please do not move the car!");
+    _print_("Waiting");
+    set_led_color(255,0,0);
+    for (int k=0; k<13; k++){
+        for (int i=0; i<FRONT_LED_NUM; i++){
+            front_strip.set_pixel_color(i,thk_LedStripController::Color(red, green, blue));
+            front_strip.show();
+            delay(500/FRONT_LED_NUM);
+            _print_(".");
+        }
+        for (int i=0; i<FRONT_LED_NUM; i++){
+            front_strip.set_pixel_color(i,thk_LedStripController::Color(0, 0, 0));
+            front_strip.show();
+            delay(500/FRONT_LED_NUM);
+            _print_(".");
+        }
+        red -= 20;  
+        green += 20;   
+    }
+
+    // Pin deklaration der Ultraschallsensoren
+    pinMode(TRIGF, OUTPUT);
+    pinMode(TRIGB, OUTPUT);
+    pinMode(ECHOF, INPUT);
+    pinMode(ECHOB, INPUT);
+
+    // Initialisierung des Motors
+    Motor.init();    
+
+    // Initialisierung der Servomotoren
+    ServoSteer.begin();
+    ServoSearch.begin();
+
+    // Initialisierung abgeschlossen
+    _println_("Ready!");
+    buzzer_signal(350, 2, 100);
+    
+    set_led_color(0,100,0);
+    all_led_blink(2,200);
+}
+
+void loop(){
+   //SENSORDATEN AUSLESEN
+
+    int TOF0 = tof_controller.get_distance_cm(0);
+    int TOF1 = tof_controller.get_distance_cm(1);
+    int TOF2 = tof_controller.get_distance_cm(2);
+    int TOF3 = tof_controller.get_distance_cm(3);
+    int TOF4 = tof_controller.get_distance_cm(4);
+
+    int US1=us_get_distance(TRIGF, ECHOF);
+    int US2=us_get_distance(TRIGB, ECHOB);
+  
+   
+   //TOF Sensor
+    Serial.print(TOF0);
+    Serial.print(" cm");
+    Serial.print("\t");
+    Serial.print(TOF1);
+    Serial.print(" cm");
+    Serial.print("\t");
+    Serial.print(TOF2);
+    Serial.print(" cm");
+    Serial.print("\t");
+    Serial.print(TOF3);
+    Serial.print(" cm");
+    Serial.print("\t");
+    Serial.print(TOF4);
+    Serial.println(" cm");
+
+  //Ultraschallsensor   
+      Serial.print (US1);
+      Serial.print(" cm");
+      Serial.print("\t");
+      Serial.print (US2);
+      Serial.print (" cm");
+      Serial.print ("\t");
+      Serial.print ("\t");
+      Serial.print ("\t");
+
+
+}