Hc sr-04 и vl53l0x совместить скетч

Вечер добрый.
Схема состоит из
Ардуино Нано
Дальномер HC SR-04
лазерный Дальномер VL53L0X
Вибромоторчик
Пищалка(buzzer)
Кнопка переключения режимов.
Имеется два скетча, нужно их совместить.
ТЗ такое вибромоторчик должен включаться при изменении расстояния.
Причем чем меньше расстояние тем сильнее должен крутиться вибромотор.
Режим 1
Шаги изменения вибрации такие( это для HC SR04)
200
160
120
Режим
Шаги изменения вибрации для VL53L0X
120
80
60
40
20
Удержание кнопки должно переключаться между датчиками измерения расстояния.
Двойное нажатие кнопки переключает сигнал вибромоторчик или пищалка.

Спойлер: скетч

Код (C++):

#include <Wire.h>
#include <VL53L0X.h>
int out1;
/* This example shows how to get single-shot range
measurements from the VL53L0X. The sensor can optionally be
configured with different ranging profiles, as described in
the VL53L0X API user manual, to get better performance for
a certain application. This code is based on the four
"SingleRanging" examples in the VL53L0X API.
The range readings are in units of mm. */
VL53L0X sensor;
// Uncomment this line to use long range mode. This
// increases the sensitivity of the sensor and extends its
// potential range, but increases the likelihood of getting
// an inaccurate reading because of reflections from objects
// other than the intended target. It works best in dark
// conditions.
#define LONG_RANGE //увеличенная дальность(можно убрать // )
// Uncomment ONE of these two lines to get
// - higher speed at the cost of lower accuracy OR
// - higher accuracy at the cost of lower speed
//#define HIGH_SPEED //высокая скорость (можно убрать // )
//#define HIGH_ACCURACY//высокая точность(можно убрать // )
int _gtv3 = 200; //начальное расстояние (1-8000)
int _gtv2 = 10; //минимальное расстояние (1-8000) должно быть меньше начального
int _gtv1 = 0;
int _swi3;
bool _bounseInputD2S = 0;
bool _bounseInputD2O = 0;
unsigned long _bounseInputD2P = 0UL;
int _swi1;
void setup()
{
pinMode(2, INPUT_PULLUP);
_bounseInputD2O =  digitalRead(2);
//  Serial.begin(9600);
//Wire.begin();
sensor.setTimeout(50);//период опроса датчика(50)
#if defined LONG_RANGE
// lower the return signal rate limit (default is 0.25 MCPS)
  sensor.setSignalRateLimit(0.1);
// increase laser pulse periods (defaults are 14 and 10 PCLKs)
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
#endif
#if defined HIGH_SPEED
  // reduce timing budget to 20 ms (default is about 33 ms)
  sensor.setMeasurementTimingBudget(20000);
//#elif defined HIGH_ACCURACY
  // increase timing budget to 200 ms
  sensor.setMeasurementTimingBudget(200000);
#endif
}
void loop()
{
bool  _bounceInputTmpD2 =  (digitalRead (2));
if (_bounseInputD2S)
    {
     if (millis() >= (_bounseInputD2P + 40))
         {_bounseInputD2O= _bounceInputTmpD2; _bounseInputD2S=0;}
     }
else
    {
     if (_bounceInputTmpD2 != _bounseInputD2O )
         {_bounseInputD2S=1; _bounseInputD2P = millis();}
      }
//  Serial.print(sensor.readRangeSingleMillimeters());
//  if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
out1=(sensor.readRangeSingleMillimeters());
//  Serial.println();
_gtv1 = out1;
if((_gtv1) > (_gtv3))
{_swi3=0;}
else
{_swi3=(map((_gtv1), ((_gtv2)), ((_gtv3)), (255), (0)));}
if(( ((_gtv1) < (_gtv2)) || (!(_bounseInputD2O)) ))
{_swi1=255;}
else
{_swi1=_swi3;}
analogWrite(5, _swi1);
}

и вот второй скетч

Спойлер: второй скетч

Код (C++):

#define trigPin 10
#define echoPin 11
#define motor 12
void setup() {
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(motor, OUTPUT);
}
void loop() {
long duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
if (distance <= 120 && distance > 20) {
analogWrite(motor, 250);
delay(50);
analogWrite(motor, 0);
delay(50);
}

else {
analogWrite(motor, 0);
}
delay(50);
}

 

Бесконтактная трость?

 

да