Skip to content
Snippets Groups Projects
Select Git revision
  • master
  • main protected
2 results

watchdog.cpp

Blame
  • basic_functions.h 3.83 KiB
    #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
      }
    }
    
    void initial_led_show() {
      _println_("Waiting");
      set_led_color(255,0,0);
      const int frequencySteps[] = {500, 700, 900, 1100, 900, 700};
      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;
      }
    }
    
    #endif