From 73ab1a94af5ff7e20ac18a6e351adbb9de7f45a8 Mon Sep 17 00:00:00 2001
From: Julia Magdalena Brandt <julia_magdalena.brandt@smail.th-koeln.de>
Date: Fri, 6 Oct 2023 15:56:20 +0200
Subject: [PATCH] Upload New File
---
basic_functions.h | 116 ++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 116 insertions(+)
create mode 100644 basic_functions.h
diff --git a/basic_functions.h b/basic_functions.h
new file mode 100644
index 0000000..b8eff12
--- /dev/null
+++ b/basic_functions.h
@@ -0,0 +1,116 @@
+#ifndef BASICFUNCTIONS_H
+#define BASICFUNCTIONS_H
+
+void buzzer_signal(int pitch, int count, int delay_time){
+ for (int i = 0; i < count; i++){
+ tone(BUZZER_PIN, pitch);
+ delay(delay_time);
+ noTone(BUZZER_PIN);
+ delay(delay_time);
+ }
+}
+
+void all_led_blink(int count, int delay_time){
+ for (int k=0; k<count; k++){
+ front_strip.set_pixel_color(thk_LedStripController::Color(red, green, blue));
+ front_strip.show();
+ back_strip.set_pixel_color(thk_LedStripController::Color(red, green, blue));
+ back_strip.show();
+ left_strip.set_pixel_color(thk_LedStripController::Color(red, green, blue));
+ left_strip.show();
+ right_strip.set_pixel_color(thk_LedStripController::Color(red, green, blue));
+ right_strip.show();
+ delay(delay_time);
+ front_strip.set_pixel_color(thk_LedStripController::Color(0, 0, 0));
+ front_strip.show();
+ back_strip.set_pixel_color(thk_LedStripController::Color(0, 0, 0));
+ back_strip.show();
+ left_strip.set_pixel_color(thk_LedStripController::Color(0, 0, 0));
+ left_strip.show();
+ right_strip.set_pixel_color(thk_LedStripController::Color(0, 0, 0));
+ right_strip.show();
+ delay(delay_time);
+ }
+}
+
+int us_get_distance(int TRIG, int ECHO)
+{
+ digitalWrite(TRIG, LOW);
+ delay(5);
+ digitalWrite(TRIG, HIGH);
+ delay(10);
+ digitalWrite(TRIG, LOW);
+ dauer = pulseIn(ECHO, HIGH);
+ entfernung = (dauer / 2) / 29.1;
+
+ return entfernung;
+}
+
+int get_front_tof_distance(){
+ int distance;
+ #ifdef TOF_VL53L0X
+ distance = tof_controller.get_distance_cm(0);
+ #elif defined(TOF_TFLUNA)
+ if( tflI2C.getData( tfDist, tfAddr)) // If read okay...
+ {
+ distance = tfDist;
+ }
+ else tflI2C.printStatus();
+ #endif
+ return distance;
+}
+
+void make_room(){
+ ServoSteer.turn(STEER_START_POS);
+ ServoSearch.turn(SEARCH_START_POS);
+
+ US_distance_b = us_get_distance(TRIGB, ECHOB);
+ US_distance_f = us_get_distance(TRIGF, ECHOF);
+ tof_distance_front = get_front_tof_distance();
+ weighted_distance = (US_distance_f+tof_distance_front)/2;
+
+ while ((weighted_distance <= MIN_DIST_F) && (US_distance_b > MIN_DIST_B)){
+ US_distance_b = us_get_distance(TRIGB, ECHOB);
+ US_distance_f = us_get_distance(TRIGF, ECHOF);
+ tof_distance_front = get_front_tof_distance();
+ weighted_distance = (US_distance_f+tof_distance_front)/2;
+ Motor.drive_backward(velocity_backward);
+ }
+
+ Motor.stop();
+}
+
+void drive_forward_controlled_velocity(){
+ US_distance_f = us_get_distance(TRIGF, ECHOF);
+ tof_distance_front = get_front_tof_distance();
+ weighted_distance = (US_distance_f+tof_distance_front)/2;
+ if ((millis()-start)>500){
+ if (int(weighted_distance) > MIN_DIST_F){
+ velocity = map(weighted_distance, 10, 100, 80, 255);
+ Motor.drive_forward(velocity);
+ }
+ else{
+ Motor.stop();
+ }
+ start = millis();
+ }
+}
+
+void set_led_color(byte r, byte g, byte b){
+ red = r;
+ green = g;
+ blue = b;
+}
+
+void end_of_programm(){
+ Motor.stop();
+ ServoSteer.end();
+ ServoSearch.end();
+ buzzer_signal(400,1,100);
+ buzzer_signal(700,1,100);
+ while (1){
+ // End of Programm
+ }
+}
+
+#endif
\ No newline at end of file
--
GitLab