basic_functions.h

#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;